Store Q/R in StateFeedbackObserverCoefficients
I want R available to get noise values for use in the EKF. I pull in Q
because I may want it and it doesn't really cost us anything.
Change-Id: I7eae2669333cf8eac99a05d10b1d2cefa305cc2f
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 42433e9..38dbd53 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -150,7 +150,9 @@
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
v_observer;
v_observer.emplace_back(new StateFeedbackObserverCoefficients<2, 4, 7>(
- Eigen::Matrix<double, 2, 7>::Identity()));
+ 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);
StateFeedbackLoop<2, 4, 7> test_loop(
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 9aa00df..e3e00b7 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -453,10 +453,17 @@
if observer_coefficient_type.startswith('StateFeedbackObserver'):
if hasattr(self, 'KalmanGain'):
KalmanGain = self.KalmanGain
+ Q = self.Q
+ R = self.R
else:
KalmanGain = numpy.linalg.inv(self.A) * self.L
+ Q = numpy.zeros(self.A.shape)
+ R = numpy.zeros((self.C.shape[0], self.C.shape[0]))
ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
- ans.append(' return %s(KalmanGain);\n' % (observer_coefficient_type,))
+ ans.append(self._DumpMatrix('Q', Q, scalar_type))
+ ans.append(self._DumpMatrix('R', R, scalar_type))
+ ans.append(' return %s(KalmanGain, Q, R);\n'
+ % (observer_coefficient_type,))
elif observer_coefficient_type.startswith('HybridKalman'):
ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous, scalar_type))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 3f9123f..c59cbab 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -362,10 +362,15 @@
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ # If we don't have an IMU, pad various matrices with zeros so that
+ # we can still have 4 measurement outputs.
if not self.has_imu:
self.KalmanGain = numpy.hstack((self.KalmanGain, numpy.matrix(numpy.zeros((7, 1)))))
self.C = numpy.vstack((self.C, numpy.matrix(numpy.zeros((1, 7)))))
self.D = numpy.vstack((self.D, numpy.matrix(numpy.zeros((1, 2)))))
+ Rtmp = numpy.zeros((4, 4))
+ Rtmp[0:3, 0:3] = self.R
+ self.R = Rtmp
self.L = self.A * self.KalmanGain
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 2511d98..fc3ff34 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -275,10 +275,15 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> Q;
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R;
StateFeedbackObserverCoefficients(
- const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &KalmanGain)
- : KalmanGain(KalmanGain) {}
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &
+ KalmanGain,
+ const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R)
+ : KalmanGain(KalmanGain), Q(Q), R(R) {}
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,