From 063d1bcd185f38062057032144a7ec8019fe89b8 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Fri, 29 Apr 2022 10:00:19 +0200
Subject: [PATCH] [NAS] Fixed H_gps incorrect initialization

---
 src/shared/algorithms/NAS/NAS.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index e119c361e..1296b639b 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -59,9 +59,11 @@ NAS::NAS(NASConfig config) : config(config)
 
     // GPS matrixes
     {
-        H_gps                = Matrix<float, 4, 6>::Identity();
-        H_gps.coeffRef(2, 2) = 0;
-        H_gps.coeffRef(5, 5) = 0;
+        H_gps                = Matrix<float, 4, 6>::Zero();
+        H_gps.coeffRef(0, 0) = 1;
+        H_gps.coeffRef(1, 1) = 1;
+        H_gps.coeffRef(2, 3) = 1;
+        H_gps.coeffRef(3, 4) = 1;
         H_gps_tr             = H_gps.transpose();
         R_gps << config.SIGMA_GPS * Matrix<float, 4, 4>::Identity();
     }
-- 
GitLab