too complex

old-commit-hash: 013e49bf90
5 years ago · 777e183af3
parent 8f71de0dbe
commit 777e183af3
1 changed files with 2 additions and 2 deletions
--- 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)