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

Clean get*innov* interface

Browse Source
This commit is contained in:
kamilritz
2019-12-05 20:09:34 +11:00
committed by Paul Riseborough
parent 86b9079bdc
commit d5dc6bb8ea
5 changed files with 242 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/terrain_estimator.cpp
+2 -11
View File
@@ -153,9 +153,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);
_terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
if (_terr_test_ratio <= 1.0f) {
if (_hagl_test_ratio <= 1.0f) {
// calculate the Kalman gain
float gain = _terrain_var / _hagl_innov_var;
// correct the state
@@ -308,13 +308,4 @@ void Ekf::getTerrainVertPos(float *ret)
memcpy(ret, &_terrain_vpos, sizeof(float));
}
void Ekf::getHaglInnov(float *hagl_innov)
{
memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov));
}
void Ekf::getHaglInnovVar(float *hagl_innov_var)
{
memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var));
}