diff --git a/EKF/common.h b/EKF/common.h index 8a8dea0436..d0090cc423 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -130,6 +130,7 @@ struct parameters { float gps_vel_noise = 5.0e-1f; // observation noise for gps velocity fusion float gps_pos_noise = 1.0f; // observation noise for gps position fusion + float pos_noaid_noise = 10.0f; // observation noise for non-aiding position fusion float baro_noise = 3.0f; // observation noise for barometric height fusion float baro_innov_gate = 3.0f; // barometric height innovation consistency gate size in standard deviations float posNE_innov_gate = 3.0f; // GPS horizontal position innovation consistency gate size in standard deviations diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index ac378fe730..aef41ee149 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -84,7 +84,12 @@ void Ekf::fuseVelPosHeight() _vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); // observation variance - user parameter defined - R[3] = fmaxf(_params.gps_pos_noise, 0.01f); + // if we are in flight and not using GPS, then use a specific parameter + if (!_control_status.flags.gps && _control_status.flags.in_air) { + R[3] = fmaxf(_params.pos_noaid_noise, 0.5f); + } else { + R[3] = fmaxf(_params.gps_pos_noise, 0.01f); + } R[3] = R[3] * R[3]; R[4] = R[3]; // innovation gate sizes