From d501d8e1d4ec2a7ee28d6fb8c9576d060c28917f Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 23 Feb 2024 16:24:54 +0100 Subject: [PATCH] ekf2: use Joseph stabilized update in direct state observations --- .../EKF/aid_sources/ZeroVelocityUpdate.cpp | 2 +- src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF/ekf_helper.cpp | 46 +++++++++++-------- src/modules/ekf2/EKF/position_fusion.cpp | 6 +-- src/modules/ekf2/EKF/velocity_fusion.cpp | 10 ++-- 5 files changed, 36 insertions(+), 30 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp index 4745f759c7..48d07d583d 100644 --- a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp @@ -66,7 +66,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye for (unsigned i = 0; i < 3; i++) { const float innovation = ekf.state().vel(i) - vel_obs(i); - ekf.fuseDirectStateMeasurement(innovation, innov_var(i), State::vel.idx + i); + ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i); } _time_last_zero_velocity_fuse = imu_delayed.time_us; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f9a73bbd86..e980626236 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -328,7 +328,7 @@ public: } // fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc) - bool fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index); + bool fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index); // gyro bias const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 1f888cd73f..3a030cf632 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -839,38 +839,44 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) } } -bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const int state_index) +bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index) { - VectorState Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. + VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. // calculate kalman gain K = PHS, where S = 1/innovation variance for (int row = 0; row < State::size; row++) { - Kfusion(row) = P(row, state_index) / innov_var; + K(row) = P(row, state_index) / innov_var; } - clearInhibitedStateKalmanGains(Kfusion); + clearInhibitedStateKalmanGains(K); - SquareMatrixState KHP; + // 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" - for (unsigned row = 0; row < State::size; row++) { - for (unsigned column = 0; column < State::size; column++) { - KHP(row, column) = Kfusion(row) * P(state_index, column); + // Step 1: conventional update + VectorState PH = P.row(state_index); + + for (unsigned i = 0; i < State::size; i++) { + for (unsigned j = 0; j <= i; j++) { + P(i, j) = P(i, j) - K(i) * PH(j); + P(j, i) = P(i, j); } } - const bool healthy = checkAndFixCovarianceUpdate(KHP); + // Step 2: stabilized update + PH = P.row(state_index); - if (healthy) { - // apply the covariance corrections - P -= KHP; - - constrainStateVariances(); - - // apply the state corrections - fuse(Kfusion, innov); - - return true; + for (unsigned i = 0; i < State::size; i++) { + for (unsigned j = 0; j <= i; j++) { + float s = .5f * (P(i, j) - PH(i) * K(j) + P(i, j) - PH(j) * K(i)); + P(i, j) = s + K(i) * R * K(j); + P(j, i) = P(i, j); + } } - return false; + constrainStateVariances(); + + // apply the state corrections + fuse(K, innov); + return true; } diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index a645196633..a1d2f85c27 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -80,8 +80,8 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::pos.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::pos.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -97,7 +97,7 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2) + && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, State::pos.idx + 2) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp index f44ecc874f..fbb079c1fd 100644 --- a/src/modules/ekf2/EKF/velocity_fusion.cpp +++ b/src/modules/ekf2/EKF/velocity_fusion.cpp @@ -83,8 +83,8 @@ void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) { // vx, vy if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -100,9 +100,9 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { // vx, vy, vz if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) - && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2) + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], State::vel.idx + 2) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us;