diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 27ad4fb..971f0b8 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -17,7 +17,8 @@
 #include "aos/common/macros.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 struct StateFeedbackHybridPlantCoefficients final {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -32,14 +33,14 @@
         U_max(other.U_max) {}
 
   StateFeedbackHybridPlantCoefficients(
-      const Eigen::Matrix<double, number_of_states, number_of_states>
+      const Eigen::Matrix<Scalar, number_of_states, number_of_states>
           &A_continuous,
-      const Eigen::Matrix<double, number_of_states, number_of_inputs>
+      const Eigen::Matrix<Scalar, number_of_states, number_of_inputs>
           &B_continuous,
-      const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
-      const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
-      const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
-      const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
+      const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
+      const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min)
       : A_continuous(A_continuous),
         B_continuous(B_continuous),
         C(C),
@@ -47,15 +48,16 @@
         U_min(U_min),
         U_max(U_max) {}
 
-  const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous;
-  const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous;
-  const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
-  const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
-  const Eigen::Matrix<double, number_of_inputs, 1> U_min;
-  const Eigen::Matrix<double, number_of_inputs, 1> U_max;
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> A_continuous;
+  const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B_continuous;
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> C;
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
 };
 
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 class StateFeedbackHybridPlant {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -77,49 +79,49 @@
 
   virtual ~StateFeedbackHybridPlant() {}
 
-  const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A() const {
     return A_;
   }
-  double A(int i, int j) const { return A()(i, j); }
-  const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+  Scalar A(int i, int j) const { return A()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B() const {
     return B_;
   }
-  double B(int i, int j) const { return B()(i, j); }
-  const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+  Scalar B(int i, int j) const { return B()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C() const {
     return coefficients().C;
   }
-  double C(int i, int j) const { return C()(i, j); }
-  const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+  Scalar C(int i, int j) const { return C()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D() const {
     return coefficients().D;
   }
-  double D(int i, int j) const { return D()(i, j); }
-  const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+  Scalar D(int i, int j) const { return D()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min() const {
     return coefficients().U_min;
   }
-  double U_min(int i, int j) const { return U_min()(i, j); }
-  const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+  Scalar U_min(int i, int j) const { return U_min()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max() const {
     return coefficients().U_max;
   }
-  double U_max(int i, int j) const { return U_max()(i, j); }
+  Scalar U_max(int i, int j) const { return U_max()(i, j); }
 
-  const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
-  double X(int i, int j) const { return X()(i, j); }
-  const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
-  double Y(int i, int j) const { return Y()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
+  Scalar X(int i, int j) const { return X()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
+  Scalar Y(int i, int j) const { return Y()(i, j); }
 
-  Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
-  double &mutable_X(int i, int j) { return mutable_X()(i, j); }
-  Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
-  double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+  Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
+  Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+  Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
+  Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
 
   const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
-                                             number_of_outputs>
+                                             number_of_outputs, Scalar>
       &coefficients(int index) const {
     return *coefficients_[index];
   }
 
   const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
-                                             number_of_outputs>
+                                             number_of_outputs, Scalar>
       &coefficients() const {
     return *coefficients_[index_];
   }
@@ -141,36 +143,38 @@
   }
 
   // Assert that U is within the hardware range.
-  virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+  virtual void CheckU(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
     for (int i = 0; i < kNumInputs; ++i) {
-      if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+      if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
+          U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
         LOG(FATAL, "U out of range\n");
       }
     }
   }
 
   // Computes the new X and Y given the control input.
-  void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U,
+  void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
               ::std::chrono::nanoseconds dt) {
     // Powers outside of the range are more likely controller bugs than things
     // that the plant should deal with.
     CheckU(U);
     ::aos::robot_state.FetchLatest();
 
-    Eigen::Matrix<double, number_of_inputs, 1> current_U =
-        DelayedU_ * (::aos::robot_state.get()
-                         ? ::aos::robot_state->voltage_battery / 12.0
-                         : 1.0);
+    Eigen::Matrix<Scalar, number_of_inputs, 1> current_U =
+        DelayedU_ *
+        (::aos::robot_state.get()
+             ? ::aos::robot_state->voltage_battery / static_cast<Scalar>(12.0)
+             : static_cast<Scalar>(1.0));
     X_ = Update(X(), current_U, dt);
     Y_ = C() * X() + D() * current_U;
     DelayedU_ = U;
   }
 
-  Eigen::Matrix<double, number_of_inputs, 1> DelayedU_;
+  Eigen::Matrix<Scalar, number_of_inputs, 1> DelayedU_;
 
-  Eigen::Matrix<double, number_of_states, 1> Update(
-      const Eigen::Matrix<double, number_of_states, 1> X,
-      const Eigen::Matrix<double, number_of_inputs, 1> &U,
+  Eigen::Matrix<Scalar, number_of_states, 1> Update(
+      const Eigen::Matrix<Scalar, number_of_states, 1> X,
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
       ::std::chrono::nanoseconds dt) {
     UpdateAB(dt);
     return A() * X + B() * U;
@@ -184,22 +188,22 @@
 
  private:
   void UpdateAB(::std::chrono::nanoseconds dt) {
-    Eigen::Matrix<double, number_of_states + number_of_inputs,
+    Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
                   number_of_states + number_of_inputs>
         M_state_continuous;
     M_state_continuous.setZero();
     M_state_continuous.template block<number_of_states, number_of_states>(0,
                                                                           0) =
         coefficients().A_continuous *
-        ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+        ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
             .count();
     M_state_continuous.template block<number_of_states, number_of_inputs>(
         0, number_of_states) =
         coefficients().B_continuous *
-        ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+        ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
             .count();
 
-    Eigen::Matrix<double, number_of_states + number_of_inputs,
+    Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
                   number_of_states + number_of_inputs>
         M_state = M_state_continuous.exp();
     A_ = M_state.template block<number_of_states, number_of_states>(0, 0);
@@ -207,11 +211,11 @@
         0, number_of_states);
   }
 
-  Eigen::Matrix<double, number_of_states, 1> X_;
-  Eigen::Matrix<double, number_of_outputs, 1> Y_;
+  Eigen::Matrix<Scalar, number_of_states, 1> X_;
+  Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
 
-  Eigen::Matrix<double, number_of_states, number_of_states> A_;
-  Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+  Eigen::Matrix<Scalar, number_of_states, number_of_states> A_;
+  Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B_;
 
 
   ::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
@@ -225,34 +229,36 @@
 
 
 // A container for all the observer coefficients.
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 struct HybridKalmanCoefficients final {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
-  const Eigen::Matrix<double, number_of_states, number_of_states> Q_continuous;
-  const Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_continuous;
-  const Eigen::Matrix<double, number_of_states, number_of_states> P_steady_state;
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous;
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_continuous;
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> P_steady_state;
 
   HybridKalmanCoefficients(
-      const Eigen::Matrix<double, number_of_states, number_of_states>
+      const Eigen::Matrix<Scalar, number_of_states, number_of_states>
           &Q_continuous,
-      const Eigen::Matrix<double, number_of_outputs, number_of_outputs>
+      const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
           &R_continuous,
-      const Eigen::Matrix<double, number_of_states, number_of_states>
+      const Eigen::Matrix<Scalar, number_of_states, number_of_states>
           &P_steady_state)
       : Q_continuous(Q_continuous),
         R_continuous(R_continuous),
         P_steady_state(P_steady_state) {}
 };
 
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 class HybridKalman {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
   explicit HybridKalman(
       ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
-          number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+          number_of_states, number_of_inputs, number_of_outputs, Scalar>>> *observers)
       : coefficients_(::std::move(*observers)) {}
 
   HybridKalman(HybridKalman &&other)
@@ -261,27 +267,27 @@
   }
 
   // Getters for Q
-  const Eigen::Matrix<double, number_of_states, number_of_states> &Q() const {
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q() const {
     return Q_;
   }
-  double Q(int i, int j) const { return Q()(i, j); }
+  Scalar Q(int i, int j) const { return Q()(i, j); }
   // Getters for R
-  const Eigen::Matrix<double, number_of_outputs, number_of_outputs> &R() const {
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R() const {
     return R_;
   }
-  double R(int i, int j) const { return R()(i, j); }
+  Scalar R(int i, int j) const { return R()(i, j); }
 
   // Getters for P
-  const Eigen::Matrix<double, number_of_states, number_of_states> &P() const {
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> &P() const {
     return P_;
   }
-  double P(int i, int j) const { return P()(i, j); }
+  Scalar P(int i, int j) const { return P()(i, j); }
 
   // Getters for X_hat
-  const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+  const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
     return X_hat_;
   }
-  Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+  Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
 
   void Reset(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
                                       number_of_outputs> *plant) {
@@ -291,8 +297,8 @@
   }
 
   void Predict(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
-                                        number_of_outputs> *plant,
-               const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+                                        number_of_outputs, Scalar> *plant,
+               const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
                ::std::chrono::nanoseconds dt) {
     // Trigger the predict step.  This will update A() and B() in the plant.
     mutable_X_hat() = plant->Update(X_hat(), new_u, dt);
@@ -303,14 +309,14 @@
 
   void Correct(
       const StateFeedbackHybridPlant<number_of_states, number_of_inputs,
-                                     number_of_outputs> &plant,
-      const Eigen::Matrix<double, number_of_inputs, 1> &U,
-      const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
-    Eigen::Matrix<double, number_of_outputs, 1> Y_bar =
+                                     number_of_outputs, Scalar> &plant,
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+      const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
+    Eigen::Matrix<Scalar, number_of_outputs, 1> Y_bar =
         Y - (plant.C() * X_hat_ + plant.D() * U);
-    Eigen::Matrix<double, number_of_outputs, number_of_outputs> S =
+    Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> S =
         plant.C() * P_ * plant.C().transpose() + R_;
-    Eigen::Matrix<double, number_of_states, number_of_outputs> KalmanGain;
+    Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
     KalmanGain =
         (S.transpose().ldlt().solve((P() * plant.C().transpose()).transpose()))
             .transpose();
@@ -350,16 +356,16 @@
                                          number_of_outputs> *plant,
                 ::std::chrono::nanoseconds dt) {
     // Now, compute the discrete time Q and R coefficients.
-    Eigen::Matrix<double, number_of_states, number_of_states> Qtemp =
+    Eigen::Matrix<Scalar, number_of_states, number_of_states> Qtemp =
         (coefficients().Q_continuous +
          coefficients().Q_continuous.transpose()) /
-        2.0;
-    Eigen::Matrix<double, number_of_outputs, number_of_outputs> Rtemp =
+        static_cast<Scalar>(2.0);
+    Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> Rtemp =
         (coefficients().R_continuous +
          coefficients().R_continuous.transpose()) /
-        2.0;
+        static_cast<Scalar>(2.0);
 
-    Eigen::Matrix<double, 2 * number_of_states, 2 * number_of_states> M_gain;
+    Eigen::Matrix<Scalar, 2 * number_of_states, 2 * number_of_states> M_gain;
     M_gain.setZero();
     // Set up the matrix M = [[-A, Q], [0, A.T]]
     M_gain.template block<number_of_states, number_of_states>(0, 0) =
@@ -370,35 +376,35 @@
         number_of_states, number_of_states) =
         plant->coefficients().A_continuous.transpose();
 
-    Eigen::Matrix<double, 2 * number_of_states, 2 *number_of_states> phi =
+    Eigen::Matrix<Scalar, 2 * number_of_states, 2 *number_of_states> phi =
         (M_gain *
-         ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+         ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
              .count())
             .exp();
 
     // Phi12 = phi[0:number_of_states, number_of_states:2*number_of_states]
     // Phi22 = phi[number_of_states:2*number_of_states,
     // number_of_states:2*number_of_states]
-    Eigen::Matrix<double, number_of_states, number_of_states> phi12 =
+    Eigen::Matrix<Scalar, number_of_states, number_of_states> phi12 =
         phi.block(0, number_of_states, number_of_states, number_of_states);
-    Eigen::Matrix<double, number_of_states, number_of_states> phi22 = phi.block(
+    Eigen::Matrix<Scalar, number_of_states, number_of_states> phi22 = phi.block(
         number_of_states, number_of_states, number_of_states, number_of_states);
 
     Q_ = phi22.transpose() * phi12;
-    Q_ = (Q_ + Q_.transpose()) / 2.0;
+    Q_ = (Q_ + Q_.transpose()) / static_cast<Scalar>(2.0);
     R_ = Rtemp /
-         ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+         ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
              .count();
   }
 
   // Internal state estimate.
-  Eigen::Matrix<double, number_of_states, 1> X_hat_;
+  Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
   // Internal covariance estimate.
-  Eigen::Matrix<double, number_of_states, number_of_states> P_;
+  Eigen::Matrix<Scalar, number_of_states, number_of_states> P_;
 
   // Discretized Q and R for the kalman filter.
-  Eigen::Matrix<double, number_of_states, number_of_states> Q_;
-  Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_;
+  Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_;
+  Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_;
 
   int index_ = 0;
   ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 229bbad..401be44 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -106,10 +106,10 @@
   return HybridKalman<3, 1, 1>(&observers);
 }
 
-StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
                   HybridKalman<3, 1, 1>>
 MakeIntegralShooterLoop() {
-  return StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+  return StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
                            HybridKalman<3, 1, 1>>(
       MakeIntegralShooterPlant(), MakeIntegralShooterController(),
       MakeIntegralShooterObserver());
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 50e4314..805e079 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -18,7 +18,8 @@
 class ControlLoopWriter(object):
   def __init__(self, gain_schedule_name, loops, namespaces=None,
                write_constants=False, plant_type='StateFeedbackPlant',
-               observer_type='StateFeedbackObserver'):
+               observer_type='StateFeedbackObserver',
+               scalar_type='double'):
     """Constructs a control loop writer.
 
     Args:
@@ -29,6 +30,7 @@
         order.  If None, the default will be used.
       plant_type: string, The C++ type of the plant.
       observer_type: string, The C++ type of the observer.
+      scalar_type: string, The C++ type of the base scalar.
     """
     self._gain_schedule_name = gain_schedule_name
     self._loops = loops
@@ -46,6 +48,7 @@
     self._constant_list = []
     self._plant_type = plant_type
     self._observer_type = observer_type
+    self._scalar_type = scalar_type
 
   def AddConstant(self, constant):
     """Adds a constant to write.
@@ -77,6 +80,8 @@
       extra_args = ', ' + extra_args
     else:
       extra_args = ''
+    if self._scalar_type != 'double':
+      extra_args += ', ' + self._scalar_type
     return '%s<%d, %d, %d%s>' % (
         typename, num_states, num_inputs, num_outputs, extra_args)
 
@@ -90,8 +95,16 @@
 
   def _LoopType(self):
     """Returns a template name for StateFeedbackLoop."""
-    extra_args = '%s, %s' % (self._PlantType(), self._ObserverType())
-    return self._GenericType('StateFeedbackLoop', extra_args)
+    num_states = self._loops[0].A.shape[0]
+    num_inputs = self._loops[0].B.shape[1]
+    num_outputs = self._loops[0].C.shape[0]
+
+    return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
+        num_states,
+        num_inputs,
+        num_outputs, self._scalar_type,
+        self._PlantType(), self._ObserverType())
+
 
   def _PlantType(self):
     """Returns a template name for StateFeedbackPlant."""
@@ -109,10 +122,8 @@
     """Returns a template name for StateFeedbackObserverCoefficients."""
     return self._GenericType(self._observer_type + 'Coefficients')
 
-  def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
-    """Writes the header file to the file named header_file.
-       Set double_appendage to true in order to include a ratio of
-       moments of inertia constant. Currently, only used for 2014 claw."""
+  def WriteHeader(self, header_file):
+    """Writes the header file to the file named header_file."""
     with open(header_file, 'w') as fd:
       header_guard = self._HeaderGuard(header_file)
       fd.write('#ifndef %s\n'
@@ -133,7 +144,7 @@
       for loop in self._loops:
         fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
         fd.write('\n')
-        fd.write(loop.DumpControllerHeader())
+        fd.write(loop.DumpControllerHeader(self._scalar_type))
         fd.write('\n')
         fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
         fd.write('\n')
@@ -167,15 +178,15 @@
       fd.write(self._namespace_start)
       fd.write('\n\n')
       for loop in self._loops:
-        fd.write(loop.DumpPlant(self._PlantCoeffType()))
+        fd.write(loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
         fd.write('\n')
 
       for loop in self._loops:
-        fd.write(loop.DumpController())
+        fd.write(loop.DumpController(self._scalar_type))
         fd.write('\n')
 
       for loop in self._loops:
-        fd.write(loop.DumpObserver(self._ObserverCoeffType()))
+        fd.write(loop.DumpObserver(self._ObserverCoeffType(), self._scalar_type))
         fd.write('\n')
 
       fd.write('%s Make%sPlant() {\n' %
@@ -288,22 +299,26 @@
     self.X_hat = (self.A * self.X_hat + self.B * U +
                   self.L * (self.Y - self.C * self.X_hat - self.D * U))
 
-  def _DumpMatrix(self, matrix_name, matrix):
+  def _DumpMatrix(self, matrix_name, matrix, scalar_type):
     """Dumps the provided matrix into a variable called matrix_name.
 
     Args:
       matrix_name: string, The variable name to save the matrix to.
       matrix: The matrix to dump.
+      scalar_type: The C++ type to use for the scalar in the matrix.
 
     Returns:
       string, The C++ commands required to populate a variable named matrix_name
         with the contents of matrix.
     """
-    ans = ['  Eigen::Matrix<double, %d, %d> %s;\n' % (
-        matrix.shape[0], matrix.shape[1], matrix_name)]
+    ans = ['  Eigen::Matrix<%s, %d, %d> %s;\n' % (
+        scalar_type, matrix.shape[0], matrix.shape[1], matrix_name)]
     for x in xrange(matrix.shape[0]):
       for y in xrange(matrix.shape[1]):
-        ans.append('  %s(%d, %d) = %s;\n' % (matrix_name, x, y, repr(matrix[x, y])))
+        write_type =  repr(matrix[x, y])
+        if scalar_type == 'float':
+          write_type += 'f'
+        ans.append('  %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
 
     return ''.join(ans)
 
@@ -316,7 +331,7 @@
     return '%s Make%sPlantCoefficients();\n' % (
         plant_coefficient_type, self._name)
 
-  def DumpPlant(self, plant_coefficient_type):
+  def DumpPlant(self, plant_coefficient_type, scalar_type):
     """Writes out a c++ function which will create a PlantCoefficients object.
 
     Returns:
@@ -325,21 +340,21 @@
     ans = ['%s Make%sPlantCoefficients() {\n' % (
         plant_coefficient_type, self._name)]
 
-    ans.append(self._DumpMatrix('C', self.C))
-    ans.append(self._DumpMatrix('D', self.D))
-    ans.append(self._DumpMatrix('U_max', self.U_max))
-    ans.append(self._DumpMatrix('U_min', self.U_min))
+    ans.append(self._DumpMatrix('C', self.C, scalar_type))
+    ans.append(self._DumpMatrix('D', self.D, scalar_type))
+    ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
+    ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
 
     if plant_coefficient_type.startswith('StateFeedbackPlant'):
-      ans.append(self._DumpMatrix('A', self.A))
-      ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
-      ans.append(self._DumpMatrix('B', self.B))
+      ans.append(self._DumpMatrix('A', self.A, scalar_type))
+      ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A), scalar_type))
+      ans.append(self._DumpMatrix('B', self.B, scalar_type))
       ans.append('  return %s'
                  '(A, A_inv, B, C, D, U_max, U_min);\n' % (
                      plant_coefficient_type))
     elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
-      ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
-      ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
+      ans.append(self._DumpMatrix('A_continuous', self.A_continuous, scalar_type))
+      ans.append(self._DumpMatrix('B_continuous', self.B_continuous, scalar_type))
       ans.append('  return %s'
                  '(A_continuous, B_continuous, C, D, U_max, U_min);\n' % (
                      plant_coefficient_type))
@@ -361,7 +376,7 @@
     """Returns the name of the controller function."""
     return 'Make%sObserverCoefficients()' % self._name
 
-  def DumpControllerHeader(self):
+  def DumpControllerHeader(self, scalar_type):
     """Writes out a c++ header declaration which will create a Controller object.
 
     Returns:
@@ -370,10 +385,11 @@
     num_states = self.A.shape[0]
     num_inputs = self.B.shape[1]
     num_outputs = self.C.shape[0]
-    return 'StateFeedbackControllerCoefficients<%d, %d, %d> %s;\n' % (
-        num_states, num_inputs, num_outputs, self.ControllerFunction())
+    return 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
+        num_states, num_inputs, num_outputs, scalar_type,
+        self.ControllerFunction())
 
-  def DumpController(self):
+  def DumpController(self, scalar_type):
     """Returns a c++ function which will create a Controller object.
 
     Returns:
@@ -382,18 +398,19 @@
     num_states = self.A.shape[0]
     num_inputs = self.B.shape[1]
     num_outputs = self.C.shape[0]
-    ans = ['StateFeedbackControllerCoefficients<%d, %d, %d> %s {\n' % (
-        num_states, num_inputs, num_outputs, self.ControllerFunction())]
+    ans = ['StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' % (
+        num_states, num_inputs, num_outputs, scalar_type,
+        self.ControllerFunction())]
 
-    ans.append(self._DumpMatrix('K', self.K))
+    ans.append(self._DumpMatrix('K', self.K, scalar_type))
     if not hasattr(self, 'Kff'):
       self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
 
-    ans.append(self._DumpMatrix('Kff', self.Kff))
+    ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
 
-    ans.append('  return StateFeedbackControllerCoefficients<%d, %d, %d>'
+    ans.append('  return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
                '(K, Kff);\n' % (
-                   num_states, num_inputs, num_outputs))
+                   num_states, num_inputs, num_outputs, scalar_type))
     ans.append('}\n')
     return ''.join(ans)
 
@@ -406,7 +423,7 @@
     return '%s %s;\n' % (
         observer_coefficient_type, self.ObserverFunction())
 
-  def DumpObserver(self, observer_coefficient_type):
+  def DumpObserver(self, observer_coefficient_type, scalar_type):
     """Returns a c++ function which will create a Observer object.
 
     Returns:
@@ -416,12 +433,12 @@
            observer_coefficient_type, self.ObserverFunction())]
 
     if observer_coefficient_type.startswith('StateFeedbackObserver'):
-      ans.append(self._DumpMatrix('L', self.L))
+      ans.append(self._DumpMatrix('L', self.L, scalar_type))
       ans.append('  return %s(L);\n' % (observer_coefficient_type,))
     elif observer_coefficient_type.startswith('HybridKalman'):
-      ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous))
-      ans.append(self._DumpMatrix('R_continuous', self.R_continuous))
-      ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state))
+      ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous, scalar_type))
+      ans.append(self._DumpMatrix('R_continuous', self.R_continuous, scalar_type))
+      ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state, scalar_type))
       ans.append('  return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
           observer_coefficient_type,))
     else:
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 34a0fcf..b9765fc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -16,7 +16,7 @@
 #include "aos/common/macros.h"
 
 template <int number_of_states, int number_of_inputs, int number_of_outputs,
-          typename PlantType, typename ObserverType>
+          typename PlantType, typename ObserverType, typename Scalar>
 class StateFeedbackLoop;
 
 // For everything in this file, "inputs" and "outputs" are defined from the
@@ -26,7 +26,8 @@
 // controller (U is an output because that's what goes to the motors and Y is an
 // input because that's what comes back from the sensors).
 
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 struct StateFeedbackPlantCoefficients final {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -41,32 +42,33 @@
         U_max(other.U_max) {}
 
   StateFeedbackPlantCoefficients(
-      const Eigen::Matrix<double, number_of_states, number_of_states> &A,
-      const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
-      const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
-      const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
-      const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
-      const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
-      const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
+      const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A,
+      const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A_inv,
+      const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B,
+      const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
+      const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min)
       : A(A), A_inv(A_inv), B(B), C(C), D(D), U_min(U_min), U_max(U_max) {}
 
-  const Eigen::Matrix<double, number_of_states, number_of_states> A;
-  const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
-  const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
-  const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
-  const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
-  const Eigen::Matrix<double, number_of_inputs, 1> U_min;
-  const Eigen::Matrix<double, number_of_inputs, 1> U_max;
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> A;
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> A_inv;
+  const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B;
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> C;
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
 };
 
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 class StateFeedbackPlant {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
   StateFeedbackPlant(
       ::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
-          number_of_states, number_of_inputs, number_of_outputs>>>
+          number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
           *coefficients)
       : coefficients_(::std::move(*coefficients)), index_(0) {
     Reset();
@@ -81,53 +83,53 @@
 
   virtual ~StateFeedbackPlant() {}
 
-  const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A() const {
     return coefficients().A;
   }
-  double A(int i, int j) const { return A()(i, j); }
-  const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv() const {
+  Scalar A(int i, int j) const { return A()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A_inv() const {
     return coefficients().A_inv;
   }
-  double A_inv(int i, int j) const { return A_inv()(i, j); }
-  const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+  Scalar A_inv(int i, int j) const { return A_inv()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B() const {
     return coefficients().B;
   }
-  double B(int i, int j) const { return B()(i, j); }
-  const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+  Scalar B(int i, int j) const { return B()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C() const {
     return coefficients().C;
   }
-  double C(int i, int j) const { return C()(i, j); }
-  const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+  Scalar C(int i, int j) const { return C()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D() const {
     return coefficients().D;
   }
-  double D(int i, int j) const { return D()(i, j); }
-  const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+  Scalar D(int i, int j) const { return D()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min() const {
     return coefficients().U_min;
   }
-  double U_min(int i, int j) const { return U_min()(i, j); }
-  const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+  Scalar U_min(int i, int j) const { return U_min()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max() const {
     return coefficients().U_max;
   }
-  double U_max(int i, int j) const { return U_max()(i, j); }
+  Scalar U_max(int i, int j) const { return U_max()(i, j); }
 
-  const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
-  double X(int i, int j) const { return X()(i, j); }
-  const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
-  double Y(int i, int j) const { return Y()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
+  Scalar X(int i, int j) const { return X()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
+  Scalar Y(int i, int j) const { return Y()(i, j); }
 
-  Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
-  double &mutable_X(int i, int j) { return mutable_X()(i, j); }
-  Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
-  double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+  Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
+  Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+  Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
+  Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
 
   const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
-                                       number_of_outputs>
+                                       number_of_outputs, Scalar>
       &coefficients(int index) const {
     return *coefficients_[index];
   }
 
   const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
-                                       number_of_outputs>
+                                       number_of_outputs, Scalar>
       &coefficients() const {
     return *coefficients_[index_];
   }
@@ -145,16 +147,17 @@
   }
 
   // Assert that U is within the hardware range.
-  virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+  virtual void CheckU(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
     for (int i = 0; i < kNumInputs; ++i) {
-      if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+      if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
+          U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
         LOG(FATAL, "U out of range\n");
       }
     }
   }
 
   // Computes the new X and Y given the control input.
-  void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+  void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
     // Powers outside of the range are more likely controller bugs than things
     // that the plant should deal with.
     CheckU(U);
@@ -163,13 +166,13 @@
   }
 
   // Computes the new Y given the control input.
-  void UpdateY(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+  void UpdateY(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
     Y_ = C() * X() + D() * U;
   }
 
-  Eigen::Matrix<double, number_of_states, 1> Update(
-      const Eigen::Matrix<double, number_of_states, 1> X,
-      const Eigen::Matrix<double, number_of_inputs, 1> &U) const {
+  Eigen::Matrix<Scalar, number_of_states, 1> Update(
+      const Eigen::Matrix<Scalar, number_of_states, 1> X,
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) const {
     return A() * X + B() * U;
   }
 
@@ -180,11 +183,11 @@
   static const int kNumInputs = number_of_inputs;
 
  private:
-  Eigen::Matrix<double, number_of_states, 1> X_;
-  Eigen::Matrix<double, number_of_outputs, 1> Y_;
+  Eigen::Matrix<Scalar, number_of_states, 1> X_;
+  Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
 
   ::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
-      number_of_states, number_of_inputs, number_of_outputs>>>
+      number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
       coefficients_;
 
   int index_;
@@ -193,27 +196,30 @@
 };
 
 // A container for all the controller coefficients.
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 struct StateFeedbackControllerCoefficients final {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
-  const Eigen::Matrix<double, number_of_inputs, number_of_states> K;
-  const Eigen::Matrix<double, number_of_inputs, number_of_states> Kff;
+  const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> K;
+  const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> Kff;
 
   StateFeedbackControllerCoefficients(
-      const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
-      const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff)
+      const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &K,
+      const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &Kff)
       : K(K), Kff(Kff) {}
 };
 
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 class StateFeedbackController {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
   explicit StateFeedbackController(
       ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
-          number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
+          number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
+          *controllers)
       : coefficients_(::std::move(*controllers)) {}
 
   StateFeedbackController(StateFeedbackController &&other)
@@ -221,14 +227,14 @@
     ::std::swap(coefficients_, other.coefficients_);
   }
 
-  const Eigen::Matrix<double, number_of_inputs, number_of_states> &K() const {
+  const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &K() const {
     return coefficients().K;
   }
-  double K(int i, int j) const { return K()(i, j); }
-  const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff() const {
+  Scalar K(int i, int j) const { return K()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &Kff() const {
     return coefficients().Kff;
   }
-  double Kff(int i, int j) const { return Kff()(i, j); }
+  Scalar Kff(int i, int j) const { return Kff()(i, j); }
 
   void Reset() {}
 
@@ -246,13 +252,13 @@
   int index() const { return index_; }
 
   const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
-                                            number_of_outputs>
+                                            number_of_outputs, Scalar>
       &coefficients(int index) const {
     return *coefficients_[index];
   }
 
   const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
-                                            number_of_outputs>
+                                            number_of_outputs, Scalar>
       &coefficients() const {
     return *coefficients_[index_];
   }
@@ -260,30 +266,32 @@
  private:
   int index_ = 0;
   ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
-      number_of_states, number_of_inputs, number_of_outputs>>>
+      number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
       coefficients_;
 };
 
 // A container for all the observer coefficients.
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 struct StateFeedbackObserverCoefficients final {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
-  const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+  const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> L;
 
   StateFeedbackObserverCoefficients(
-      const Eigen::Matrix<double, number_of_states, number_of_outputs> &L)
+      const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &L)
       : L(L) {}
 };
 
-template <int number_of_states, int number_of_inputs, int number_of_outputs>
+template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double>
 class StateFeedbackObserver {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
   explicit StateFeedbackObserver(
       ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
-          number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+          number_of_states, number_of_inputs, number_of_outputs, Scalar>>> *observers)
       : coefficients_(::std::move(*observers)) {}
 
   StateFeedbackObserver(StateFeedbackObserver &&other)
@@ -291,32 +299,32 @@
     ::std::swap(coefficients_, other.coefficients_);
   }
 
-  const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+  const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &L() const {
     return coefficients().L;
   }
-  double L(int i, int j) const { return L()(i, j); }
+  Scalar L(int i, int j) const { return L()(i, j); }
 
-  const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+  const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
     return X_hat_;
   }
-  Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+  Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
 
   void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
-                                number_of_outputs> * /*loop*/) {
+                                number_of_outputs, Scalar> * /*loop*/) {
     X_hat_.setZero();
   }
 
   void Predict(StateFeedbackPlant<number_of_states, number_of_inputs,
-                                  number_of_outputs> *plant,
-               const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+                                  number_of_outputs, Scalar> *plant,
+               const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
                ::std::chrono::nanoseconds /*dt*/) {
     mutable_X_hat() = plant->Update(X_hat(), new_u);
   }
 
   void Correct(const StateFeedbackPlant<number_of_states, number_of_inputs,
-                                        number_of_outputs> &plant,
-               const Eigen::Matrix<double, number_of_inputs, 1> &U,
-               const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+                                        number_of_outputs, Scalar> &plant,
+               const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+               const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
     mutable_X_hat() +=
         plant.A_inv() * L() * (Y - plant.C() * X_hat() - plant.D() * U);
   }
@@ -335,32 +343,33 @@
   int index() const { return index_; }
 
   const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
-                                          number_of_outputs>
+                                          number_of_outputs, Scalar>
       &coefficients(int index) const {
     return *coefficients_[index];
   }
 
   const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
-                                          number_of_outputs>
+                                          number_of_outputs, Scalar>
       &coefficients() const {
     return *coefficients_[index_];
   }
 
  private:
   // Internal state estimate.
-  Eigen::Matrix<double, number_of_states, 1> X_hat_;
+  Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
 
   int index_ = 0;
   ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
-      number_of_states, number_of_inputs, number_of_outputs>>>
+      number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
       coefficients_;
 };
 
 template <int number_of_states, int number_of_inputs, int number_of_outputs,
+          typename Scalar = double,
           typename PlantType = StateFeedbackPlant<
-              number_of_states, number_of_inputs, number_of_outputs>,
+              number_of_states, number_of_inputs, number_of_outputs, Scalar>,
           typename ObserverType = StateFeedbackObserver<
-              number_of_states, number_of_inputs, number_of_outputs>>
+              number_of_states, number_of_inputs, number_of_outputs, Scalar>>
 class StateFeedbackLoop {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -368,7 +377,7 @@
   explicit StateFeedbackLoop(
       PlantType &&plant,
       StateFeedbackController<number_of_states, number_of_inputs,
-                              number_of_outputs> &&controller,
+                              number_of_outputs, Scalar> &&controller,
       ObserverType &&observer)
       : plant_(::std::move(plant)),
         controller_(::std::move(controller)),
@@ -389,43 +398,43 @@
 
   virtual ~StateFeedbackLoop() {}
 
-  const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+  const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
     return observer().X_hat();
   }
-  double X_hat(int i, int j) const { return X_hat()(i, j); }
-  const Eigen::Matrix<double, number_of_states, 1> &R() const { return R_; }
-  double R(int i, int j) const { return R()(i, j); }
-  const Eigen::Matrix<double, number_of_states, 1> &next_R() const {
+  Scalar X_hat(int i, int j) const { return X_hat()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_states, 1> &R() const { return R_; }
+  Scalar R(int i, int j) const { return R()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_states, 1> &next_R() const {
     return next_R_;
   }
-  double next_R(int i, int j) const { return next_R()(i, j); }
-  const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
-  double U(int i, int j) const { return U()(i, j); }
-  const Eigen::Matrix<double, number_of_inputs, 1> &U_uncapped() const {
+  Scalar next_R(int i, int j) const { return next_R()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> &U() const { return U_; }
+  Scalar U(int i, int j) const { return U()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_uncapped() const {
     return U_uncapped_;
   }
-  double U_uncapped(int i, int j) const { return U_uncapped()(i, j); }
-  const Eigen::Matrix<double, number_of_inputs, 1> &ff_U() const {
+  Scalar U_uncapped(int i, int j) const { return U_uncapped()(i, j); }
+  const Eigen::Matrix<Scalar, number_of_inputs, 1> &ff_U() const {
     return ff_U_;
   }
-  double ff_U(int i, int j) const { return ff_U()(i, j); }
+  Scalar ff_U(int i, int j) const { return ff_U()(i, j); }
 
-  Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() {
+  Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() {
     return observer_.mutable_X_hat();
   }
-  double &mutable_X_hat(int i, int j) { return mutable_X_hat()(i, j); }
-  Eigen::Matrix<double, number_of_states, 1> &mutable_R() { return R_; }
-  double &mutable_R(int i, int j) { return mutable_R()(i, j); }
-  Eigen::Matrix<double, number_of_states, 1> &mutable_next_R() {
+  Scalar &mutable_X_hat(int i, int j) { return mutable_X_hat()(i, j); }
+  Eigen::Matrix<Scalar, number_of_states, 1> &mutable_R() { return R_; }
+  Scalar &mutable_R(int i, int j) { return mutable_R()(i, j); }
+  Eigen::Matrix<Scalar, number_of_states, 1> &mutable_next_R() {
     return next_R_;
   }
-  double &mutable_next_R(int i, int j) { return mutable_next_R()(i, j); }
-  Eigen::Matrix<double, number_of_inputs, 1> &mutable_U() { return U_; }
-  double &mutable_U(int i, int j) { return mutable_U()(i, j); }
-  Eigen::Matrix<double, number_of_inputs, 1> &mutable_U_uncapped() {
+  Scalar &mutable_next_R(int i, int j) { return mutable_next_R()(i, j); }
+  Eigen::Matrix<Scalar, number_of_inputs, 1> &mutable_U() { return U_; }
+  Scalar &mutable_U(int i, int j) { return mutable_U()(i, j); }
+  Eigen::Matrix<Scalar, number_of_inputs, 1> &mutable_U_uncapped() {
     return U_uncapped_;
   }
-  double &mutable_U_uncapped(int i, int j) {
+  Scalar &mutable_U_uncapped(int i, int j) {
     return mutable_U_uncapped()(i, j);
   }
 
@@ -433,7 +442,7 @@
   PlantType *mutable_plant() { return &plant_; }
 
   const StateFeedbackController<number_of_states, number_of_inputs,
-                                number_of_outputs>
+                                number_of_outputs, Scalar>
       &controller() const {
     return controller_;
   }
@@ -465,23 +474,23 @@
   }
 
   // Corrects X_hat given the observation in Y.
-  void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+  void Correct(const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
     observer_.Correct(this->plant(), U(), Y);
   }
 
-  const Eigen::Matrix<double, number_of_states, 1> error() const {
+  const Eigen::Matrix<Scalar, number_of_states, 1> error() const {
     return R() - X_hat();
   }
 
   // Returns the calculated controller power.
-  virtual const Eigen::Matrix<double, number_of_inputs, 1> ControllerOutput() {
+  virtual const Eigen::Matrix<Scalar, number_of_inputs, 1> ControllerOutput() {
     // TODO(austin): Should this live in StateSpaceController?
     ff_U_ = FeedForward();
     return controller().K() * error() + ff_U_;
   }
 
   // Calculates the feed forwards power.
-  virtual const Eigen::Matrix<double, number_of_inputs, 1> FeedForward() {
+  virtual const Eigen::Matrix<Scalar, number_of_inputs, 1> FeedForward() {
     // TODO(austin): Should this live in StateSpaceController?
     return controller().Kff() * (next_R() - plant().A() * R());
   }
@@ -511,7 +520,7 @@
     }
   }
 
-  void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+  void UpdateObserver(const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
                       ::std::chrono::nanoseconds dt) {
     observer_.Predict(this->mutable_plant(), new_u, dt);
   }
@@ -528,7 +537,8 @@
  protected:
   PlantType plant_;
 
-  StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs>
+  StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs,
+                          Scalar>
       controller_;
 
   ObserverType observer_;
@@ -539,17 +549,17 @@
   static constexpr int kNumInputs = number_of_inputs;
 
   // Portion of U which is based on the feed-forwards.
-  Eigen::Matrix<double, number_of_inputs, 1> ff_U_;
+  Eigen::Matrix<Scalar, number_of_inputs, 1> ff_U_;
 
  private:
   // Current goal (Used by the feed-back controller).
-  Eigen::Matrix<double, number_of_states, 1> R_;
+  Eigen::Matrix<Scalar, number_of_states, 1> R_;
   // Goal to go to in the next cycle (Used by Feed-Forward controller.)
-  Eigen::Matrix<double, number_of_states, 1> next_R_;
+  Eigen::Matrix<Scalar, number_of_states, 1> next_R_;
   // Computed output after being capped.
-  Eigen::Matrix<double, number_of_inputs, 1> U_;
+  Eigen::Matrix<Scalar, number_of_inputs, 1> U_;
   // Computed output before being capped.
-  Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
+  Eigen::Matrix<Scalar, number_of_inputs, 1> U_uncapped_;
 
   DISALLOW_COPY_AND_ASSIGN(StateFeedbackLoop);
 };