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

EKF: Enable fallback to baro alt when using range finder for height

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-13 21:17:51 +11:00
parent 370f643f42
commit c58ab3e256
5 changed files with 52 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+2 -2
View File
@@ -110,7 +110,7 @@ void Ekf::fuseVelPosHeight()
if (_control_status.flags.baro_hgt) {
fuse_map[5] = true;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt);
_vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt + _baro_hgt_offset);
// observation variance - user parameter defined
R[5] = fmaxf(_params.baro_noise, 0.01f);
R[5] = R[5] * R[5];
@@ -120,7 +120,7 @@ void Ekf::fuseVelPosHeight()
} else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) {
fuse_map[5] = true;
// use range finder with tilt correction
_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng *_R_prev(2, 2),
_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_prev(2, 2),
_params.rng_gnd_clearance));
// observation variance - user parameter defined
R[5] = fmaxf(_params.range_noise, 0.01f);