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

EKF: Scale position observation noise with GPS quality

Browse Source 
This allows the filter to adapt to variations in GPs quality.
The range of adjustment in observation noise is limited to the range between a lower limit set by the GPS observation noise parameter and an upper limit set by the no-aiding observation noise.
This commit is contained in:
Paul Riseborough
2016-02-25 10:16:32 +11:00
parent 5483e7a477
commit a30830a7a9
3 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+5 -1
View File
@@ -40,6 +40,7 @@
*/
#include "ekf.h"
#include "mathlib.h"
void Ekf::fuseVelPosHeight()
{
@@ -90,7 +91,10 @@ void Ekf::fuseVelPosHeight()
R[3] = fmaxf(_params.pos_noaid_noise, 0.5f);
} else {
R[3] = fmaxf(_params.gps_pos_noise, 0.01f);
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
R[3] = math::constrain(_gps_hpos_accuracy, lower_limit, upper_limit);
}
R[3] = R[3] * R[3];