needed for stability

6 years ago · 00e05bc7d8
parent fcf879d783
commit 00e05bc7d8
1 changed files with 1 additions and 1 deletions
--- a/selfdrive/locationd/models/live_kf.py
+++ b/selfdrive/locationd/models/live_kf.py
@ -168,7 +168,7 @@ class LiveKalman():
    h_acc_sym = (gravity + acceleration)
    h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])

-    speed = sp.sqrt(vx**2 + vy**2 + vz**2)
+    speed = sp.sqrt(vx**2 + vy**2 + vz**2 + 1e-6)
    h_speed_sym = sp.Matrix([speed * odo_scale])

    h_pos_sym = sp.Matrix([x, y, z])