diff --git a/EKF/control.cpp b/EKF/control.cpp index 9148d0cf84..aaf48dbb87 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -300,7 +300,7 @@ void Ekf::controlExternalVisionFusion() // innovation gate size ev_pos_innov_gates(0) = fmaxf(_params.ev_pos_innov_gate, 1.0f); - fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio, false); + fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio); } // determine if we should use the velocity observations @@ -712,7 +712,7 @@ void Ekf::controlGpsFusion() // fuse GPS measurement fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates,gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); - fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio, false); + fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); } } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { diff --git a/EKF/ekf.h b/EKF/ekf.h index 9b1532e493..35136f5433 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -622,7 +622,7 @@ private: Vector3f &innov_var, Vector2f &test_ratio); bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, - Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate); + Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false); bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio);