From 777e183af32ee2cf22c1c7fec5142d9ab4d6fa6c Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Thu, 14 May 2020 16:30:00 -0700 Subject: [PATCH] too complex old-commit-hash: 013e49bf907539d119fbebcf02f4ce3749849065 --- selfdrive/locationd/models/live_kf.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index f2c00e5811..92c5652036 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -159,13 +159,13 @@ class LiveKalman(): # Observation functions # imu_rot = euler_rotate(*imu_angles) - h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias, + h_gyro_sym = sp.Matrix([vroll + roll_bias, vpitch + pitch_bias, vyaw + yaw_bias]) pos = sp.Matrix([x, y, z]) gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos) - h_acc_sym = imu_rot * (gravity + acceleration) + h_acc_sym = (gravity + acceleration) h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) speed = sp.sqrt(vx**2 + vy**2 + vz**2)