Add ephemeris parsing to laikad (#24642)

* Always send valid messages

* Use ephemeris and move astrodog in laikad

* improve test

* Always correct measurements

* Cleanup

* Fix test

* Update laika
old-commit-hash: a51aaaf197
taco
Gijs Koning 3 years ago committed by GitHub
parent afbdbc7c39
commit 8976820211
  1. 2
      laika_repo
  2. 51
      selfdrive/locationd/laikad.py
  3. 32
      selfdrive/locationd/test/test_laikad.py

@ -1 +1 @@
Subproject commit f5f76d28b4827c3fb706d542729651ceef6c06bd
Subproject commit 231eafbf659309b85acb5b575b7f898e7a4f196e

@ -4,8 +4,11 @@ from typing import List
import numpy as np
from collections import defaultdict
from scipy import linalg
from cereal import log, messaging
from laika import AstroDog
from laika.ephemeris import convert_ublox_ephem
from laika.helpers import ConstellationId
from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox
from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
@ -19,28 +22,25 @@ MAX_TIME_GAP = 10
class Laikad:
def __init__(self):
def __init__(self, use_internet):
self.astro_dog = AstroDog(use_internet=use_internet)
self.gnss_kf = GNSSKalman(GENERATED_DIR)
def process_ublox_msg(self, ublox_msg, dog: AstroDog, ublox_mono_time: int):
def process_ublox_msg(self, ublox_msg, ublox_mono_time: int):
if ublox_msg.which == 'measurementReport':
report = ublox_msg.measurementReport
new_meas = read_raw_ublox(report)
measurements = process_measurements(new_meas, dog)
pos_fix = calc_pos_fix(measurements)
measurements = process_measurements(new_meas, self.astro_dog)
pos_fix = calc_pos_fix(measurements, min_measurements=4)
# To get a position fix a minimum of 5 measurements are needed.
# Each report can contain less and some measurement can't be processed.
if len(pos_fix) > 0:
measurements = correct_measurements(measurements, pos_fix[0][:3], dog)
meas_msgs = [create_measurement_msg(m) for m in measurements]
# Each report can contain less and some measurements can't be processed.
corrected_measurements = []
if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100:
corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog)
t = ublox_mono_time * 1e-9
self.update_localizer(pos_fix, t, measurements)
self.update_localizer(pos_fix, t, corrected_measurements)
localizer_valid = self.localizer_valid(t)
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist()
@ -49,6 +49,8 @@ class Laikad:
bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std)
meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.GnssMeasurements.Measurement.new_message
dat.gnssMeasurements = {
@ -59,6 +61,11 @@ class Laikad:
"correctedMeasurements": meas_msgs
}
return dat
elif ublox_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(ublox_msg.ephemeris)
self.astro_dog.add_ephem(ephem, self.astro_dog.orbits)
# elif ublox_msg.which == 'ionoData':
# todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]):
# Check time and outputs are valid
@ -67,9 +74,10 @@ class Laikad:
if len(pos_fix) == 0:
return
post_est = pos_fix[0][:3].tolist()
if self.gnss_kf.filter.filter_time is None:
filter_time = self.gnss_kf.filter.filter_time
if filter_time is None:
cloudlog.info("Init gnss kalman filter")
elif (self.gnss_kf.filter.filter_time - t) > MAX_TIME_GAP:
elif (t - filter_time) > MAX_TIME_GAP:
cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
else:
cloudlog.error("Gnss kalman filter state is nan")
@ -82,7 +90,7 @@ class Laikad:
def localizer_valid(self, t: float):
filter_time = self.gnss_kf.filter.filter_time
return filter_time is not None and (filter_time - t) < MAX_TIME_GAP and \
return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and \
all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))
def init_gnss_localizer(self, est_pos):
@ -97,11 +105,10 @@ def create_measurement_msg(meas: GNSSMeasurement):
c = log.GnssMeasurements.CorrectedMeasurement.new_message()
c.constellationId = meas.constellation_id.value
c.svId = meas.sv_id
observables = meas.observables_final
c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0
c.pseudorange = float(observables['C1C'])
c.pseudorange = float(meas.observables_final['C1C'])
c.pseudorangeStd = float(meas.observables_std['C1C'])
c.pseudorangeRate = float(observables['D1C'])
c.pseudorangeRate = float(meas.observables_final['D1C'])
c.pseudorangeRateStd = float(meas.observables_std['D1C'])
c.satPos = meas.sat_pos_final.tolist()
c.satVel = meas.sat_vel.tolist()
@ -134,19 +141,17 @@ def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std):
def main():
dog = AstroDog(use_internet=True)
sm = messaging.SubMaster(['ubloxGnss'])
pm = messaging.PubMaster(['gnssMeasurements'])
laikad = Laikad()
laikad = Laikad(use_internet=True)
while True:
sm.update()
# Todo if no internet available use latest ephemeris
if sm.updated['ubloxGnss']:
ublox_msg = sm['ubloxGnss']
msg = laikad.process_ublox_msg(ublox_msg, dog, sm.logMonoTime['ubloxGnss'])
msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss'])
if msg is not None:
pm.send('gnssMeasurements', msg)

@ -2,7 +2,6 @@
import unittest
from datetime import datetime
from laika import AstroDog
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId
from laika.raw_gnss import GNSSMeasurement
@ -18,11 +17,11 @@ def get_log(segs=range(0)):
return [m for m in logs if m.which() == 'ubloxGnss']
def verify_messages(lr, dog, laikad):
def process_msgs(lr, laikad: Laikad):
good_msgs = []
for m in lr:
msg = laikad.process_ublox_msg(m.ubloxGnss, dog, m.logMonoTime)
if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0:
msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime)
if msg is not None:
good_msgs.append(msg)
return good_msgs
@ -44,14 +43,31 @@ class TestLaikad(unittest.TestCase):
def test_laika_online(self):
# Set to offline forces to use ephemeris messages
dog = AstroDog(use_internet=True)
laikad = Laikad()
correct_msgs = verify_messages(self.logs, dog, laikad)
laikad = Laikad(use_internet=True)
msgs = process_msgs(self.logs, laikad)
correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0]
correct_msgs_expected = 560
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]))
def test_laika_offline(self):
# Set to offline forces to use ephemeris messages
laikad = Laikad(use_internet=False)
msgs = process_msgs(self.logs, laikad)
correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0]
self.assertEqual(256, len(correct_msgs))
self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
def test_laika_offline_ephem_at_start(self):
# Test offline but process ephemeris msgs of segment first
laikad = Laikad(use_internet=False)
ephemeris_logs = [m for m in self.logs if m.ubloxGnss.which() == 'ephemeris']
msgs = process_msgs(ephemeris_logs+self.logs, laikad)
correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0]
self.assertEqual(554, len(correct_msgs))
self.assertGreaterEqual(554, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
if __name__ == "__main__":
unittest.main()

Loading…
Cancel
Save