Remove unused states from locationd yawrate kalman filer

selfdrive/locationd/locationd_yawrate.cc

@@ -10,7 +10,7 @@
 #include "locationd_yawrate.h"
 
 
-void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas) {
+void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
   double dt = current_time - prev_update_time;
 
   if (dt < 0) {
@@ -19,18 +19,12 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double
     prev_update_time = current_time;
   }
 
-  A <<
-    1,  0, 0,  0,
-    0,  1,  0,  0,
-    0,  0,  1,  0,
-    0,  0,  0,  1;
-
   x = A * x;
   P = A * P * A.transpose() + dt * Q;
 
   double y = meas - C * x;
   double S = R + C * P * C.transpose();
-  Eigen::Vector4d K = P * C.transpose() * (1.0 / S);
+  Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
   x = x + K * y;
   P = (I - K * C) * P;
 }
@@ -63,27 +57,25 @@ void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_sta
 
 
 Localizer::Localizer() {
-  // States: [yaw rate, yaw rate diff, gyro bias, gyro bias diff]
+  // States: [yaw rate, gyro bias]
-  I <<
+  A <<
-    1, 0, 0, 0,
+    1, 0,
-    0, 1, 0, 0,
+    0, 1;
-    0, 0, 1, 0,
-    0, 0, 0, 1;
 
   Q <<
-    pow(.1, 2.0), 0, 0, 0,
+    pow(.1, 2.0), 0,
-    0, 0, 0, 0,
+    0, pow(0.005/ 100.0, 2.0),
-    0, 0, pow(0.005/ 100.0, 2.0), 0,
-    0, 0, 0, 0;
   P <<
-    pow(10000.0, 2.0), 0, 0, 0,
+    pow(10000.0, 2.0), 0,
-    0, pow(10000.0, 2.0), 0, 0,
+    0, pow(10000.0, 2.0);
-    0, 0, pow(10000.0, 2.0), 0,
+
-    0, 0, 0, pow(10000.0, 2.0);
+  I <<
-
+    1, 0,
-  C_posenet << 1, 0, 0, 0;
+    0, 1;
-  C_gyro << 1, 0, 1, 0;
+
-  x << 0, 0, 0, 0;
+  C_posenet << 1, 0;
+  C_gyro << 1, 1;
+  x << 0, 0;
 
   R_gyro = pow(0.025, 2.0);
 }

selfdrive/locationd/locationd_yawrate.h

@@ -7,22 +7,22 @@
 
 class Localizer
 {
-  Eigen::Matrix4d A;
+  Eigen::Matrix2d A;
-  Eigen::Matrix4d I;
+  Eigen::Matrix2d I;
-  Eigen::Matrix4d Q;
+  Eigen::Matrix2d Q;
-  Eigen::Matrix<double, 1, 4> C_posenet;
+  Eigen::Matrix<double, 1, 2> C_posenet;
-  Eigen::Matrix<double, 1, 4> C_gyro;
+  Eigen::Matrix<double, 1, 2> C_gyro;
 
   double R_gyro;
 
-  void update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas);
+  void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
   void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
   void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
   void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
 
 public:
-  Eigen::Vector4d x;
+  Eigen::Vector2d x;
-  Eigen::Matrix4d P;
+  Eigen::Matrix2d P;
   double steering_angle = 0;
   double car_speed = 0;
   double posenet_speed = 0;