rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Divide mag variance by Be squared to cope with normalization correctly

Browse Source
This commit is contained in:
Corvus Corax 2013-06-06 17:54:59 +02:00
parent bab95e7ca8
commit b0db191703
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -240,10 +240,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->init_stage == 0) {
// Reset the INS algorithm
INSGPSInit();
// variance is measured in mGaus, but internally the EKF works with a normalized vector. Scaling factor from old AHRS code (1000*1000)
INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX] / 1e6f,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / 1e6f,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] / 1e6f }
// variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2
float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2];
INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX] / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY] / Be2,
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] / Be2 }
);
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],