spacecruft/nopenpilot
spacecruft/nopenpilot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

simplify and better

Browse source
This commit is contained in:
Harald Schafer 2020-01-21 16:37:22 -08:00
parent bbb37309fc
commit c32ebcb2c6
1 changed files with 11 additions and 11 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

22
selfdrive/locationd/locationd_yawrate.cc
View file

@ -20,9 +20,9 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double
} }
A << A <<
1, dt, 0, 0, 1, 0, 0, 0,
0, 1, 0, 0, 0, 1, 0, 0,
0, 0, 1, dt, 0, 0, 1, 0,
0, 0, 0, 1; 0, 0, 0, 1;
x = A * x; 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) { 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]; double meas = camera_odometry.getRot()[2];
update_state(C_posenet, R, current_time, meas); update_state(C_posenet, R, current_time, meas);
@ -71,21 +71,21 @@ Localizer::Localizer() {
0, 0, 0, 1; 0, 0, 0, 1;
Q << Q <<
pow(.1, 2.0), 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0,
0, pow(0.1, 2.0), 0, 0, 0, 0, pow(0.005/ 100.0, 2.0), 0,
0, 0, 0, 0, 0, 0, 0, 0;
0, 0, pow(0.005 / 100.0, 2.0), 0;
P << P <<
pow(100.0, 2.0), 0, 0, 0, pow(10000.0, 2.0), 0, 0, 0,
0, pow(100.0, 2.0), 0, 0, 0, pow(10000.0, 2.0), 0, 0,
0, 0, pow(100.0, 2.0), 0, 0, 0, pow(10000.0, 2.0), 0,
0, 0, 0, pow(100.0, 2.0); 0, 0, 0, pow(10000.0, 2.0);
C_posenet << 1, 0, 0, 0; C_posenet << 1, 0, 0, 0;
C_gyro << 1, 0, 1, 0; C_gyro << 1, 0, 1, 0;
x << 0, 0, 0, 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) { void Localizer::handle_log(cereal::Event::Reader event) {