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

EKF: Always reset covariance matrix terms when doing vel pos state reset

Browse Source 
If state errors were large before the reset, then failure to reset the covariance matrix terms can result in incorrect fusion of position and velocity measurements after the reset due to inconsistencies in the covariance matrix.
This commit is contained in:
Paul Riseborough
2017-11-13 07:05:56 +11:00
parent 141264fe63
commit 279fc836f7
1 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+11
View File
@@ -56,8 +56,12 @@ bool Ekf::resetVelocity()
// this reset is only called if we have new gps data at the fusion time horizon
_state.vel = _gps_sample_delayed.vel;
// use GPS accuracy to reset variances
setDiag(P,4,6,sq(_gps_sample_delayed.sacc));
} else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) {
_state.vel.setZero();
zeroOffDiag(P,4,6);
} else {
return false;
@@ -106,15 +110,22 @@ bool Ekf::resetPosition()
_state.pos(0) = _gps_sample_delayed.pos(0);
_state.pos(1) = _gps_sample_delayed.pos(1);
// use GPS accuracy to reset variances
setDiag(P,7,8,sq(_gps_sample_delayed.hacc));
} else if (_control_status.flags.opt_flow) {
_state.pos(0) = 0.0f;
_state.pos(1) = 0.0f;
zeroOffDiag(P,7,8);
} else if (_control_status.flags.ev_pos) {
// this reset is only called if we have new ev data at the fusion time horizon
_state.pos(0) = _ev_sample_delayed.posNED(0);
_state.pos(1) = _ev_sample_delayed.posNED(1);
// use EV accuracy to reset variances
setDiag(P,7,8,sq(_ev_sample_delayed.posErr));
} else {
return false;
}