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..81563a65 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 @@ -45,10 +45,10 @@ class Localizer Matrix<double, 5, 1> x_hat; Matrix<double, 5, 5> R; Matrix<double, 5, 5> P; - Matrix<double, 2, 2> Q_GPS; + Matrix<double, 3, 3> Q_GPS; Matrix<double, 1, 1> Q_Encoder; Matrix<double, 1, 1> Q_IMU; - Matrix<double, 2, 5> C_GPS; + Matrix<double, 3, 5> C_GPS; Matrix<double, 1, 5> C_Encoder; Matrix<double, 1, 5> C_IMU; 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 1f5b0618..6b283485 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 @@ -55,10 +55,11 @@ void Localizer::GPS_Callback(const robobuggy::GPS::ConstPtr &msg) } prev_position_utm = p; - Matrix<double, 2, 1> z; + Matrix<double, 3, 1> z; z << p.easting, - p.northing + p.northing, + heading_cartesian ; kalman_filter(C_GPS, Q_GPS, z); @@ -146,8 +147,9 @@ void Localizer::init_P() void Localizer::init_Q_GPS() { Q_GPS << - 1, 0, - 0, 1 + 1, 0, 0, + 0, 1, 0, + 0, 0, 0.01 ; std::stringstream s; @@ -184,7 +186,8 @@ void Localizer::init_C_GPS() { C_GPS << 1, 0, 0, 0, 0, - 0, 1, 0, 0, 0 + 0, 1, 0, 0, 0, + 0, 0, 0, 1, 0 ; std::stringstream s;