dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Fix bug in use of gps velocity noise parameter (#401) 

This fixes a bug introduced by an earlier feature request PR. The parameter is supposed to define the lower limit on the observation noise.
This commit is contained in:
Paul Riseborough 2018-02-28 11:48:53 +11:00 committed by GitHub
parent cdc6efc5d6
commit 78e983073a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/control.cpp
View File

@ -525,7 +525,7 @@ void Ekf::controlGpsFusion()
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);
_velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_params.gps_vel_noise, 0.01f));
_velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
// calculate innovations
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);