fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 12:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Allow use of large GPS position observation noise when using other aiding sources (#403)

Browse Source
This commit is contained in:
Paul Riseborough
2018-03-27 11:31:37 +11:00
committed by GitHub
parent 8a012436f9
commit 1bb4c17c0e
1 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+10 -2
View File
@@ -523,8 +523,16 @@ void Ekf::controlGpsFusion()
// calculate observation process noise
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) {
// if we are using other sources of aiding, then relax the upper observation
// noise limit which prevents bad GPS perturbing the position estimate
_posObsNoiseNE = fmaxf(_gps_sample_delayed.hacc, lower_limit);
} else {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
}
_velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
// calculate innovations