mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-23 15:02:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

too complex

Browse Source
This commit is contained in:
Harald Schafer
2020-05-14 16:30:00 -07:00
parent 28bf5d1dd8
commit 013e49bf90
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/models/live_kf.py
+2 -2
View File
@@ -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)