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

simplify and better

pull/994/head
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 <<
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) {