fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 07:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: add baro bias estimator using GNSS altitude

Browse Source
This commit is contained in:
bresch
2021-07-19 14:36:40 +02:00
committed by Daniel Agar
parent 10f4fc7783
commit 54c7e74de3
13 changed files with 337 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+3 -1
View File
@@ -1161,6 +1161,7 @@ void Ekf::controlHeightFusion()
break;
}
updateBaroHgtBias();
updateBaroHgtOffset();
if (_control_status.flags.rng_hgt
@@ -1184,7 +1185,8 @@ void Ekf::controlHeightFusion()
Vector3f baro_hgt_obs_var;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset;
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
// observation variance - user parameter defined
baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
// innovation gate size