Add Embedded Controls Code

.gitignore

@@ -3,3 +3,5 @@
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 .vscode/ipch
+
+.envrc

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

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<kNumStates, 1> State;
+  typedef Matrix<kNumInputs, 1> Input;
+
+  Dynamics(DynamicsConstants constants);
+
+  private:
+    State f(State X, Input U);
+
+    float32_t prop_thrust(float32_t omega);
+
+    DynamicsConstants constants_;
+};
+};
+
+#endif // DYNAMICS_H_

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_

include/lie.h

@@ -0,0 +1,56 @@
+#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<float32_t, 4> 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;
+
+    // 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);
+
+    // Quaternion composition aka the quaternion product
+    static Quaternion compose(Quaternion a, Quaternion b);
+
+  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_

include/lqr.h

@@ -0,0 +1,72 @@
+#ifndef LQR_H_
+#define LQR_H_
+
+#include "matrix.h"
+#include "cstddef"
+
+namespace controls {
+
+template <size_t States, size_t Inputs>
+class LQR {
+  public:
+    static constexpr int kNumTaylorExpansions = 5;
+
+    LQR(Matrix<States, States> Q, Matrix<Inputs, Inputs> 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<Inputs, States> SolveDare(Matrix<States, States> A, Matrix<States, Inputs> B, float32_t dt, int steps=500) {
+      Matrix<States, States> A_d;
+      Matrix<States, Inputs> B_d;
+
+      DiscretizeAB(&A_d, &B_d, A, B, dt);
+
+      Matrix<States, States> Q_d = Q_ * dt;
+      Matrix<Inputs, Inputs> R_d = R_ * (1.0 / dt);
+
+      Matrix<States, States> 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<States, States>::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<States, States> &A_d, Matrix<States, Inputs> &B_d, Matrix<States, States> A, Matrix<States, States> B, float32_t dt) {
+      // A_d = e^A(dt)
+      auto term1 = A * dt;
+      A_d = Matrix<States, States>::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<States, States>::Identity() * dt + inner_accum) * B;
+    }
+
+    Matrix<States, States> Q_;
+    Matrix<Inputs, Inputs> R_;
+};
+
+};
+
+#endif // LQR_H_