rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

check variances for validity and reinitialize if invalid

Browse Source
This commit is contained in:
Corvus Corax 2013-06-06 00:07:11 +02:00
parent f22a0d299e
commit bab95e7ca8
1 changed files with 7 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
flight/modules/StateEstimation/filterekf.c
View File

@ -422,6 +422,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
EKFStateVarianceGet(&vardata); EKFStateVarianceGet(&vardata);
INSGetP(vardata.P); INSGetP(vardata.P);
EKFStateVarianceSet(&vardata); EKFStateVarianceSet(&vardata);
int t;
for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
if (invalid_var(vardata.P[t])) {
INSResetP(this->ekfConfiguration.P);
break;
}
}
// all sensor data has been used, reset! // all sensor data has been used, reset!
this->work.updated = 0; this->work.updated = 0;