mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-09 07:32:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Laikad: Use filter for correcting measurements (#24824)

Browse Source 
* Update laikad.

* Update log error
old-commit-hash: 724b322909152d58f626b6e76fd34d7d03d67250
This commit is contained in:
Gijs Koning
2022-06-13 11:45:35 +02:00
committed by GitHub
parent 2eca17d464
commit 11033eba53
2 changed files with 43 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
SConstruct
+1 -1
View File
@@ -359,6 +359,7 @@ Export('cereal', 'messaging', 'visionipc')
rednose_config = {
'generated_folder': '#selfdrive/locationd/models/generated',
'to_build': {
'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']),
'car': ('#selfdrive/locationd/models/car_kf.py', True, []),
},
@@ -366,7 +367,6 @@ rednose_config = {
if arch != "larch64":
rednose_config['to_build'].update({
'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []),
'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []),
'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []),
selfdrive/locationd/laikad.py
+42 -25
View File
@@ -6,6 +6,8 @@ from typing import List, Optional
import numpy as np
from collections import defaultdict
from numpy.linalg import linalg
from cereal import log, messaging
from laika import AstroDog
from laika.constants import SECS_IN_MIN
@@ -37,24 +39,33 @@ class Laikad:
latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow)
self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block)
new_meas = read_raw_ublox(report)
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 measurements can't be processed.
corrected_measurements = []
if len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000:
corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog)
processed_measurements = process_measurements(new_meas, self.astro_dog)
pos_fix = calc_pos_fix(processed_measurements, min_measurements=4)
t = ublox_mono_time * 1e-9
kf_pos_std = None
if all(self.kf_valid(t)):
self.gnss_kf.predict(t)
kf_pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal()))
# If localizer is valid use its position to correct measurements
if kf_pos_std is not None and linalg.norm(kf_pos_std) < 100:
est_pos = self.gnss_kf.x[GStates.ECEF_POS]
elif len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000:
est_pos = pos_fix[0][:3]
else:
est_pos = None
corrected_measurements = []
if est_pos is not None:
corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog)
self.update_localizer(pos_fix, t, corrected_measurements)
localizer_valid = self.localizer_valid(t)
kf_valid = all(self.kf_valid(t))
ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist()
pos_std = self.gnss_kf.P[GStates.ECEF_POS].flatten().tolist()
vel_std = self.gnss_kf.P[GStates.ECEF_VELOCITY].flatten().tolist()
pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist()
vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist()
bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std)
@@ -62,9 +73,9 @@ class Laikad:
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
dat.gnssMeasurements = {
"positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=localizer_valid),
"velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=localizer_valid),
"bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=localizer_valid),
"positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid),
"velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid),
"bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=kf_valid),
"ubloxMonoTime": ublox_mono_time,
"correctedMeasurements": meas_msgs
}
@@ -77,18 +88,21 @@ class Laikad:
def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]):
# Check time and outputs are valid
if not self.localizer_valid(t):
# A position fix is needed when resetting the kalman filter.
valid = self.kf_valid(t)
if not all(valid):
if not valid[0]:
cloudlog.info("Init gnss kalman filter")
elif not valid[1]:
cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
elif not valid[2]:
cloudlog.error("Gnss kalman filter state is nan")
else:
cloudlog.error("Gnss kalman std too far")
if len(pos_fix) == 0:
cloudlog.error("Position fix not available when resetting kalman filter")
return
post_est = pos_fix[0][:3].tolist()
filter_time = self.gnss_kf.filter.filter_time
if filter_time is None:
cloudlog.info("Init gnss kalman filter")
elif abs(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")
self.init_gnss_localizer(post_est)
if len(measurements) > 0:
kf_add_observations(self.gnss_kf, t, measurements)
@@ -96,9 +110,12 @@ class Laikad:
# Ensure gnss filter is updated even with no new measurements
self.gnss_kf.predict(t)
def localizer_valid(self, t: float):
def kf_valid(self, t: float):
filter_time = self.gnss_kf.filter.filter_time
return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))
return [filter_time is not None,
filter_time is not None and abs(t - filter_time) < MAX_TIME_GAP,
all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])),
linalg.norm(self.gnss_kf.P[GStates.ECEF_POS]) < 1e5]
def init_gnss_localizer(self, est_pos):
x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial))