From 57025f2c3ba48e9d29b4586c949be92746982ee6 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] Fixed correctAcc
using R_acc instead of R_mag
Co-Authored-By: Davide Basso <davide.basso@skywarder.eu>
---
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 de9aece2c..1b9fdaad5 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -313,8 +313,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