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

EKF: Fix bug in calculation of terrain observation variance

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-14 21:17:11 +10:00
parent 7afd015b49
commit 172f4be594
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+1 -1
View File
@@ -99,7 +99,7 @@ void Ekf::fuseHagl()
_hagl_innov = pred_hagl - meas_hagl;
// calculate the observation variance adding the variance of the vehicles own height uncertainty and factoring in the effect of tilt on measurement error
float obs_variance = fmaxf(P[8][8], 0.0f) + sq(_params.range_noise / _R_to_earth(2, 2));
float obs_variance = fmaxf(P[9][9], 0.0f) + sq(_params.range_noise / _R_to_earth(2, 2));
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);