diff --git a/laika_repo b/laika_repo index 278b44ba8c..b740b71c82 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 278b44ba8c2dec28adc5b30cce13dabc195f15fc +Subproject commit b740b71c82a748e3520b1599487d9a7aaf728670 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 2eb1e94526..10c5e1b7e5 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -97,19 +97,25 @@ class Laikad: def get_lsq_fix(self, t, measurements): if self.last_fix_t is None or abs(self.last_fix_t - t) > 0: - min_measurements = 7 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 6 - position_solution, pr_residuals = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements) + min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 4 + position_solution, pr_residuals, pos_std = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements) if len(position_solution) < 3: return None position_estimate = position_solution[:3] - #TODO median abs residual is decent estimate of std, can be improved with measurements stds and/or DOP - position_std = np.median(np.abs(pr_residuals)) * np.ones(3) - velocity_solution, prr_residuals = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements) + + position_std_residual = np.median(np.abs(pr_residuals)) + position_std = np.median(np.abs(pos_std))/10 + position_std = max(position_std_residual, position_std) * np.ones(3) + + velocity_solution, prr_residuals, vel_std = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements) if len(velocity_solution) < 3: return None - velocity_estimate = velocity_solution[:3] - velocity_std = np.median(np.abs(prr_residuals)) * np.ones(3) + + velocity_std_residual = np.median(np.abs(prr_residuals)) + velocity_std = np.median(np.abs(vel_std))/10 + velocity_std = max(velocity_std, velocity_std_residual) * np.ones(3) + return position_estimate, position_std, velocity_estimate, velocity_std def is_good_report(self, gnss_msg): diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index d89f521228..10ac7790b6 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -160,7 +160,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 = 554 + 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])) @@ -180,7 +180,7 @@ class TestLaikad(unittest.TestCase): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV) # Disable fetch_orbits to test NAV only correct_msgs = verify_messages(self.logs, laikad) - correct_msgs_expected = 554 + 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])) @@ -195,8 +195,8 @@ class TestLaikad(unittest.TestCase): downloader_mock.side_effect = DownloadFailed laikad = Laikad(auto_update=False) correct_msgs = verify_messages(self.logs, laikad) - self.assertEqual(0, len(correct_msgs)) - self.assertEqual(0, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + self.assertEqual(255, len(correct_msgs)) + self.assertEqual(255, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) def test_laika_get_orbits(self): laikad = Laikad(auto_update=False) @@ -282,7 +282,7 @@ class TestLaikad(unittest.TestCase): gm = msg.gnssMeasurements if len(gm.correctedMeasurements) != 0 and gm.positionECEF.valid: cnt += 1 - self.assertEqual(cnt, 554) + self.assertEqual(cnt, 559) def dict_has_values(self, dct): self.assertGreater(len(dct), 0) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 9991154b9a..b44c9c2f46 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -772f30de36fc7f8421dabb779cc02f45eb83d7bb +dfa8e947c4ef76a9d89974a434e94a078e1ccc6a \ No newline at end of file