simplify and better

selfdrive/locationd/locationd_yawrate.cc

@@ -20,9 +20,9 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double
   }
 
   A <<
-    1,  dt, 0,  0,
+    1,  0, 0,  0,
     0,  1,  0,  0,
-    0,  0,  1,  dt,
+    0,  0,  1,  0,
     0,  0,  0,  1;
 
   x = A * x;
@@ -46,7 +46,7 @@ void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reade
 }
 
 void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
-  double R = pow(30.0 *camera_odometry.getRotStd()[2], 2);
+  double R = pow(10 * camera_odometry.getRotStd()[2], 2);
   double meas = camera_odometry.getRot()[2];
   update_state(C_posenet, R, current_time, meas);
 
@@ -71,21 +71,21 @@ Localizer::Localizer() {
     0, 0, 0, 1;
 
   Q <<
+    pow(.1, 2.0), 0, 0, 0,
     0, 0, 0, 0,
-    0, pow(0.1, 2.0), 0, 0,
-    0, 0, 0, 0,
-    0, 0, pow(0.005 / 100.0, 2.0), 0;
+    0, 0, pow(0.005/ 100.0, 2.0), 0,
+    0, 0, 0, 0;
   P <<
-    pow(100.0, 2.0), 0, 0, 0,
-    0, pow(100.0, 2.0), 0, 0,
-    0, 0, pow(100.0, 2.0), 0,
-    0, 0, 0, pow(100.0, 2.0);
+    pow(10000.0, 2.0), 0, 0, 0,
+    0, pow(10000.0, 2.0), 0, 0,
+    0, 0, pow(10000.0, 2.0), 0,
+    0, 0, 0, pow(10000.0, 2.0);
 
   C_posenet << 1, 0, 0, 0;
   C_gyro << 1, 0, 1, 0;
   x << 0, 0, 0, 0;
 
-  R_gyro = pow(0.25, 2.0);
+  R_gyro = pow(0.025, 2.0);
 }
 
 void Localizer::handle_log(cereal::Event::Reader event) {