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/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<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_
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..cae2247
--- /dev/null
+++ b/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_
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 <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_
diff --git a/include/matrix.h b/include/matrix.h
new file mode 100644
index 0000000..05e09d0
--- /dev/null
+++ b/include/matrix.h
@@ -0,0 +1,183 @@
+#ifndef MATRIX_H_
+#define MATRIX_H_
+
+#include "arm_math.h"
+#include <Arduino.h>
+
+// Wrapper around a f32 cmsis dsp matrix object
+// TODO(max): Chain optimization is required
+
+template <int R, int C> class Matrix;
+template <int Rows, int Cols> class Matrix {
+public:
+  static constexpr size_t kDataSize = Rows * Cols;
+
+  template <int R, int C> friend class Matrix;
+
+  Matrix(const float32_t data[kDataSize])
+      : matrix_instance_{Rows, Cols, matrix_data_} {
+    std::copy(data, data + kDataSize, matrix_data_);
+  }
+  
+  explicit Matrix(const std::array<float32_t, kDataSize> 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;
+  }
+
+  template <int Row = Rows, int Col = Cols, 
+              typename = typename std::enable_if<Row == 1 && Col == 1>::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);
+    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<Rows, Cols> &rhs) const {
+    Matrix result;
+
+    arm_mat_add_f32(&matrix_instance_, &rhs.matrix_instance_,
+                    &result.matrix_instance_);
+
+    return result;
+  }
+
+  Matrix operator-(const Matrix<Rows, Cols> &rhs) const {
+    Matrix result;
+
+    arm_mat_sub_f32(&matrix_instance_, &rhs.matrix_instance_,
+                    &result.matrix_instance_);
+
+    return result;
+  }
+
+  template <int RhsCols>
+  Matrix<Rows, RhsCols> operator*(const Matrix<Cols, RhsCols> &rhs) const {
+    Matrix<Rows, RhsCols> result;
+
+    arm_mat_mult_f32(&matrix_instance_, &rhs.matrix_instance_,
+                     &result.matrix_instance_);
+
+    return result;
+  }
+
+  Matrix<Rows, Cols> operator*(const float32_t scalar) const {
+    Matrix<Rows, Cols> out;
+
+    arm_mat_scale_f32(&matrix_instance_, scalar, &out.matrix_instance_);
+
+    return out;
+  }
+
+  friend Matrix operator*(const float32_t rhs, const Matrix &lhs) {
+    return lhs * rhs;
+  }
+
+  Matrix<Rows, Cols> operator/(const float32_t scalar) const {
+    Matrix<Rows, Cols> out;
+
+    arm_mat_scale_f32(&matrix_instance_, 1.0f / scalar, &out.matrix_instance_);
+
+    return out;
+  }
+
+  float32_t operator[](uint32_t i) {
+    static_assert(Cols == 1, "[] operator only defined for column vector");
+
+    return at(i, 0);
+  }
+
+  Matrix<Cols, Rows> transpose() const {
+    Matrix<Cols, Rows> 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;
+  }
+
+  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<float32_t *>(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;
+  }
+
+  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/dynamics.cc b/src/dynamics.cc
new file mode 100644
index 0000000..52470a8
--- /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
new file mode 100644
index 0000000..ebd1adf
--- /dev/null
+++ b/src/lie.cc
@@ -0,0 +1,122 @@
+#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<float32_t, 4> 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_);
+}
+
+// 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);
+}
+
+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);
+}
+
+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);
+}
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
new file mode 100644
index 0000000..d1b54bb
--- /dev/null
+++ b/src/main.cc
@@ -0,0 +1,18 @@
+#include <Arduino.h>
+
+#include "matrix.h"
+#include "lie.h"
+
+#include <memory>
+
+
+void setup() {
+  Serial.begin(115200);
+
+  Matrix<1,2> mat({1, 2});
+
+  while (!Serial) {
+  }
+}
+
+void loop() {}