diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index e119c361e8a72e7f898725c4b942a720b6b6076e..1296b639b48483d3a0309d5136e77c5a97c5fc7e 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();
     }