retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Fix locationd_yawrate A matrix

albatross
Willem Melching 2020-01-21 13:52:45 -08:00
parent cf70368f67
commit ec6e83ca2d
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
selfdrive/locationd/locationd_yawrate.cc
View File

@ -19,6 +19,12 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double
prev_update_time = current_time;
}
A <<
1, dt, 0, 0,
0, 1, 0, 0,
0, 0, 1, dt,
0, 0, 0, 1;
x = A * x;
P = A * P * A.transpose() + dt * Q;
@ -58,11 +64,6 @@ void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_sta
Localizer::Localizer() {
// States: [yaw rate, yaw rate diff, gyro bias, gyro bias diff]
A <<
1, 1, 0, 0,
0, 1, 0, 0,
0, 0, 1, 1,
0, 0, 0, 1;
I <<
1, 0, 0, 0,
0, 1, 0, 0,