diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 3cb1c76080..14255f6d71 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -194,6 +194,10 @@ void Ekf::resetHaglRng() void Ekf::stopHaglRngFusion() { _hagl_sensor_status.flags.range_finder = false; + _hagl_innov = 0.f; + _hagl_innov_var = 0.f; + _hagl_test_ratio = 0.f; + _innov_check_fail_status.flags.reject_hagl = false; } void Ekf::fuseHaglRng() @@ -290,6 +294,10 @@ void Ekf::startHaglFlowFusion() void Ekf::stopHaglFlowFusion() { _hagl_sensor_status.flags.flow = false; + _hagl_innov = 0.f; + _hagl_innov_var = 0.f; + _hagl_test_ratio = 0.f; + _innov_check_fail_status.flags.reject_hagl = false; } void Ekf::resetHaglFlow()