Fixed hard coded value in EKF.

rosflight · Jul 12, 2024 · 99def8f · 99def8f
1 parent f2761da
commit 99def8f
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/rosplane/src/estimator_ekf.cpp b/rosplane/src/estimator_ekf.cpp
@@ -81,7 +81,7 @@ std::tuple<Eigen::MatrixXf, Eigen::VectorXf> EstimatorEKF::propagate_model(Eigen
 
 std::tuple<Eigen::MatrixXf, Eigen::VectorXf> EstimatorEKF::single_measurement_update(float measurement, float measurement_prediction, float measurement_variance, Eigen::VectorXf measurement_jacobian, Eigen::VectorXf x, Eigen::MatrixXf P)
 {
-  Eigen::MatrixXf I(7,7);
+  Eigen::MatrixXf I(x.size(),x.size());
   I = Eigen::MatrixXf::Identity(x.size(), x.size());
   Eigen::VectorXf L = (P * measurement_jacobian) / (measurement_variance + (measurement_jacobian.transpose() * P * measurement_jacobian));
   P = (I - L * measurement_jacobian.transpose()) * P;