From 818695cfe7f49bb96750b323bd2f5008063d9d1d Mon Sep 17 00:00:00 2001 From: Harald Schafer Date: Thu, 21 May 2020 17:47:05 -0700 Subject: [PATCH] needed for stability old-commit-hash: 00e05bc7d881feaf82529b4066789e41ee555c42 --- selfdrive/locationd/models/live_kf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index c8d4c7ed24..c583d7813b 100755 --- 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])