From 6179592ccd1d287ef63e1c051fe6cbadf7be4800 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 25 Jun 2018 21:33:59 -0400 Subject: [PATCH 1/2] Fixed a longtime issue in the localizer where we would need to keep swapping x and y around It turns out that the reason for the inconsistencies was that we were generating the pose message incorrectly Also cleaned up the magic indices so that we don't have any more off-by-one errors with respect to the indexing --- .../include/transistor/localizer/Localizer.h | 6 ++++++ .../src/transistor/localizer/Localizer.cpp | 18 ++++++++++-------- 2 files changed, 16 insertions(+), 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 index b7d4e418..db6e65c4 100755 --- 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 index d994e09b..68c22c1a 100755 --- 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,8 +354,8 @@ void Localizer::kalman_filter(MatrixXd c, MatrixXd q, MatrixXd z) Matrix x_pre = A * x; Matrix 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; @@ -361,6 +363,6 @@ void Localizer::kalman_filter(MatrixXd c, MatrixXd q, MatrixXd z) 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)); } From 5bea7e87468d17e352d07c975e6007212cd3dbe4 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 25 Jun 2018 21:42:05 -0400 Subject: [PATCH 2/2] Forgot a couple semicolons --- .../src/robobuggy/src/transistor/localizer/Localizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 index 68c22c1a..3bd07a93 100755 --- 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,8 +282,8 @@ void Localizer::update_position_estimate() { propagate(); - double easting_x = x_hat(ROW_X, 0) - double northing_y = x_hat(ROW_Y, 0) + 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(ROW_HEADING, 0);