From 3f563cf8dfb391745ca3e9af58bc58daa94cb809 Mon Sep 17 00:00:00 2001
From: Emilio Corigliano <emilio.corigliano@skywarder.eu>
Date: Sat, 25 Nov 2023 17:41:34 +0100
Subject: [PATCH] [NAS] Used correctly the acceleration R matrix in correctAcc
---
src/shared/algorithms/NAS/NAS.cpp | 3 +--
1 file changed, 1 insertion(+), 2 deletions(-)
diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index 099e557b7..722485c94 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -278,8 +278,7 @@ void NAS::correctAcc(const Vector3f& acc)
Matrix<float, 3, 6> H;
H << M, Matrix3f::Zero(3, 3);
Matrix<float, 6, 6> Pq = P.block<6, 6>(IDX_QUAT, IDX_QUAT);
- // TODO: Change R_mag with R_acc
- Matrix<float, 3, 3> S = H * Pq * H.transpose() + R_mag;
+ Matrix<float, 3, 3> S = H * Pq * H.transpose() + R_acc;
Matrix<float, 6, 3> K = Pq * H.transpose() * S.inverse();
Matrix<float, 6, 1> dx = K * (acc - aEst);
--
GitLab