From 1c8af02e549efb2f353f6ede17a6663a2873f25a Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Fri, 8 Jul 2022 18:20:16 +0200
Subject: [PATCH] laikad: add residual threshold for pos_fix (#25082)

* laikad: add residual threshold for pos_fix

* update ref

* update test
old-commit-hash: 7a4c33795a12eefff4c3e5c311f40eae2cf3506b
---
 selfdrive/locationd/laikad.py            | 5 +++--
 selfdrive/locationd/test/test_laikad.py  | 8 ++++----
 selfdrive/test/process_replay/ref_commit | 2 +-
 3 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py
index e262407e02..67867e82b3 100755
--- a/selfdrive/locationd/laikad.py
+++ b/selfdrive/locationd/laikad.py
@@ -28,6 +28,7 @@ from system.swaglog import cloudlog
 MAX_TIME_GAP = 10
 EPHEMERIS_CACHE = 'LaikadEphemeris'
 CACHE_VERSION = 0.1
+POS_FIX_RESIDUAL_THRESHOLD = 100.0
 
 
 class Laikad:
@@ -89,9 +90,9 @@ class Laikad:
 
   def get_est_pos(self, t, processed_measurements):
     if self.last_pos_fix_t is None or abs(self.last_pos_fix_t - t) >= 2:
-      min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4
+      min_measurements = 6 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 5
       pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements)
-      if len(pos_fix) > 0:
+      if len(pos_fix) > 0 and np.median(np.abs(pos_fix_residual)) < POS_FIX_RESIDUAL_THRESHOLD:
         self.last_pos_fix = pos_fix[:3]
         self.last_pos_residual = pos_fix_residual
         self.last_pos_fix_t = t
diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py
index 3a7c073b55..c10a470d1a 100755
--- a/selfdrive/locationd/test/test_laikad.py
+++ b/selfdrive/locationd/test/test_laikad.py
@@ -138,7 +138,7 @@ class TestLaikad(unittest.TestCase):
     laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
     correct_msgs = verify_messages(self.logs, laikad)
 
-    correct_msgs_expected = 560
+    correct_msgs_expected = 555
     self.assertEqual(correct_msgs_expected, len(correct_msgs))
     self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
 
@@ -159,7 +159,7 @@ class TestLaikad(unittest.TestCase):
     # Disable fetch_orbits to test NAV only
     laikad.fetch_orbits = Mock()
     correct_msgs = verify_messages(self.logs, laikad)
-    correct_msgs_expected = 560
+    correct_msgs_expected = 559
     self.assertEqual(correct_msgs_expected, len(correct_msgs))
     self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
 
@@ -168,8 +168,8 @@ class TestLaikad(unittest.TestCase):
     downloader_mock.side_effect = IOError
     laikad = Laikad(auto_update=False)
     correct_msgs = verify_messages(self.logs, laikad)
-    self.assertEqual(256, len(correct_msgs))
-    self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
+    self.assertEqual(16, len(correct_msgs))
+    self.assertEqual(16, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
 
   def test_laika_get_orbits(self):
     laikad = Laikad(auto_update=False)
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 4908b86182..7fced5ad62 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-bd2ea158977f5c26658bed8ac683b72c2c592d06
\ No newline at end of file
+0da0928230d11dd4c76293b9e77b027eb4a1e291
\ No newline at end of file