mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-05 16:25:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

laikad: fixes to run on device (#24879)

Browse Source 
* Always run laikad on device!

* Update laika

* Update laika

* Fix gps week and time of week in msg

* Reset kalman filter if pos_fix or last_known_position

* put behind file

* move pr parsing into common file

Co-authored-by: Willem Melching <willem.melching@gmail.com>
This commit is contained in:
Gijs Koning
2022-06-16 17:45:08 +02:00
committed by GitHub
parent 2699bae778
commit dc98511b7a
8 changed files with 135 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files
laika_repo
+1 -1
Submodule laika_repo updated: 951ab080b9...44f048bc1f
release/files_common
+7 -2
View File
@@ -209,15 +209,20 @@ selfdrive/locationd/generated/ubx.h
selfdrive/locationd/generated/gps.cpp
selfdrive/locationd/generated/gps.h
selfdrive/locationd/laikad.py
selfdrive/locationd/laikad_helpers.py
selfdrive/locationd/locationd.cc
selfdrive/locationd/locationd.h
selfdrive/locationd/locationd.cc
selfdrive/locationd/paramsd.py
selfdrive/locationd/models/.gitignore
selfdrive/locationd/models/live_kf.py
selfdrive/locationd/models/car_kf.py
selfdrive/locationd/models/constants.py
selfdrive/locationd/models/gnss_kf.py
selfdrive/locationd/models/live_kf.py
selfdrive/locationd/models/live_kf.h
selfdrive/locationd/models/live_kf.cc
selfdrive/locationd/models/constants.py
selfdrive/locationd/models/gnss_helpers.py
selfdrive/locationd/calibrationd.py
selfdrive/locationd/laikad.py
+12 -91
View File
@@ -1,23 +1,22 @@
#!/usr/bin/env python3
import json
import time
from collections import defaultdict
from concurrent.futures import Future, ProcessPoolExecutor
from enum import IntEnum
from typing import List, Optional
import numpy as np
from collections import defaultdict
import sympy
from cereal import log, messaging
from common.params import Params, put_nonblocking
from laika import AstroDog
from laika.constants import EARTH_ROTATION_RATE, SECS_IN_HR, SECS_IN_MIN, SPEED_OF_LIGHT
from laika.constants import SECS_IN_HR, SECS_IN_MIN
from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId
from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox
from selfdrive.locationd.laikad_helpers import calc_pos_fix_gauss_newton, get_posfix_sympy_fun
from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
from selfdrive.locationd.models.gnss_kf import GNSSKalman
from selfdrive.locationd.models.gnss_kf import States as GStates
@@ -91,6 +90,8 @@ class Laikad:
dat = messaging.new_message("gnssMeasurements")
measurement_msg = log.LiveLocationKalman.Measurement.new_message
dat.gnssMeasurements = {
"gpsWeek": report.gpsWeek,
"gpsTimeOfWeek": report.rcvTow,
"positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid),
"velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid),
"positionFixECEF": measurement_msg(value=pos_fix, std=pos_fix_residual, valid=len(pos_fix) > 0),
@@ -115,7 +116,12 @@ class Laikad:
cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
elif not valid[2]:
cloudlog.error("Gnss kalman filter state is nan")
self.init_gnss_localizer(est_pos)
if est_pos is not None:
cloudlog.info(f"Reset kalman filter with {est_pos}")
self.init_gnss_localizer(est_pos)
else:
cloudlog.info("Could not reset kalman filter")
return
if len(measurements) > 0:
kf_add_observations(self.gnss_kf, t, measurements)
else:
@@ -130,8 +136,7 @@ class Laikad:
def init_gnss_localizer(self, est_pos):
x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial))
if est_pos is not None:
x_initial[GStates.ECEF_POS] = est_pos
x_initial[GStates.ECEF_POS] = est_pos
p_initial_diag[GStates.ECEF_POS] = 1000 ** 2
self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag)
@@ -235,90 +240,6 @@ def deserialize_hook(dct):
return dct
def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C1C', min_measurements=6):
'''
Calculates gps fix using gauss newton method
To solve the problem a minimal of 4 measurements are required.
If Glonass is included 5 are required to solve for the additional free variable.
returns:
0 -> list with positions
'''
if x0 is None:
x0 = [0, 0, 0, 0, 0]
n = len(measurements)
if n < min_measurements:
return [], []
Fx_pos = pr_residual(measurements, posfix_functions, signal=signal)
x = gauss_newton(Fx_pos, x0)
residual, _ = Fx_pos(x, weight=1.0)
return x.tolist(), residual.tolist()
def pr_residual(measurements, posfix_functions, signal='C1C'):
def Fx_pos(inp, weight=None):
vals, gradients = [], []
for meas in measurements:
pr = meas.observables[signal]
pr += meas.sat_clock_err * SPEED_OF_LIGHT
w = (1 / meas.observables_std[signal]) if weight is None else weight
val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, w)
vals.append(val)
gradients.append(gradient)
return np.asarray(vals), np.asarray(gradients)
return Fx_pos
def gauss_newton(fun, b, xtol=1e-8, max_n=25):
for _ in range(max_n):
# Compute function and jacobian on current estimate
r, J = fun(b)
# Update estimate
delta = np.linalg.pinv(J) @ r
b -= delta
# Check step size for stopping condition
if np.linalg.norm(delta) < xtol:
break
return b
def get_posfix_sympy_fun(constellation):
# Unknowns
x, y, z = sympy.Symbol('x'), sympy.Symbol('y'), sympy.Symbol('z')
bc = sympy.Symbol('bc')
bg = sympy.Symbol('bg')
var = [x, y, z, bc, bg]
# Knowns
pr = sympy.Symbol('pr')
sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z')
weight = sympy.Symbol('weight')
theta = EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT
val = sympy.sqrt(
(sat_x * sympy.cos(theta) + sat_y * sympy.sin(theta) - x) ** 2 +
(sat_y * sympy.cos(theta) - sat_x * sympy.sin(theta) - y) ** 2 +
(sat_z - z) ** 2
)
if constellation == ConstellationId.GLONASS:
res = weight * (val - (pr - bc - bg))
elif constellation == ConstellationId.GPS:
res = weight * (val - (pr - bc))
else:
raise NotImplementedError(f"Constellation {constellation} not supported")
res = [res] + [sympy.diff(res, v) for v in var]
return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res)
class EphemerisSourceType(IntEnum):
nav = 0
nasaUltraRapid = 1
selfdrive/locationd/laikad_helpers.py
+89
View File
@@ -0,0 +1,89 @@
import numpy as np
import sympy
from laika.constants import EARTH_ROTATION_RATE, SPEED_OF_LIGHT
from laika.helpers import ConstellationId
def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C1C', min_measurements=6):
'''
Calculates gps fix using gauss newton method
To solve the problem a minimal of 4 measurements are required.
If Glonass is included 5 are required to solve for the additional free variable.
returns:
0 -> list with positions
'''
if x0 is None:
x0 = [0, 0, 0, 0, 0]
n = len(measurements)
if n < min_measurements:
return [], []
Fx_pos = pr_residual(measurements, posfix_functions, signal=signal)
x = gauss_newton(Fx_pos, x0)
residual, _ = Fx_pos(x, weight=1.0)
return x.tolist(), residual.tolist()
def pr_residual(measurements, posfix_functions, signal='C1C'):
def Fx_pos(inp, weight=None):
vals, gradients = [], []
for meas in measurements:
pr = meas.observables[signal]
pr += meas.sat_clock_err * SPEED_OF_LIGHT
w = (1 / meas.observables_std[signal]) if weight is None else weight
val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, w)
vals.append(val)
gradients.append(gradient)
return np.asarray(vals), np.asarray(gradients)
return Fx_pos
def gauss_newton(fun, b, xtol=1e-8, max_n=25):
for _ in range(max_n):
# Compute function and jacobian on current estimate
r, J = fun(b)
# Update estimate
delta = np.linalg.pinv(J) @ r
b -= delta
# Check step size for stopping condition
if np.linalg.norm(delta) < xtol:
break
return b
def get_posfix_sympy_fun(constellation):
# Unknowns
x, y, z = sympy.Symbol('x'), sympy.Symbol('y'), sympy.Symbol('z')
bc = sympy.Symbol('bc')
bg = sympy.Symbol('bg')
var = [x, y, z, bc, bg]
# Knowns
pr = sympy.Symbol('pr')
sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z')
weight = sympy.Symbol('weight')
theta = EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT
val = sympy.sqrt(
(sat_x * sympy.cos(theta) + sat_y * sympy.sin(theta) - x) ** 2 +
(sat_y * sympy.cos(theta) - sat_x * sympy.sin(theta) - y) ** 2 +
(sat_z - z) ** 2
)
if constellation == ConstellationId.GLONASS:
res = weight * (val - (pr - bc - bg))
elif constellation == ConstellationId.GPS:
res = weight * (val - (pr - bc))
else:
raise NotImplementedError(f"Constellation {constellation} not supported")
res = [res] + [sympy.diff(res, v) for v in var]
return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res)
selfdrive/locationd/models/gnss_helpers.py
+21
View File
@@ -0,0 +1,21 @@
import numpy as np
def parse_prr(m):
from laika.raw_gnss import GNSSMeasurement
sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
m[GNSSMeasurement.SAT_VEL]))
R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
z_i = m[GNSSMeasurement.PRR]
return z_i, R_i, sat_pos_vel_i
def parse_pr(m):
from laika.raw_gnss import GNSSMeasurement
pseudorange = m[GNSSMeasurement.PR]
pseudorange_stdev = m[GNSSMeasurement.PR_STD]
sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
np.array([m[GNSSMeasurement.GLONASS_FREQ]])))
z_i = np.atleast_1d(pseudorange)
R_i = np.atleast_2d(pseudorange_stdev**2)
return z_i, R_i, sat_pos_freq_i
selfdrive/locationd/models/gnss_kf.py
+1 -1
View File
@@ -7,7 +7,7 @@ import sympy as sp
from rednose.helpers.ekf_sym import EKF_sym, gen_code
from selfdrive.locationd.models.constants import ObservationKind
from selfdrive.locationd.models.loc_kf import parse_pr, parse_prr
from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr
class States():
selfdrive/locationd/models/loc_kf.py
+3 -22
View File
@@ -5,34 +5,15 @@ import sys
import numpy as np
import sympy as sp
from selfdrive.locationd.models.constants import ObservationKind
from rednose.helpers.ekf_sym import EKF_sym, gen_code
from rednose.helpers.lst_sq_computer import LstSqComputer
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
from selfdrive.locationd.models.constants import ObservationKind
from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
def parse_prr(m):
from laika.raw_gnss import GNSSMeasurement
sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
m[GNSSMeasurement.SAT_VEL]))
R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
z_i = m[GNSSMeasurement.PRR]
return z_i, R_i, sat_pos_vel_i
def parse_pr(m):
from laika.raw_gnss import GNSSMeasurement
pseudorange = m[GNSSMeasurement.PR]
pseudorange_stdev = m[GNSSMeasurement.PR_STD]
sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
np.array([m[GNSSMeasurement.GLONASS_FREQ]])))
z_i = np.atleast_1d(pseudorange)
R_i = np.atleast_2d(pseudorange_stdev**2)
return z_i, R_i, sat_pos_freq_i
class States():
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
ECEF_ORIENTATION = slice(3, 7) # quat for orientation of phone in ecef
selfdrive/manager/process_config.py
+1
View File
@@ -57,6 +57,7 @@ procs = [
# Experimental
PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=os.path.isfile("/persist/comma/use-quectel-rawgps")),
PythonProcess("laikad", "selfdrive.locationd.laikad", enabled=os.path.isfile("/persist/comma/use-laikad")),
]
managed_processes = {p.name: p for p in procs}