From e82a52e8ff4998d3b1e45de635af8b216753091d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 25 May 2013 23:06:30 +0200 Subject: [PATCH] filterekf: changed mag variance to 1/1000000 to compensate for normalization, as it was done on old AHRS --- flight/modules/StateEstimation/filterekf.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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],