diff --git a/src/subscriber_gnss_fix.cpp b/src/subscriber_gnss_fix.cpp index 5ce3ca8b056b70d58ecafd4c4bbe5604b600ea8d..f987893f7c03ccf537175f32fb19b1b9306935f2 100644 --- a/src/subscriber_gnss_fix.cpp +++ b/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(); }