diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 7680688..3fa138c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -41,7 +41,7 @@
     coefs.emplace_back(new StateFeedbackPlantCoefficients<4, 2, 2, double>(
         dt_config.make_drivetrain_loop().plant().coefficients(ii)));
   }
-  return StateFeedbackPlant<4, 2, 2, double>(&coefs);
+  return StateFeedbackPlant<4, 2, 2, double>(std::move(coefs));
 }
 
 }  // namespace
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 6285024..23a3d21 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -63,8 +63,8 @@
   StateFeedbackHybridPlant(
       ::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
           number_of_states, number_of_inputs, number_of_outputs>>>
-          *coefficients)
-      : coefficients_(::std::move(*coefficients)), index_(0) {
+          &&coefficients)
+      : coefficients_(::std::move(coefficients)), index_(0) {
     Reset();
   }
 
@@ -106,14 +106,14 @@
   Scalar U_max(int i, int j) const { return U_max()(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); }
+  Scalar X(int i) const { return X()(i); }
   const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
-  Scalar Y(int i, int j) const { return Y()(i, j); }
+  Scalar Y(int i) const { return Y()(i); }
 
   Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
-  Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+  Scalar &mutable_X(int i) { return mutable_X()(i); }
   Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
-  Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+  Scalar &mutable_Y(int i) { return mutable_Y()(i); }
 
   const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
                                              number_of_outputs, Scalar>
@@ -247,8 +247,12 @@
   explicit HybridKalman(
       ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
           number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
-          *observers)
-      : coefficients_(::std::move(*observers)) {}
+          &&observers)
+      : coefficients_(::std::move(observers)) {
+    R_.setZero();
+    X_hat_.setZero();
+    P_ = coefficients().P_steady_state;
+  }
 
   HybridKalman(HybridKalman &&other) : index_(other.index_) {
     ::std::swap(coefficients_, other.coefficients_);
@@ -281,22 +285,37 @@
     return X_hat_;
   }
   Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+  Scalar &mutable_X_hat(int i) { return mutable_X_hat()(i); }
+  Scalar X_hat(int i) const { return X_hat_(i); }
 
   void Reset(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
                                       number_of_outputs> *plant) {
     X_hat_.setZero();
     P_ = coefficients().P_steady_state;
-    UpdateQR(plant, ::std::chrono::milliseconds(5));
+    UpdateQR(plant, coefficients().Q_continuous, coefficients().R_continuous,
+             ::std::chrono::milliseconds(5));
   }
 
   void Predict(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
                                         number_of_outputs, Scalar> *plant,
                const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
                ::std::chrono::nanoseconds dt) {
+    Predict(plant, new_u, dt, coefficients().Q_continuous,
+            coefficients().R_continuous);
+  }
+
+  void Predict(
+      StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+                               number_of_outputs, Scalar> *plant,
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
+      ::std::chrono::nanoseconds dt,
+      Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous,
+      Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
+          R_continuous) {
     // Trigger the predict step.  This will update A() and B() in the plant.
     mutable_X_hat() = plant->Update(X_hat(), new_u, dt);
 
-    UpdateQR(plant, dt);
+    UpdateQR(plant, Q_continuous, R_continuous, dt);
     P_ = plant->A() * P_ * plant->A().transpose() + Q_;
   }
 
@@ -305,18 +324,29 @@
                                      number_of_outputs, Scalar> &plant,
       const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
       const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
+    DynamicCorrect(plant.C(), plant.D(), U, Y, R_);
+  }
+
+  // Corrects based on the sensor information available.
+  template <int number_of_measurements>
+  void DynamicCorrect(
+      const Eigen::Matrix<Scalar, number_of_measurements, number_of_states> &C,
+      const Eigen::Matrix<Scalar, number_of_measurements, number_of_inputs> &D,
+      const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+      const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y,
+      const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R) {
     Eigen::Matrix<Scalar, number_of_outputs, 1> Y_bar =
-        Y - (plant.C() * X_hat_ + plant.D() * U);
+        Y - (C * X_hat_ + D * U);
     Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> S =
-        plant.C() * P_ * plant.C().transpose() + R_;
+        C * P_ * C.transpose() + R;
     Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
-    KalmanGain =
-        (S.transpose().ldlt().solve((P() * plant.C().transpose()).transpose()))
-            .transpose();
+    KalmanGain = (S.transpose().ldlt().solve((P() * C.transpose()).transpose()))
+                     .transpose();
     X_hat_ = X_hat_ + KalmanGain * Y_bar;
-    P_ = (plant.coefficients().A_continuous.Identity() -
-          KalmanGain * plant.C()) *
-         P();
+    P_ =
+        (Eigen::Matrix<Scalar, number_of_states, number_of_states>::Identity() -
+         KalmanGain * C) *
+        P();
   }
 
   // Sets the current controller to be index, clamped to be within range.
@@ -345,17 +375,17 @@
   }
 
  private:
-  void UpdateQR(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
-                                         number_of_outputs> *plant,
-                ::std::chrono::nanoseconds dt) {
-    ::frc971::controls::DiscretizeQ(coefficients().Q_continuous,
-                                    plant->coefficients().A_continuous, dt,
-                                    &Q_);
+  void UpdateQR(
+      StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+                               number_of_outputs> *plant,
+      Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous,
+      Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_continuous,
+      ::std::chrono::nanoseconds dt) {
+    frc971::controls::DiscretizeQ(Q_continuous,
+                                  plant->coefficients().A_continuous, dt, &Q_);
 
     Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> Rtemp =
-        (coefficients().R_continuous +
-         coefficients().R_continuous.transpose()) /
-        static_cast<Scalar>(2.0);
+        (R_continuous + R_continuous.transpose()) / static_cast<Scalar>(2.0);
 
     R_ = Rtemp / ::aos::time::TypedDurationInSeconds<Scalar>(dt);
   }
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 13f74fa..c5b0771 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -84,7 +84,7 @@
   plants[0] = ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>(
       new StateFeedbackHybridPlantCoefficients<3, 1, 1>(
           MakeIntegralShooterPlantCoefficients()));
-  return StateFeedbackHybridPlant<3, 1, 1>(&plants);
+  return StateFeedbackHybridPlant<3, 1, 1>(std::move(plants));
 }
 
 StateFeedbackController<3, 1, 1> MakeIntegralShooterController() {
@@ -94,7 +94,7 @@
       ::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>(
           new StateFeedbackControllerCoefficients<3, 1, 1>(
               MakeIntegralShooterControllerCoefficients()));
-  return StateFeedbackController<3, 1, 1>(&controllers);
+  return StateFeedbackController<3, 1, 1>(std::move(controllers));
 }
 
 HybridKalman<3, 1, 1> MakeIntegralShooterObserver() {
@@ -103,7 +103,7 @@
   observers[0] = ::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>(
       new HybridKalmanCoefficients<3, 1, 1>(
           MakeIntegralShooterObserverCoefficients()));
-  return HybridKalman<3, 1, 1>(&observers);
+  return HybridKalman<3, 1, 1>(std::move(observers));
 }
 
 StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
@@ -134,7 +134,7 @@
       v_plant;
   v_plant.emplace_back(
       new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
-  StateFeedbackPlant<2, 4, 7> plant(&v_plant);
+  StateFeedbackPlant<2, 4, 7> plant(std::move(v_plant));
   plant.Update(Eigen::Matrix<double, 4, 1>::Zero());
   plant.Reset();
   plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
@@ -145,7 +145,7 @@
   v_controller.emplace_back(new StateFeedbackControllerCoefficients<2, 4, 7>(
       Eigen::Matrix<double, 4, 2>::Identity(),
       Eigen::Matrix<double, 4, 2>::Identity()));
-  StateFeedbackController<2, 4, 7> controller(&v_controller);
+  StateFeedbackController<2, 4, 7> controller(std::move(v_controller));
 
   ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
       v_observer;
@@ -153,7 +153,7 @@
       Eigen::Matrix<double, 2, 7>::Identity(),
       Eigen::Matrix<double, 2, 2>::Identity(),
       Eigen::Matrix<double, 7, 7>::Identity()));
-  StateFeedbackObserver<2, 4, 7> observer(&v_observer);
+  StateFeedbackObserver<2, 4, 7> observer(std::move(v_observer));
 
   StateFeedbackLoop<2, 4, 7> test_loop(
       ::std::move(plant), ::std::move(controller), ::std::move(observer));
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 74141c1..cea1fdd 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -216,7 +216,7 @@
                 fd.write('  plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
                          % (index, self._PlantCoeffType(),
                             self._PlantCoeffType(), loop.PlantFunction()))
-            fd.write('  return %s(&plants);\n' % self._PlantType())
+            fd.write('  return %s(std::move(plants));\n' % self._PlantType())
             fd.write('}\n\n')
 
             fd.write('%s Make%sController() {\n' % (self._ControllerType(),
@@ -229,7 +229,8 @@
                     '  controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
                     % (index, self._ControllerCoeffType(),
                        self._ControllerCoeffType(), loop.ControllerFunction()))
-            fd.write('  return %s(&controllers);\n' % self._ControllerType())
+            fd.write('  return %s(std::move(controllers));\n' %
+                     self._ControllerType())
             fd.write('}\n\n')
 
             fd.write('%s Make%sObserver() {\n' % (self._ObserverType(),
@@ -241,7 +242,8 @@
                     '  observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
                     % (index, self._ObserverCoeffType(),
                        self._ObserverCoeffType(), loop.ObserverFunction()))
-            fd.write('  return %s(&observers);\n' % self._ObserverType())
+            fd.write(
+                '  return %s(std::move(observers));\n' % self._ObserverType())
             fd.write('}\n\n')
 
             fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 4250df2..f591847 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -69,8 +69,8 @@
   StateFeedbackPlant(
       ::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
           number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
-          *coefficients)
-      : coefficients_(::std::move(*coefficients)), index_(0) {
+          &&coefficients)
+      : coefficients_(::std::move(coefficients)), index_(0) {
     Reset();
   }
 
@@ -222,8 +222,8 @@
   explicit StateFeedbackController(
       ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
           number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
-          *controllers)
-      : coefficients_(::std::move(*controllers)) {}
+          &&controllers)
+      : coefficients_(::std::move(controllers)) {}
 
   StateFeedbackController(StateFeedbackController &&other)
       : index_(other.index_) {
@@ -300,8 +300,8 @@
   explicit StateFeedbackObserver(
       ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
           number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
-          *observers)
-      : coefficients_(::std::move(*observers)) {}
+          &&observers)
+      : coefficients_(::std::move(observers)) {}
 
   StateFeedbackObserver(StateFeedbackObserver &&other)
       : X_hat_(other.X_hat_), index_(other.index_) {