From 75e5332e4c5697e8aca1c7e8f050c434f55c343b Mon Sep 17 00:00:00 2001
From: Emilio Corigliano <emilio.corigliano@skywarder.eu>
Date: Sun, 29 Sep 2024 16:39:30 +0200
Subject: [PATCH] [NAS] Corrected H calculation in correctBaro
---
src/shared/algorithms/NAS/NAS.cpp | 5 +++--
1 file changed, 3 insertions(+), 2 deletions(-)
diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index 4dba09605..1b162b6a1 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -156,7 +156,8 @@ void NAS::predictGyro(const GyroscopeData& angularSpeed)
void NAS::correctBaro(const float pressure)
{
- float altitude = -x(2);
+ float d = x(2);
+ float altitude = -d;
float temp = Aeroutils::relTemperature(altitude, reference.refTemperature);
@@ -165,7 +166,7 @@ void NAS::correctBaro(const float pressure)
// Compute gradient of the altitude-pressure function
H[2] = Constants::a * Constants::n * reference.refPressure *
- powf(1 + Constants::a * altitude / temp, Constants::n - 1) / temp;
+ powf(1 + Constants::a * d / temp, Constants::n - 1) / temp;
Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
float S = H * Pl * H.transpose() + R;
--
GitLab