make it converge within 1min

6 years ago · fe250d6a9e
parent 58f4f5aac4
commit fe250d6a9e
1 changed files with 2 additions and 2 deletions
--- a/selfdrive/locationd/locationd_yawrate.cc
+++ b/selfdrive/locationd/locationd_yawrate.cc
@ -40,7 +40,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(10 * camera_odometry.getRotStd()[2], 2);
+  double R = pow(5 * camera_odometry.getRotStd()[2], 2);
  double meas = camera_odometry.getRot()[2];
  update_state(C_posenet, R, current_time, meas);

@ -64,7 +64,7 @@ Localizer::Localizer() {

  Q <<
    pow(.1, 2.0), 0,
-    0, pow(0.005/ 100.0, 2.0),
+    0, pow(0.05/ 100.0, 2.0),
  P <<
    pow(10000.0, 2.0), 0,
    0, pow(10000.0, 2.0);