From e29b6fef3a147976fcc360dc93669c57ca4601f1 Mon Sep 17 00:00:00 2001
From: Emilio Corigliano <emilio.corigliano@skywarder.eu>
Date: Wed, 4 Sep 2024 12:22:33 +0200
Subject: [PATCH] [NAS] Updated correctBaro
Co-Authored-By: Davide Basso <davide.basso@skywarder.eu>
---
src/shared/algorithms/NAS/NAS.cpp | 27 +++++++++++++++++----------
src/shared/algorithms/NAS/NASConfig.h | 2 +-
2 files changed, 18 insertions(+), 11 deletions(-)
diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index 692a618ff..de9aece2c 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -153,28 +153,35 @@ void NAS::predictGyro(const GyroscopeData& angularSpeed)
void NAS::correctBaro(const float pressure)
{
- Matrix<float, 1, 6> H = Matrix<float, 1, 6>::Zero();
+ float altitude = -x(2);
+
+ float temp = Aeroutils::relTemperature(altitude, reference.refTemperature);
- // Temperature at current altitude. Since in NED the altitude is negative,
- // mslTemperature returns temperature at current altitude and not at msl
- float temp = Aeroutils::relTemperature(-x(2), reference.refTemperature);
+ Matrix<float, 1, 6> H = Matrix<float, 1, 6>::Zero();
+ Matrix<float, 1, 1> R = Matrix<float, 1, 1>(powf(config.SIGMA_BAR, 2));
// Compute gradient of the altitude-pressure function
H[2] = Constants::a * Constants::n * reference.refPressure *
- powf(1 - Constants::a * x(2) / temp, -Constants::n - 1) / temp;
+ powf(1 + Constants::a * altitude / temp, Constants::n - 1) / temp;
Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
- Matrix<float, 1, 1> S =
- H * Pl * H.transpose() + Matrix<float, 1, 1>(config.SIGMA_BAR);
- Matrix<float, 6, 1> K = Pl * H.transpose() * S.inverse();
- P.block<6, 6>(0, 0) = (Matrix<float, 6, 6>::Identity() - K * H) * Pl;
+ Matrix<float, 1, 1> S = H * Pl * H.transpose() + R;
+
+ // If not invertible, don't do the correction and return
+ if (S.determinant() < 1e-3)
+ {
+ return;
+ }
float y_hat =
Aeroutils::relPressure(reference.refAltitude - x(2),
reference.mslPressure, reference.mslTemperature);
+ Matrix<float, 1, 1> e = Matrix<float, 1, 1>(pressure - y_hat);
+ Matrix<float, 6, 1> K = Pl * H.transpose() * S.inverse();
+ P.block<6, 6>(0, 0) = (Matrix<float, 6, 6>::Identity() - K * H) * Pl;
// Update the state
- x.head<6>() = x.head<6>() + K * (pressure - y_hat);
+ x.head<6>() = x.head<6>() + K * e;
}
void NAS::correctGPS(const Vector4f& gps)
diff --git a/src/shared/algorithms/NAS/NASConfig.h b/src/shared/algorithms/NAS/NASConfig.h
index 9c3338907..ab2020d5a 100644
--- a/src/shared/algorithms/NAS/NASConfig.h
+++ b/src/shared/algorithms/NAS/NASConfig.h
@@ -37,7 +37,7 @@ struct NASConfig
Eigen::Vector4f SIGMA_GPS; ///< [millideg^2, millideg^2, m^2/s^2, m^2/s^2]
///< estimated GPS variance. position from test,
///< velocity from datasheet
- float SIGMA_BAR; ///< [m^2] Estimated altitude variance
+ float SIGMA_BAR; ///< [Pa] Estimated altitude variance
float SIGMA_POS; ///< [m^2] Estimated variance of the position noise
float SIGMA_VEL; ///< [(m/s)^2] Estimated variance of the velocity noise
float SIGMA_PITOT; ///< [Pa^2] Estimated variance of the pitot velocity
--
GitLab