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

Merge pull request #1259 from PX4/ekf_fix 

EKF filter fix
This commit is contained in:
Lorenz Meier 2014-08-08 08:46:55 +02:00
parent 95fc701376 20c3a329c7
commit 2a7848c769
1 changed files with 28 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
View File

@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED()
// apply a 5-sigma threshold
current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
{
if (current_ekf_state.velHealth || staticMode) {
current_ekf_state.velHealth = true;
current_ekf_state.velFailTime = millis();
} else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) {
// XXX check
current_ekf_state.velHealth = true;
ResetVelocity();
ResetStoredStates();
// do not fuse bad data
fuseVelData = false;
}
else
{
@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED()
{
current_ekf_state.posHealth = true;
current_ekf_state.posFailTime = millis();
if (current_ekf_state.posTimeout) {
ResetPosition();
// XXX cross-check the state reset
ResetStoredStates();
// do not fuse position data on this time
// step
fusePosData = false;
}
}
else
{
@ -1070,10 +1087,18 @@ void AttPosEKF::FuseVelposNED()
// apply a 10-sigma threshold
current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode)
{
current_ekf_state.hgtHealth = true;
current_ekf_state.hgtFailTime = millis();
// if we just reset from a timeout, do not fuse
// the height data, but reset height and stored states
if (current_ekf_state.hgtTimeout) {
ResetHeight();
ResetStoredStates();
fuseHgtData = false;
}
}
else
{