dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Use consistent position noise values during alignment

This commit is contained in:
Paul Riseborough 2016-05-18 20:11:08 +10:00
parent b9a3712ccb
commit e272d5f003
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/vel_pos_fusion.cpp
View File

@ -88,11 +88,10 @@ void Ekf::fuseVelPosHeight()
// being used
if (!_control_status.flags.gps && !_control_status.flags.ev_pos) {
// No observations - use a static position to constrain drift
if (_control_status.flags.in_air) {
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
} else {
R[3] = _params.gps_pos_noise;
R[3] = 0.5f;
}
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);