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