changed localizer to take out imu since we're having troubles with it

CMU-Robotics-Club · Apr 5, 2018 · 10df1b9 · 10df1b9
1 parent 78da1fd
commit 10df1b9
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 7 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
@@ -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
@@ -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;