diff --git a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp index c88f7f7542..3a5e5447bb 100644 --- a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -81,6 +81,8 @@ void Ekf::controlGravityFusion(const imuSample &imu) 0.25f); // innovation gate // update the states and covariance using sequential fusion + bool fused[3] {}; + for (uint8_t index = 0; index <= 2; index++) { // Calculate Kalman gains and observation jacobians if (index == 0) { @@ -108,10 +110,16 @@ void Ekf::controlGravityFusion(const imuSample &imu) const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { - measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]); + fused[index] = measurementUpdate(K, H, + _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]); } } - _aid_src_gravity.fused = true; - _aid_src_gravity.time_last_fuse = imu.time_us; + if (fused[0] && fused[1] && fused[2]) { + _aid_src_gravity.fused = true; + _aid_src_gravity.time_last_fuse = imu.time_us; + + } else { + _aid_src_gravity.fused = false; + } }