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/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,