From 6d819343aad926d2a156b6e028c93dc79d5cba96 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 Mar 2024 11:02:08 +0100 Subject: [PATCH] ekf2: fix direct state measurement update for suboptimal K case The duration of a unit test had to be increased because the incorrect covariance matrix update, was making the unit test passing faster (over-optimistic variance). --- src/modules/ekf2/EKF/ekf.h | 9 +++++++-- src/modules/ekf2/EKF/ekf_helper.cpp | 10 ++++++++-- src/modules/ekf2/test/test_EKF_initialization.cpp | 2 +- 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index edffd17974..bc087558e9 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -485,9 +485,14 @@ public: #else // Efficient implementation of the Joseph stabilized covariance update // Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006" + // P = (I - K * H) * P * (I - K * H).T + K * R * K.T + // = P_temp * (I - H.T * K.T) + K * R * K.T + // = P_temp - P_temp * H.T * K.T + K * R * K.T // Step 1: conventional update - VectorState PH = P * H; + // Compute P_temp and store it in P to avoid allocating more memory + // P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major + VectorState PH = P * H; // H is stored as a column vector. H is in fact H.T for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j < State::size; j++) { @@ -496,7 +501,7 @@ public: } // Step 2: stabilized update - PH = P * H; + PH = P * H; // H is stored as a column vector. H is in fact H.T for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j <= i; j++) { diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 032d35fb31..bb9e901f57 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -865,18 +865,24 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c #else // Efficient implementation of the Joseph stabilized covariance update // Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006" + // P = (I - K * H) * P * (I - K * H).T + K * R * K.T + // = P_temp * (I - H.T * K.T) + K * R * K.T + // = P_temp - P_temp * H.T * K.T + K * R * K.T // Step 1: conventional update + // Compute P_temp and store it in P to avoid allocating more memory + // P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major VectorState PH = P.row(state_index); for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j < State::size; j++) { - P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed) + P(i, j) -= K(i) * PH(j); // P is now not symmetric if K is not optimal (e.g.: some gains have been zeroed) } } // Step 2: stabilized update - PH = P.row(state_index); + // P (or "P_temp") is not symmetric so we must take the column + PH = P.col(state_index); for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j <= i; j++) { diff --git a/src/modules/ekf2/test/test_EKF_initialization.cpp b/src/modules/ekf2/test/test_EKF_initialization.cpp index eed2108641..142a4ea3ef 100644 --- a/src/modules/ekf2/test/test_EKF_initialization.cpp +++ b/src/modules/ekf2/test/test_EKF_initialization.cpp @@ -346,7 +346,7 @@ TEST_F(EkfInitializationTest, initializeWithTiltNotAtRest) _ekf->set_vehicle_at_rest(false); _sensor_simulator.simulateOrientation(quat_sim); //_sensor_simulator.runSeconds(_init_tilt_period); - _sensor_simulator.runSeconds(7); + _sensor_simulator.runSeconds(10); EXPECT_TRUE(_ekf->control_status_flags().tilt_align);