diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc index e415f9adc8..4b93b68014 100644 --- a/selfdrive/locationd/locationd_yawrate.cc +++ b/selfdrive/locationd/locationd_yawrate.cc @@ -40,7 +40,7 @@ void Localizer::handle_sensor_events(capnp::List::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);