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

EKF: Publish innovation test ratios

Browse Source
This commit is contained in:
Paul Riseborough
2016-10-06 09:02:29 +02:00
committed by Lorenz Meier
parent 5ab90048e1
commit 744b79c1b2
5 changed files with 43 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+2 -2
View File
@@ -106,9 +106,9 @@ void Ekf::fuseHagl()
// perform an innovation consistency check and only fuse data if it passes
float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
float test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
_terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
if (test_ratio <= 1.0f) {
if (_terr_test_ratio <= 1.0f) {
// calculate the Kalman gain
float gain = _terrain_var / _hagl_innov_var;
// correct the state