From 9bfc11b6603d4f7688d815a993b79924f2d3d5c6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 3 Feb 2016 18:40:32 +1100 Subject: [PATCH] EKF: Use specific position observation noise when flying without GPS A larger position uncertainty is required when flying without GPS to reduce tilt attitude estimation errors caused by vehicle manoeuvring. This needs to be tuneable to allow optimisation for different use cases (e.g. outdoor vs indoor). --- EKF/common.h | 1 + EKF/vel_pos_fusion.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) 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