From 3097e1cb0e4cfe97e5c78f04e9885cb88373925f Mon Sep 17 00:00:00 2001 From: Maxwell Henderson Date: Fri, 13 Feb 2026 09:40:17 -0800 Subject: [PATCH 1/6] Add new main pio with matrix wrapper Signed-off-by: Maxwell Henderson --- .gitignore | 2 + include/README | 37 +++++++++++++ include/matrix.h | 137 +++++++++++++++++++++++++++++++++++++++++++++++ platformio.ini | 4 ++ requirements.txt | 1 + src/main.cc | 18 +++++++ 6 files changed, 199 insertions(+) create mode 100644 include/README create mode 100644 include/matrix.h create mode 100644 platformio.ini create mode 100644 requirements.txt create mode 100644 src/main.cc diff --git a/.gitignore b/.gitignore index 89cc49c..59fc8c9 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,5 @@ .vscode/c_cpp_properties.json .vscode/launch.json .vscode/ipch + +.envrc diff --git a/include/README b/include/README new file mode 100644 index 0000000..49819c0 --- /dev/null +++ b/include/README @@ -0,0 +1,37 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the convention is to give header files names that end with `.h'. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/include/matrix.h b/include/matrix.h new file mode 100644 index 0000000..88092cf --- /dev/null +++ b/include/matrix.h @@ -0,0 +1,137 @@ +#ifndef MATRIX_H_ +#define MATRIX_H_ + +#include "arm_math.h" +#include + +// Wrapper around a f32 cmsis dsp matrix object +// TODO(max): Chain optimization is required + +template class Matrix; +template class Matrix { +public: + static constexpr size_t kDataSize = Rows * Cols; + + template friend class Matrix; + + Matrix(float32_t data[kDataSize]) + : matrix_instance_{Rows, Cols, matrix_data_} { + std::copy(data, data + kDataSize, matrix_data_); + } + + explicit Matrix(const std::array values) + : matrix_instance_{Rows, Cols, matrix_data_} { + std::copy(values.begin(), values.end(), matrix_data_); + } + + Matrix(const Matrix &other) : matrix_instance_{Rows, Cols, matrix_data_} { + std::copy(other.matrix_data_, other.matrix_data_ + kDataSize, matrix_data_); + } + + Matrix &operator=(const Matrix &other) { + std::copy(other.matrix_data_, other.matrix_data_ + kDataSize, matrix_data_); + + matrix_instance_ = {Rows, Cols, matrix_data_}; + return *this; + } + + static constexpr Matrix Zero() { + Matrix matrix{}; + std::fill(matrix.matrix_data_, matrix.matrix_data_ + kDataSize, 0.0f); + matrix.matrix_instance_ = {Rows, Cols, matrix.matrix_data_}; + return matrix; + } + + static constexpr Matrix Identity() { + static_assert(Rows == Cols, "Identity matrix must be square"); + + Matrix matrix = Zero(); + + for (int i = 0; i < Rows; i++) { + matrix.set(i, i, 1.0f); + } + + return matrix; + } + + Matrix operator+(const Matrix &rhs) const { + Matrix result; + + arm_mat_add_f32(&matrix_instance_, &rhs.matrix_instance_, + &result.matrix_instance_); + + return result; + } + + Matrix operator-(const Matrix &rhs) const { + Matrix result; + + arm_mat_sub_f32(&matrix_instance_, &rhs.matrix_instance_, + &result.matrix_instance_); + + return result; + } + + template + Matrix operator*(const Matrix &rhs) const { + Matrix result; + + arm_mat_mult_f32(&matrix_instance_, &rhs.matrix_instance_, + &result.matrix_instance_); + + return result; + } + + Matrix operator*(const float32_t scalar) const { + Matrix out; + + arm_mat_scale_f32(&matrix_instance_, scalar, &out.matrix_instance_); + + return out; + } + + Matrix transpose() const { + Matrix result; + + arm_mat_trans_f32(&matrix_instance_, &result.matrix_instance_); + + return result; + } + + Matrix inverse() const { + Matrix result; + + arm_mat_inverse_f32(&matrix_instance_, &result.matrix_instance_); + + return result; + } + + constexpr inline void set(int i, int j, float32_t data) { + matrix_data_[i * Cols + j] = data; + } + + constexpr inline const float32_t at(int i, int j) const { + return matrix_data_[i * Cols + j]; + } + + void print() const { + for (int i = 0; i < Rows; i++) { + for (int j = 0; j < Cols; j++) { + Serial.print(at(i, j)); + if (j != (Cols - 1)) { + Serial.print(" ,"); + } + } + Serial.println(); + } + } + +protected: + Matrix() : matrix_instance_{Rows, Cols, matrix_data_} {} + arm_matrix_instance_f32 matrix_instance_; + + // We want to store the data here so it doesnt go out scope + float32_t matrix_data_[kDataSize]; +}; + +#endif // MATRIX_H_ diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..5215e18 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,4 @@ +[env:mainboard] +platform = teensy +board = teensy41 +framework = arduino diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..7c71512 --- /dev/null +++ b/requirements.txt @@ -0,0 +1 @@ +platformio diff --git a/src/main.cc b/src/main.cc new file mode 100644 index 0000000..9d8ba2c --- /dev/null +++ b/src/main.cc @@ -0,0 +1,18 @@ +#include + +#include "arm_math.h" +#include "matrix.h" + +#include + + +void setup() { + Serial.begin(115200); + + Matrix<1,2> mat({1, 2}); + + while (!Serial) { + } +} + +void loop() {} From adb9e7e14b52a83272164ba7ac84752c51e6a126 Mon Sep 17 00:00:00 2001 From: Maxwell Henderson Date: Fri, 13 Feb 2026 11:57:04 -0800 Subject: [PATCH 2/6] Add LQR code Signed-off-by: Maxwell Henderson --- include/lqr.h | 72 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/lqr.cc | 1 + src/main.cc | 1 + 3 files changed, 74 insertions(+) create mode 100644 include/lqr.h create mode 100644 src/lqr.cc diff --git a/include/lqr.h b/include/lqr.h new file mode 100644 index 0000000..653df42 --- /dev/null +++ b/include/lqr.h @@ -0,0 +1,72 @@ +#ifndef LQR_H_ +#define LQR_H_ + +#include "matrix.h" +#include "cstddef" + +namespace controls { + +template +class LQR { + public: + static constexpr int kNumTaylorExpansions = 5; + + LQR(Matrix Q, Matrix R): + Q_(Q), + R_(R) {} + + // Solves the DARE problem for an infinite horizon and outputs our K matrix + // TODO(max): Is this the best way to do this? We are gonna run into numerical stability because of f32. + Matrix SolveDare(Matrix A, Matrix B, float32_t dt, int steps=500) { + Matrix A_d; + Matrix B_d; + + DiscretizeAB(&A_d, &B_d, A, B, dt); + + Matrix Q_d = Q_ * dt; + Matrix R_d = R_ * (1.0 / dt); + + Matrix X_k = Q_d; + + auto G = B_d * R_d.inverse() * B_d.transpose(); + + for (int i = 0; i < steps; i++) { + X_k = A_d.transpose() * X_k * (Matrix::Identity() + G * X_k).inverse() * A_d + Q_d; + } + + auto K = (R_d + B_d.transpose() * X_k * B_d).inverse() * B_d.transpose() * X_k * A_d; + + return K; + } + private: + // We are discretizing by doing two seperate taylor expansions for A_d and B_d + void DiscretizeAB(Matrix &A_d, Matrix &B_d, Matrix A, Matrix B, float32_t dt) { + // A_d = e^A(dt) + auto term1 = A * dt; + A_d = Matrix::Identity() + term1; + + // Taylor expand a set number of times + for (size_t i = 2; i <= kNumTaylorExpansions; i++) { + term1 = (term1 * A * dt) / i; + A_d += term1; + } + + // B_d = (int(0, dt, e^At dt) * B) + + auto term2 = (A * dt * dt) / 2.0f; + auto inner_accum = term2; + for (size_t i = 2; i <= kNumTaylorExpansions; i++) { + term2 = (term2 * A * dt) / (i+1); + inner_accum += term2; + } + + B_d = (Matrix::Identity() * dt + inner_accum) * B; + } + + Matrix Q_; + Matrix R_; +}; + +}; + +#endif // LQR_H_ diff --git a/src/lqr.cc b/src/lqr.cc new file mode 100644 index 0000000..23083c7 --- /dev/null +++ b/src/lqr.cc @@ -0,0 +1 @@ +#include "lqr.h" diff --git a/src/main.cc b/src/main.cc index 9d8ba2c..17b79ea 100644 --- a/src/main.cc +++ b/src/main.cc @@ -2,6 +2,7 @@ #include "arm_math.h" #include "matrix.h" +#include "lqr.h" #include From 25cf2204298805f30878d56c69fc595e622d91ec Mon Sep 17 00:00:00 2001 From: Maxwell Henderson Date: Fri, 13 Feb 2026 18:10:48 -0800 Subject: [PATCH 3/6] Define SO(3) Operators Signed-off-by: Maxwell Henderson --- include/fast_math_utils.h | 9 +++++ include/lie.h | 48 ++++++++++++++++++++++ include/matrix.h | 23 ++++++++++- src/lie.cc | 84 +++++++++++++++++++++++++++++++++++++++ src/main.cc | 3 +- 5 files changed, 164 insertions(+), 3 deletions(-) create mode 100644 include/fast_math_utils.h create mode 100644 include/lie.h create mode 100644 src/lie.cc diff --git a/include/fast_math_utils.h b/include/fast_math_utils.h new file mode 100644 index 0000000..4bfd8f6 --- /dev/null +++ b/include/fast_math_utils.h @@ -0,0 +1,9 @@ +#ifndef FAST_MATH_UTILS_H_ +#define FAST_MATH_UTILS_H_ + +#include "arm_math.h" + +#define fcos(x) arm_cos_f32(x) +#define fsin(x) arm_sin_f32(x) + +#endif // FAST_MATH_UTILS_H_ diff --git a/include/lie.h b/include/lie.h new file mode 100644 index 0000000..abda2da --- /dev/null +++ b/include/lie.h @@ -0,0 +1,48 @@ +#ifndef LIE_H_ +#define LIE_H_ + +#include "matrix.h" +#include "math.h" + +// Defines operators and classes for SO(3) +namespace lie { + +// Scalar last quaternion +class Quaternion { + public: + Quaternion(float32_t quat[4]); + + explicit Quaternion(const std::array values); + + Quaternion(const Matrix<3, 1> vector, float32_t scalar); + + Quaternion(const Quaternion &other); + + // Returns the scalar part of the quaternion + float32_t scalar() const; + + // Returns the vector part of the quaternion + const Matrix<3, 1> vector() const; + + protected: + float32_t quat_[4]; +}; + +namespace operators { +// R^3 -> SO(3) +Quaternion exp(Matrix<3, 1> phi); + +// SO(3) -> R^3 +Matrix<3, 1> log(Quaternion phi); + +// Defines the Gamma operator which is the jacobian of exponential map. +// Also defined as the right jacobian in a lot of literature +Matrix<3, 3> gamma(Matrix<3, 1> phi); + +// This is just the skew operator, doesn't need to be defined in lie::operators but right now thats the only place its used. +Matrix<3, 3> skew(Matrix<3, 1> vector); +} // operators + +} // lie + +#endif // LIE_H_ diff --git a/include/matrix.h b/include/matrix.h index 88092cf..2e04a21 100644 --- a/include/matrix.h +++ b/include/matrix.h @@ -14,7 +14,7 @@ template class Matrix { template friend class Matrix; - Matrix(float32_t data[kDataSize]) + Matrix(const float32_t data[kDataSize]) : matrix_instance_{Rows, Cols, matrix_data_} { std::copy(data, data + kDataSize, matrix_data_); } @@ -90,6 +90,18 @@ template class Matrix { return out; } + friend Matrix operator*(const float32_t rhs, const Matrix &lhs) { + return lhs * rhs; + } + + Matrix operator/(const float32_t scalar) const { + Matrix out; + + arm_mat_scale_f32(&matrix_instance_, 1.0f / scalar, &out.matrix_instance_); + + return out; + } + Matrix transpose() const { Matrix result; @@ -106,6 +118,15 @@ template class Matrix { return result; } + float32_t norm() const { + static_assert(Cols == 1, "Euclidian norm only defined for vectors"); + + float32_t result = 0; + // We const cast because the version of cmsis we're using doesn't have a const here. + arm_rms_f32(const_cast(matrix_data_), Rows, &result); + return result; + } + constexpr inline void set(int i, int j, float32_t data) { matrix_data_[i * Cols + j] = data; } diff --git a/src/lie.cc b/src/lie.cc new file mode 100644 index 0000000..3f6f29d --- /dev/null +++ b/src/lie.cc @@ -0,0 +1,84 @@ +#include "lie.h" + +using lie::Quaternion; + +constexpr float32_t epsilon = 1e-4; + +Quaternion lie::operators::exp(Matrix<3, 1> phi) { + float32_t norm = phi.norm(); + + if (norm < epsilon) { + return Quaternion({phi.at(0, 0) / 2.0f, phi.at(1, 0) / 2.0f, phi.at(2, 0) / 2.0f, 1.0f}); + } + + float32_t scalar = cos(norm / 2.0f); + + Matrix<3, 1> vector = sin(norm / 2.0f) * (phi / norm); + + return Quaternion(vector, scalar); +} + +Matrix<3, 1> lie::operators::log(Quaternion phi) { + const Matrix<3, 1> vector = phi.vector(); + float32_t norm = vector.norm(); + + if (norm < epsilon) { + float32_t sign = std::copysign(1, phi.scalar()); + + return sign * vector; + } + + return 2 * atan2(vector.norm(), phi.scalar()) * (vector / vector.norm()); +} + +Matrix<3, 3> lie::operators::gamma(Matrix<3, 1> phi) { + float32_t norm = phi.norm(); + + Matrix<3, 3> skew_phi = lie::operators::skew(phi); + + if (norm < epsilon) { + return Matrix<3, 3>::Identity() + (0.5f * skew_phi); + } + + return Matrix<3, 3>::Identity() + ((1 - cos(norm)) * skew_phi) / (norm * norm) + ((norm - sin(norm)) * skew_phi * skew_phi) / (norm * norm * norm); +} + +Matrix<3, 3> lie::operators::skew(Matrix<3, 1> vector) { + float32_t a1 = vector.at(0, 0); + float32_t a2 = vector.at(1, 0); + float32_t a3 = vector.at(2, 0); + + return Matrix<3, 3>({ + 0.0, -a3, a2, + a3, 0.0, -a1, + -a2, a1, 0.0 + }); +} + +Quaternion::Quaternion(float32_t quat[4]) { + std::copy(quat, quat + 4, quat_); +} + +Quaternion::Quaternion(const std::array values) { + std::copy(values.begin(), values.end(), quat_); +} + +Quaternion::Quaternion(const Matrix<3, 1> vector, float32_t scalar) { + quat_[0] = vector.at(0, 0); + quat_[1] = vector.at(1, 0); + quat_[2] = vector.at(2, 0); + + quat_[3] = scalar; +} + +Quaternion::Quaternion(const Quaternion &other) { + std::copy(other.quat_, other.quat_ + 4, quat_); +} + +float32_t Quaternion::scalar() const { + return quat_[3]; +} + +const Matrix<3, 1> Quaternion::vector() const { + return Matrix<3, 1>(quat_); +} diff --git a/src/main.cc b/src/main.cc index 17b79ea..d1b54bb 100644 --- a/src/main.cc +++ b/src/main.cc @@ -1,8 +1,7 @@ #include -#include "arm_math.h" #include "matrix.h" -#include "lqr.h" +#include "lie.h" #include From 64b177d3ae747496dbadaf61db49ecc6b187ac1c Mon Sep 17 00:00:00 2001 From: Maxwell Henderson Date: Tue, 17 Feb 2026 19:35:15 -0800 Subject: [PATCH 4/6] Expand lie operators Signed-off-by: Maxwell Henderson --- include/lie.h | 3 +++ include/matrix.h | 13 +++++++++++++ src/lie.cc | 9 +++++++++ 3 files changed, 25 insertions(+) diff --git a/include/lie.h b/include/lie.h index abda2da..6775275 100644 --- a/include/lie.h +++ b/include/lie.h @@ -23,6 +23,9 @@ class Quaternion { // Returns the vector part of the quaternion const Matrix<3, 1> vector() const; + + // Applies the rotation of the quaternion to a vector p + Matrix<3, 1> apply(const Matrix<3, 1> &p) const; protected: float32_t quat_[4]; diff --git a/include/matrix.h b/include/matrix.h index 2e04a21..20bda87 100644 --- a/include/matrix.h +++ b/include/matrix.h @@ -126,6 +126,19 @@ template class Matrix { arm_rms_f32(const_cast(matrix_data_), Rows, &result); return result; } + + // Matrix cross product which is only valid for 3, 1 matrices. + static Matrix<3, 1> cross(Matrix<3, 1> a, Matrix<3, 1> b) { + const float32_t a1 = a.at(0, 0); + const float32_t a2 = a.at(1, 0); + const float32_t a3 = a.at(2, 0); + + const float32_t b1 = b.at(0, 0); + const float32_t b2 = b.at(1, 0); + const float32_t b3 = b.at(2, 0); + + return Matrix<3, 1>({a2 * b3 - a3 * b2, a3 * b1 - a1 * b3, a1 * b2 - a2 * b1}); + } constexpr inline void set(int i, int j, float32_t data) { matrix_data_[i * Cols + j] = data; diff --git a/src/lie.cc b/src/lie.cc index 3f6f29d..45651ca 100644 --- a/src/lie.cc +++ b/src/lie.cc @@ -82,3 +82,12 @@ float32_t Quaternion::scalar() const { const Matrix<3, 1> Quaternion::vector() const { return Matrix<3, 1>(quat_); } + +// Implements the second algorithm in "Used methods" +// https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation +Matrix<3, 1> Quaternion::apply(const Matrix<3, 1> &p) const { + const Matrix<3, 1> v = vector(); + const float32_t w = scalar(); + + return p + Matrix<3, 1>::cross(2 * v, Matrix<3, 1>::cross(v, p) + w * p); +} From 9f5c64deb7ff4e606e984895a2f71533ea6e3b84 Mon Sep 17 00:00:00 2001 From: Maxwell Henderson Date: Wed, 18 Feb 2026 14:01:40 -0800 Subject: [PATCH 5/6] Add partial dynamics code Signed-off-by: Maxwell Henderson --- include/dynamics.h | 94 ++++++++++++++++++++++++++++++++++++++++++++++ include/lie.h | 2 + include/matrix.h | 6 +++ src/dynamics.cc | 30 +++++++++++++++ src/lie.cc | 17 +++++++++ 5 files changed, 149 insertions(+) create mode 100644 include/dynamics.h create mode 100644 src/dynamics.cc diff --git a/include/dynamics.h b/include/dynamics.h new file mode 100644 index 0000000..ca40845 --- /dev/null +++ b/include/dynamics.h @@ -0,0 +1,94 @@ +#ifndef DYNAMICS_H_ +#define DYNAMICS_H_ + +#include "matrix.h" + +namespace controls { +class Dynamics { + public: + enum States { + kX = 0, + kY = 1, + kZ = 2, + + kXdot = 3, + kYdot = 4, + kZdot = 5, + + kQuatX = 6, + kQuatY = 7, + kQuatZ = 8, + kQuatW = 9, + + kOmegaX = 10, + kOmegaY = 11, + kOmegaZ = 12, + + // External torques and forces + kFX = 13, + kFY = 14, + kFZ = 15, + + kTX = 16, + kTY = 17, + kTZ = 18, + kNumStates = 19, + }; + + enum ErrorStates { + kDX = 0, + kDY = 1, + kDZ = 2, + + kDXdot = 3, + kDYdot = 4, + kDZdot = 5, + + kDThetaX = 6, + kDThetaY = 7, + kDThetaZ = 8, + + kDOmegaX = 9, + kDOmegaY = 10, + kDomegaZ = 11, + + kDFX = 12, + kDFY = 13, + kDFZ = 14, + + kDTX = 15, + KDTY = 16, + KDTZ = 17, + kNumErrorStates = 18, + }; + + enum Inputs { + kWProp = 0, + kTrw = 1, + kTheta = 2, + kPsi = 3, + kNumInputs = 4, + }; + + struct DynamicsConstants { + float32_t mass; + float32_t r; + + Matrix<3, 3> inertia; + }; + + typedef Matrix State; + typedef Matrix Input; + + Dynamics(DynamicsConstants constants); + + private: + State f(State X, Input U); + + float32_t prop_thrust(float32_t omega); + + DynamicsConstants constants_; +}; +}; + +#endif // DYNAMICS_H_ diff --git a/include/lie.h b/include/lie.h index 6775275..841ae65 100644 --- a/include/lie.h +++ b/include/lie.h @@ -27,6 +27,8 @@ class Quaternion { // Applies the rotation of the quaternion to a vector p Matrix<3, 1> apply(const Matrix<3, 1> &p) const; + static Quaternion from_zyx(float32_t roll, float32_t pitch, float32_t yaw); + protected: float32_t quat_[4]; }; diff --git a/include/matrix.h b/include/matrix.h index 20bda87..69dd9de 100644 --- a/include/matrix.h +++ b/include/matrix.h @@ -102,6 +102,12 @@ template class Matrix { return out; } + float32_t operator[](uint32_t i) { + static_assert(Cols == 1, "[] operator only defined for column vector"); + + return at(i, 0); + } + Matrix transpose() const { Matrix result; diff --git a/src/dynamics.cc b/src/dynamics.cc new file mode 100644 index 0000000..460eec3 --- /dev/null +++ b/src/dynamics.cc @@ -0,0 +1,30 @@ +#include "dynamics.h" + +#include "lie.h" + +using controls::Dynamics; +typedef controls::Dynamics::States States; +typedef controls::Dynamics::Inputs Inputs; +typedef controls::Dynamics::State State; +typedef controls::Dynamics::Input Input; + +using lie::Quaternion; + +Dynamics::Dynamics(Dynamics::DynamicsConstants constants): + constants_(constants) {} + +State Dynamics::f(State X, Input U) { + const Matrix<3, 1> lift_prop{0.0, 0.0, prop_thrust(U[Inputs::kWProp])}; + + Quaternion prop_body_quat = Quaternion::from_zyx(0.0, U[Inputs::kTheta], U[Inputs::kPsi]); + + auto lift_body = prop_body_quat.apply(lift_prop); + + const Matrix<3, 1> r{0.0, 0.0, constants_.r}; + + // T = Matrix<3,1>::cross(r, lift_body) + U[Inputs::kTrw] + Matrix<3, 1>{0.0, 0.0, self.prop_torque()}; +} + +float32_t Dynamics::prop_thrust(float32_t omega) { + return 0.000020759 * omega*omega - 0.00605731 * omega + 1.12302; +} diff --git a/src/lie.cc b/src/lie.cc index 45651ca..c44e0e8 100644 --- a/src/lie.cc +++ b/src/lie.cc @@ -91,3 +91,20 @@ Matrix<3, 1> Quaternion::apply(const Matrix<3, 1> &p) const { return p + Matrix<3, 1>::cross(2 * v, Matrix<3, 1>::cross(v, p) + w * p); } + +Quaternion Quaternion::from_zyx(float32_t roll, float32_t pitch, float32_t yaw) { + const float32_t cx = cos(roll/2.0); + const float32_t cy = cos(pitch/2.0); + const float32_t cz = cos(yaw/2.0); + + const float32_t sx = sin(roll/2.0); + const float32_t sy = sin(pitch/2.0); + const float32_t sz = sin(yaw/2.0); + + const float32_t w = cx * cy * cz + sx * sy * sz; + const float32_t x = sx * cy * cz - cx * sy * sz; + const float32_t y = cx * sy * cz + sx * cy * sz; + const float32_t z = cx * cy * sz - sx * sy * cz; + + return Quaternion(Matrix<3, 1>({x, y, z}), w); +} From 241537c094d6e535263c8403cfdc7f36503b02b7 Mon Sep 17 00:00:00 2001 From: Maxwell Henderson Date: Wed, 18 Feb 2026 18:08:30 -0800 Subject: [PATCH 6/6] Add quaternion composition Signed-off-by: Maxwell Henderson --- include/lie.h | 3 +++ include/matrix.h | 6 ++++++ src/dynamics.cc | 4 ++-- src/lie.cc | 12 ++++++++++++ 4 files changed, 23 insertions(+), 2 deletions(-) diff --git a/include/lie.h b/include/lie.h index 841ae65..cae2247 100644 --- a/include/lie.h +++ b/include/lie.h @@ -29,6 +29,9 @@ class Quaternion { static Quaternion from_zyx(float32_t roll, float32_t pitch, float32_t yaw); + // Quaternion composition aka the quaternion product + static Quaternion compose(Quaternion a, Quaternion b); + protected: float32_t quat_[4]; }; diff --git a/include/matrix.h b/include/matrix.h index 69dd9de..05e09d0 100644 --- a/include/matrix.h +++ b/include/matrix.h @@ -35,6 +35,12 @@ template class Matrix { return *this; } + template ::type> + operator float() const { + return matrix_data_[0]; + } + static constexpr Matrix Zero() { Matrix matrix{}; std::fill(matrix.matrix_data_, matrix.matrix_data_ + kDataSize, 0.0f); diff --git a/src/dynamics.cc b/src/dynamics.cc index 460eec3..52470a8 100644 --- a/src/dynamics.cc +++ b/src/dynamics.cc @@ -14,13 +14,13 @@ Dynamics::Dynamics(Dynamics::DynamicsConstants constants): constants_(constants) {} State Dynamics::f(State X, Input U) { - const Matrix<3, 1> lift_prop{0.0, 0.0, prop_thrust(U[Inputs::kWProp])}; + const Matrix<3, 1> lift_prop({0.0, 0.0, prop_thrust(U[Inputs::kWProp])}); Quaternion prop_body_quat = Quaternion::from_zyx(0.0, U[Inputs::kTheta], U[Inputs::kPsi]); auto lift_body = prop_body_quat.apply(lift_prop); - const Matrix<3, 1> r{0.0, 0.0, constants_.r}; + const Matrix<3, 1> r({0.0, 0.0, constants_.r}); // T = Matrix<3,1>::cross(r, lift_body) + U[Inputs::kTrw] + Matrix<3, 1>{0.0, 0.0, self.prop_torque()}; } diff --git a/src/lie.cc b/src/lie.cc index c44e0e8..ebd1adf 100644 --- a/src/lie.cc +++ b/src/lie.cc @@ -108,3 +108,15 @@ Quaternion Quaternion::from_zyx(float32_t roll, float32_t pitch, float32_t yaw) return Quaternion(Matrix<3, 1>({x, y, z}), w); } + +Quaternion Quaternion::compose(Quaternion a, Quaternion b) { + const float32_t aw = a.scalar(); + const float32_t bw = b.scalar(); + + auto a_v = a.vector(); + auto b_v = b.vector(); + + auto vector = aw * b_v + bw * a_v + Matrix<3,1>::cross(a_v, b_v); + + return Quaternion(vector, aw * bw - a_v.transpose() * b_v); +}