mavros_extras: Wheel odometry plugin, twist covariance matrix non-use…

…d diagonal elements zeroed.
sayali-purdue · Jan 12, 2019 · 034be9f · 034be9f
1 parent adbdd97
commit 034be9f
Showing 1 changed file with 6 additions and 6 deletions.
diff --git a/mavros_extras/src/plugins/wheel_odometry.cpp b/mavros_extras/src/plugins/wheel_odometry.cpp
@@ -242,8 +242,8 @@ class WheelOdometryPlugin : public plugin::PluginBase {
 		// covariance
 		ftf::EigenMapCovariance6d twist_cov_map(twist_cov.covariance.data());
 		twist_cov_map.setZero();
-		twist_cov_map.block<3, 3>(0, 0).diagonal() << rtwist_cov(0), rtwist_cov(1), -1.0;
-		twist_cov_map.block<3, 3>(3, 3).diagonal() << -1.0, -1.0, rtwist_cov(2);
+		twist_cov_map.block<2, 2>(0, 0).diagonal() << rtwist_cov(0), rtwist_cov(1);
+		twist_cov_map.block<1, 1>(5, 5).diagonal() << rtwist_cov(2);
 
 		// Publish twist
 		if (twist_send) {
@@ -408,13 +408,13 @@ class WheelOdometryPlugin : public plugin::PluginBase {
 		double px; // dP/dTheta
 		double qx; // dQ/dTheta
 		if (std::abs(theta) > 1.e-5) {
-		  px = (theta*cos_theta - sin_theta) / (theta*theta);
-		  qx = (theta*sin_theta - (1-cos_theta)) / (theta*theta);
+			px = (theta*cos_theta - sin_theta) / (theta*theta);
+			qx = (theta*sin_theta - (1-cos_theta)) / (theta*theta);
 		}
 		// Limits for theta -> 0
 		else {
-		  px = 0;
-		  qx = 0.5;
+			px = 0;
+			qx = 0.5;
 		}
 		// dM/dTheta
 		Eigen::Matrix3d M_theta;