From ec6e83ca2d82fb6afe31ceb1067c93b56b09b9e2 Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Tue, 21 Jan 2020 13:52:45 -0800
Subject: [PATCH] Fix locationd_yawrate A matrix

---
 selfdrive/locationd/locationd_yawrate.cc | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc
index b2b87557c..044e97f85 100644
--- a/selfdrive/locationd/locationd_yawrate.cc
+++ b/selfdrive/locationd/locationd_yawrate.cc
@@ -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,