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

filterekf: changed mag variance to 1/1000000 to compensate for normalization, as it was done on old AHRS

Browse Source
This commit is contained in:
Corvus Corax 2013-05-25 23:06:30 +02:00
parent 7bf58f09c5
commit e82a52e8ff
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -233,9 +233,10 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->init_stage == 0) {
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX],
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY],
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] }
// 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 }
);
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],