diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 762b5e8ba..cdf8a7b99 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -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],