Merge pull request #29 from fixposition/main

Add missing covariance initialization (fixposition#74)
Greenzie · Nov 21, 2024 · 015157d · 015157d
2 parents 53e6481 + 55facad
commit 015157d
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp
@@ -186,6 +186,8 @@ inline times::GpsTime ConvertGpsTime(const std::string& gps_wno, const std::stri
 inline Eigen::Matrix<double, 3, 3> BuildCovMat3D(const double xx, const double yy, const double zz, 
                                                  const double xy, const double yz, const double xz) {
     Eigen::Matrix<double, 3, 3> cov;
+    cov.setZero();
+
     // Diagonals
     cov(0, 0) = xx;   // 0
     cov(1, 1) = yy;   // 4
@@ -227,6 +229,8 @@ inline Eigen::Matrix<double, 6, 6> BuildCovMat6D(const double xx, const double y
                                                  const double yz, const double xz, double xx1, const double yy1,
                                                  const double zz1, const double xy1, const double yz1, double xz1) {
     Eigen::Matrix<double, 6, 6> cov;
+    cov.setZero();
+
     // Diagonals
     cov(0, 0) = xx;   // 0
     cov(1, 1) = yy;   // 7