[MEA] Last small changes

- Doing computeForce before correctBaro
- Checking if S matrix (scalar) is invertible

src/shared/algorithms/MEA/MEA.cpp

@@ -75,12 +75,12 @@ void MEA::update(const Step &step)
     // First run the prediction step
     predict(step);
 
-    // Compute applied force/mach/CD/rho
-    computeForce(step);
-
     // Run cc pressure correction
     correctBaro(step);
 
+    // Compute applied force/mach/CD/rho
+    computeForce(step);
+
     // Run accelerometer correction
     correctAccel(step);
 
@@ -109,7 +109,7 @@ void MEA::computeForce(const Step &step)
 
     // NOTE: Here we assume that verticalSpeed roughly equals the total speed,
     // so that we don't depend on N/E speed components, which can be quite
-    // unreliable in case of no GPS fix
+    // unreliable in case of no GPS fix or during powered ascent
     mach = Aeroutils::computeMach(-step.mslAltitude, step.verticalSpeed,
                                   Constants::MSL_TEMPERATURE);
 
@@ -138,6 +138,11 @@ void MEA::correctBaro(const Step &step)
         return;
 
     float S = baroH * P * baroH.transpose() + baroR;
+
+    // If not invertible, do not invert it
+    if (S < 1e-3)
+        return;
+
     Matrix<float, 3, 1> K = (P * baroH.transpose()) / S;
 
     P = (Matrix<float, 3, 3>::Identity() - K * baroH) * P;
@@ -164,6 +169,11 @@ void MEA::correctAccel(const Step &step)
     float accelR = alpha * q + c;
 
     float S = accelH * P * accelH.transpose() + accelR;
+
+    // If not invertible, do not invert it
+    if (S < 1e-3)
+        return;
+
     Matrix<float, 3, 1> K = (P * accelH.transpose()) / S;
 
     P = (Matrix<float, 3, 3>::Identity() - K * accelH) * P;