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

EKF: Correct for sensor noise and baro offset during alignment

Browse Source
This commit is contained in:
Paul Riseborough
2016-02-10 10:25:27 +11:00
parent a6515543da
commit 40e174b81c
5 changed files with 81 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+1 -1
View File
@@ -100,7 +100,7 @@ void Ekf::fuseVelPosHeight()
if (_fuse_height) {
fuse_map[5] = true;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt);
_vel_pos_innov[5] = _state.pos(2) - (_baro_at_alignment -_baro_sample_delayed.hgt);
// observation variance - user parameter defined
R[5] = fmaxf(_params.baro_noise, 0.01f);
R[5] = R[5] * R[5];