Merge pull request KumarRobotics#80 from nobleo/fix/covariance_unit

Fix unit in covariance calculation
rpng · Mar 10, 2020 · 62a3f50 · 62a3f50
2 parents 8eae10a + 0c01407
commit 62a3f50
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/ublox_gps/src/node.cpp b/ublox_gps/src/node.cpp
@@ -1751,7 +1751,7 @@ void HpPosRecProduct::callbackNavRelPosNed(const ublox_msgs::NavRELPOSNED9 &m) {
     // When heading is reported to be valid, use accuracy reported in 1e-5 deg units
     if (m.flags & ublox_msgs::NavRELPOSNED9::FLAGS_REL_POS_HEAD_VALID)
     {
-      imu_.orientation_covariance[8] = pow(m.accHeading / 10000.0, 2);
+      imu_.orientation_covariance[8] = pow(m.accHeading * 1e-5 / 180.0 * M_PI, 2);
     }
 
     imu_pub.publish(imu_);