From a0ab5cf0d7311322e2ba34d41307cd3a97b90fb7 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 28 Feb 2017 10:44:55 +0100 Subject: [PATCH] ekf vel_pos_fusion: added height sensor offset to range innovation calculation Signed-off-by: Roman --- EKF/vel_pos_fusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 7a20ff4caa..93981fc023 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -159,7 +159,7 @@ void Ekf::fuseVelPosHeight() fuse_map[5] = true; // use range finder with tilt correction _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, - _params.rng_gnd_clearance)); + _params.rng_gnd_clearance)) - _hgt_sensor_offset; // observation variance - user parameter defined R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); // innovation gate size