AttPosEKF: Fix sensor loss recovery

This commit is contained in:
Johan Jansen 2015-02-13 10:52:10 +01:00
parent 331352c75d
commit f64a8d7cb0
3 changed files with 26 additions and 20 deletions

View File

@ -633,14 +633,10 @@ void AttitudePositionEstimatorEKF::task_main()
}
else if (!_ekf->statesInitialised) {
// North, East Down position (m)
float posNED[3] = {0.0f, 0.0f, 0.0f};
float initVelNED[3];
float initVelNED[3] = {0.0f, 0.0f, 0.0f};
initVelNED[0] = 0.0f;
initVelNED[1] = 0.0f;
initVelNED[2] = 0.0f;
_ekf->posNE[0] = posNED[0];
_ekf->posNE[1] = posNED[1];
_ekf->posNE[0] = 0.0f;
_ekf->posNE[1] = 0.0f;
_local_pos.ref_alt = _baro_ref;
_baro_ref_offset = 0.0f;
@ -1291,7 +1287,6 @@ void AttitudePositionEstimatorEKF::pollData()
if (dtGoodGPS > POS_RESET_THRESHOLD) {
_ekf->ResetPosition();
_ekf->ResetVelocity();
_ekf->ResetStoredStates();
}
}

View File

@ -1121,10 +1121,9 @@ void AttPosEKF::FuseVelposNED()
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;
}
@ -1152,9 +1151,6 @@ void AttPosEKF::FuseVelposNED()
if (current_ekf_state.posTimeout) {
ResetPosition();
// XXX cross-check the state reset
ResetStoredStates();
// do not fuse position data on this time
// step
fusePosData = false;
@ -1183,7 +1179,6 @@ void AttPosEKF::FuseVelposNED()
// the height data, but reset height and stored states
if (current_ekf_state.hgtTimeout) {
ResetHeight();
ResetStoredStates();
fuseHgtData = false;
}
}
@ -2855,6 +2850,12 @@ void AttPosEKF::ResetPosition(void)
// reset the states from the GPS measurements
states[7] = posNE[0];
states[8] = posNE[1];
// stored horizontal position states to prevent subsequent GPS measurements from being rejected
for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
storedStates[7][i] = states[7];
storedStates[8][i] = states[8];
}
}
}
@ -2862,6 +2863,11 @@ void AttPosEKF::ResetHeight(void)
{
// write to the state vector
states[9] = -hgtMea;
// stored horizontal position states to prevent subsequent Barometer measurements from being rejected
for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
storedStates[9][i] = states[9];
}
}
void AttPosEKF::ResetVelocity(void)
@ -2871,10 +2877,16 @@ void AttPosEKF::ResetVelocity(void)
states[5] = 0.0f;
states[6] = 0.0f;
} else if (GPSstatus >= GPS_FIX_3D) {
//Do not use Z velocity, we trust the Barometer history more
states[4] = velNED[0]; // north velocity from last reading
states[5] = velNED[1]; // east velocity from last reading
states[6] = velNED[2]; // down velocity from last reading
// stored horizontal position states to prevent subsequent GPS measurements from being rejected
for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
storedStates[4][i] = states[4];
storedStates[5][i] = states[5];
}
}
}
@ -2992,10 +3004,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
// Fill error report
GetFilterState(&last_ekf_error);
ResetStoredStates();
ResetVelocity();
ResetPosition();
ResetHeight();
ResetStoredStates();
// Timeout cleared with this reset
current_ekf_state.imuTimeout = false;
@ -3009,10 +3021,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
// Fill error report, but not setting error flag
GetFilterState(&last_ekf_error);
ResetStoredStates();
ResetVelocity();
ResetPosition();
ResetHeight();
ResetStoredStates();
ret = 0;
}
@ -3182,6 +3194,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
states[20] = magBias.y; // Magnetic Field Bias Y
states[21] = magBias.z; // Magnetic Field Bias Z
ResetStoredStates();
ResetVelocity();
ResetPosition();
ResetHeight();

View File

@ -298,8 +298,6 @@ public:
void RecallOmega(float *omegaForFusion, uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);