Merge pull request #501 from CMU-Robotics-Club/vivaan_localizer_fixes

Fixed a longtime issue in the localizer
CMU-Robotics-Club · Aug 28, 2018 · 14b4cec · 14b4cec
2 parents 0cef359 + 2070e7c
commit 14b4cec
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 8 deletions.
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/include/transistor/localizer/Localizer.h b/Software/real_time/ROS_RoboBuggy/src/robobuggy/include/transistor/localizer/Localizer.h
@@ -33,6 +33,12 @@ class Localizer
     long int previous_update_time_ms;
     long int prev_encoder_time;
 
+    const int ROW_X = 0;
+    const int ROW_Y = 1;
+    const int ROW_VEL = 2;
+    const int ROW_HEADING = 3;
+    const int ROW_TH_DOT = 4;
+
     ros::NodeHandle nh;
     ros::Publisher pose_pub;
     ros::Subscriber gps_sub;

diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/src/transistor/localizer/Localizer.cpp b/Software/real_time/ROS_RoboBuggy/src/robobuggy/src/transistor/localizer/Localizer.cpp
@@ -282,9 +282,11 @@ void Localizer::update_position_estimate()
 {
     propagate();
 
-    geodesy::UTMPoint utm_point(x_hat(1, 0), x_hat(0, 0), 17, 'T');
+    double easting_x = x_hat(ROW_X, 0);
+    double northing_y = x_hat(ROW_Y, 0);
+    geodesy::UTMPoint utm_point(easting_x, northing_y, 17, 'T');
     geographic_msgs::GeoPoint gps_point = geodesy::toMsg(utm_point);
-    double heading = x_hat(3, 0);
+    double heading = x_hat(ROW_HEADING, 0);
 
     robobuggy::Pose p;
     ros::Time time_now = ros::Time::now();
@@ -301,8 +303,8 @@ void Localizer::update_motion_model(double dt)
 {
 
     A <<
-      1, 0, dt * cos(x(3, 0)), 0, 0,
-      0, 1, dt * sin(x(3, 0)), 0, 0,
+      1, 0, dt * cos(x(ROW_HEADING, 0)), 0, 0,
+      0, 1, dt * sin(x(ROW_HEADING, 0)), 0, 0,
       0, 0, 1, 0, 0,
       0, 0, 0, 1, dt,
       0, 0, tan(current_steering_angle) / WHEELBASE_M, 0, 0
@@ -352,15 +354,15 @@ void Localizer::kalman_filter(MatrixXd c, MatrixXd q, MatrixXd z)
     Matrix<double, 5, 1> x_pre = A * x;
     Matrix<double, 5, 5> P_pre = A * P * A.transpose() + R;
 
-    x_pre(3, 0) = clamp_angle(x_pre(3, 0));
-    x_pre(4, 0) = clamp_angle(x_pre(4, 0));
+    x_pre(ROW_HEADING, 0) = clamp_angle(x_pre(ROW_HEADING, 0));
+    x_pre(ROW_TH_DOT, 0) = clamp_angle(x_pre(ROW_TH_DOT, 0));
 
     MatrixXd residual = z - c * x_pre;
     MatrixXd K = c * P_pre * c.transpose() + q;
     K = P_pre * c.transpose() * K.inverse();
     x = x_pre + K * residual;
     P = (MatrixXd::Identity(5,5) - (K * c)) * P_pre;
 
-    x(3, 0) = clamp_angle(x(3, 0));
-    x(4, 0) = clamp_angle(x(4, 0));
+    x(ROW_HEADING, 0) = clamp_angle(x(ROW_HEADING, 0));
+    x(ROW_TH_DOT, 0) = clamp_angle(x(ROW_TH_DOT, 0));
 }