diff --git a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp index 4ecb5b5325..d9635c8e4a 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp @@ -63,7 +63,13 @@ void Ekf::controlFakeHgtFusion() // always protect against extreme values that could result in a NaN if (aid_src.test_ratio < sq(100.0f / innov_gate)) { - fuseVerticalPosition(aid_src); + if (!aid_src.innovation_rejected + && 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; + } } const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);