LP-588 INS13 - Scale down Mag variance setting by Be² + Small fixes

Author: f5soh

flight/libraries/inc/insgps.h

@@ -58,7 +58,7 @@ void INSGPSInit();
 void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT);
 void INSCovariancePrediction(float dT);
 void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3],
-                   float BaroAlt, uint16_t SensorsUsed);
+                   const float BaroAlt, uint16_t SensorsUsed);
 void INSResetP(const float PDiag[13]);
 void INSGetVariance(float PDiag[13]);
 void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3]);

flight/libraries/insgps13state.c

@@ -92,7 +92,6 @@ static struct EKFData {
     float H[NUMV][NUMX];
     // local magnetic unit vector in NED frame
     float Be[3];
-    float BeScaleFactor;
     // covariance matrix and state vector
     float P[NUMX][NUMX];
     float X[NUMX];
@@ -281,26 +280,25 @@ void INSSetGyroBiasVar(const float gyro_bias_var[3])
     ekf.Q[8] = gyro_bias_var[2];
 }
 
-// must be called AFTER SetMagNorth
-void INSSetMagVar(const float mag_var[3])
+void INSSetMagVar(const float scaled_mag_var[3])
 {
-    ekf.R[6] = mag_var[0] * ekf.BeScaleFactor;
-    ekf.R[7] = mag_var[1] * ekf.BeScaleFactor;
-    ekf.R[8] = mag_var[2] * ekf.BeScaleFactor;
+    ekf.R[6] = scaled_mag_var[0];
+    ekf.R[7] = scaled_mag_var[1];
+    ekf.R[8] = scaled_mag_var[2];
 }
 
-void INSSetBaroVar(float baro_var)
+void INSSetBaroVar(const float baro_var)
 {
     ekf.R[9] = baro_var;
 }
 
 void INSSetMagNorth(const float B[3])
 {
-    ekf.BeScaleFactor = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
+    float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
 
-    ekf.Be[0] = B[0] * ekf.BeScaleFactor;
-    ekf.Be[1] = B[1] * ekf.BeScaleFactor;
-    ekf.Be[2] = B[2] * ekf.BeScaleFactor;
+    ekf.Be[0] = B[0] * invmag;
+    ekf.Be[1] = B[1] * invmag;
+    ekf.Be[2] = B[2] * invmag;
 }
 
 void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT)
@@ -402,8 +400,6 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[
     Z[3] = Vel[0];
     Z[4] = Vel[1];
     Z[5] = Vel[2];
-    // magnetometer data in any units (use unit vector) and in body frame
-
 
     if (SensorsUsed & MAG_SENSORS) {
         // magnetometer data in any units (use unit vector) and in body frame

flight/modules/StateEstimation/filterekf.c

@@ -232,9 +232,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
             // Reset the INS algorithm
             INSGPSInit();
             // variance is measured in mGaus, but internally the EKF works with a normalized  vector. Scale down by Be^2
-            INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX,
-                                      this->ekfConfiguration.R.MagY,
-                                      this->ekfConfiguration.R.MagZ }
+            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.MagX / Be2,
+                                      this->ekfConfiguration.R.MagY / Be2,
+                                      this->ekfConfiguration.R.MagZ / Be2 }
                          );
             INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.AccelX,
                                         this->ekfConfiguration.Q.AccelY,