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

EKF: Fix vel pos innovation logging bug

This commit is contained in:
Paul Riseborough 2017-11-24 14:25:44 +11:00
parent 882f4d44e5
commit 89be63d6c2
2 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/control.cpp
View File

@ -530,6 +530,7 @@ void Ekf::controlGpsFusion()
// calculate innovations
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);

12
EKF/vel_pos_fusion.cpp
View File

@ -74,8 +74,6 @@ void Ekf::fuseVelPosHeight()
if (_fuse_vert_vel) {
fuse_map[2] = true;
// vertical velocity innovation
innovation[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
@ -111,11 +109,11 @@ void Ekf::fuseVelPosHeight()
float deadzone_start = 0.25f * _params.baro_noise;
float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (_control_status.flags.gnd_effect) {
if (_vel_pos_innov[5] < -deadzone_start) {
if (_vel_pos_innov[5] <= -deadzone_end) {
_vel_pos_innov[5] += deadzone_end;
if (innovation[5] < -deadzone_start) {
if (innovation[5] <= -deadzone_end) {
innovation[5] += deadzone_end;
} else {
_vel_pos_innov[5] = -deadzone_start;
innovation[5] = -deadzone_start;
}
}
}
@ -153,6 +151,8 @@ void Ekf::fuseVelPosHeight()
gate_size[5] = fmaxf(_params.ev_innov_gate, 1.0f);
}
// update innovation class variable for logging purposes
_vel_pos_innov[5] = innovation[5];
}
// calculate innovation test ratios