From c98cd404e033976667ef6a112dfe7c2149aec9f9 Mon Sep 17 00:00:00 2001
From: Emilio Corigliano <emilio.corigliano@skywarder.eu>
Date: Thu, 29 Aug 2024 15:21:29 +0200
Subject: [PATCH] [MEA] Last small changes
- Doing computeForce before correctBaro
- Checking if S matrix (scalar) is invertible
---
src/shared/algorithms/MEA/MEA.cpp | 22 ++++++++++++++++------
1 file changed, 16 insertions(+), 6 deletions(-)
diff --git a/src/shared/algorithms/MEA/MEA.cpp b/src/shared/algorithms/MEA/MEA.cpp
index a30f73b6f..0408f3d51 100644
--- a/src/shared/algorithms/MEA/MEA.cpp
+++ b/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);
@@ -137,7 +137,12 @@ void MEA::correctBaro(const Step &step)
if (!step.hasCCPressure)
return;
- float S = baroH * P * baroH.transpose() + baroR;
+ 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;
@@ -163,7 +168,12 @@ void MEA::correctAccel(const Step &step)
float accelR = alpha * q + c;
- float S = accelH * P * accelH.transpose() + accelR;
+ 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;
--
GitLab