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

EKF: Add arbitrary pitch offset for range sensor

Browse Source
This commit is contained in:
Paul Riseborough
2017-02-22 14:12:05 +01:00
committed by Roman
parent d9688fae7d
commit cf9c8de167
6 changed files with 23 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+2 -2
View File
@@ -155,10 +155,10 @@ void Ekf::fuseVelPosHeight()
// innovation gate size
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
} else if (_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f)) {
} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_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_to_earth(2, 2),
_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
_params.rng_gnd_clearance));
// observation variance - user parameter defined
R[5] = fmaxf(_params.range_noise, 0.01f);