New CaptureGnssFix api

src/subscriber_gnss_fix.cpp

@@ -54,12 +54,17 @@ void SubscriberGnssFix::callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
     if (cov_mode_ == "msg" or cov_mode_ == "factor")
         cov_ = cov_factor_ * Eigen::Map<const Eigen::Matrix3d>(msg->position_covariance.data());
 
+    // Cov fix has the 4th element being clock bias (no information about this)
+    Matrix4d cov_fix = Matrix4d::Identity() * 0.1;
+    cov_fix.topLeftCorner<3,3>() = cov_;
+    
     CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
                                                                       sensor_ptr_,
-                                                                      Eigen::Vector3d(msg->latitude * M_PI / 180.0,
+                                                                      Eigen::Vector4d(msg->latitude * M_PI / 180.0,
                                                                                       msg->longitude * M_PI / 180.0,
-                                                                                      msg->altitude),
-                                                                      cov_,
+                                                                                      msg->altitude,
+                                                                                      0),
+                                                                      cov_fix,
                                                                       false); // false = {LatLonAlt fix and ENU cov}
     cap_gnss_ptr->process();
 }