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(); }