Qcom localizer: erratic clocks have an erratic error rate too (#28654)

Erratic clocks drift erratically too
2 years ago · af9edb020d
parent 164dd5015e
commit af9edb020d
1 changed files with 3 additions and 2 deletions
--- a/selfdrive/locationd/models/loc_kf.py
+++ b/selfdrive/locationd/models/loc_kf.py
@ -334,12 +334,13 @@ class LocKalman():


    # process noise
-    clock_error_drift = 100.0 if erratic_clock else 0.1
+    q_clock_error = 100.0 if erratic_clock else 0.1
+    q_clock_error_rate = 10 if erratic_clock else 0.0
    self.Q = np.diag([0.03**2, 0.03**2, 0.03**2,
                      0.0**2, 0.0**2, 0.0**2,
                      0.0**2, 0.0**2, 0.0**2,
                      0.1**2, 0.1**2, 0.1**2,
-                      (clock_error_drift)**2, (0)**2,
+                      (q_clock_error)**2, (q_clock_error_rate)**2,
                      (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
                      (0.02 / 100)**2,
                      3**2, 3**2, 3**2,