sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 64 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>
pull/214/head
Gijs Koning 3 years ago committed by GitHub
parent 2699bae778
commit dc98511b7a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 135 additions and 117 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      laika_repo
  2. 9
      release/files_common
  3. 103
      selfdrive/locationd/laikad.py
  4. 89
      selfdrive/locationd/laikad_helpers.py
  5. 21
      selfdrive/locationd/models/gnss_helpers.py
  6. 2
      selfdrive/locationd/models/gnss_kf.py
  7. 25
      selfdrive/locationd/models/loc_kf.py
  8. 1
      selfdrive/manager/process_config.py

2
laika_repo

@ -1 +1 @@
Subproject commit 951ab080b998ee3edde6229654d1a4cb63cda6a9
Subproject commit 44f048bc1f58ae9e28dfdeb98e40aea3e0f2b699

9
release/files_common
Unescape 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

103
selfdrive/locationd/laikad.py
Unescape 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

89
selfdrive/locationd/laikad_helpers.py
Unescape 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)

21
selfdrive/locationd/models/gnss_helpers.py
Unescape 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

2
selfdrive/locationd/models/gnss_kf.py
Unescape 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():

25
selfdrive/locationd/models/loc_kf.py
Unescape View File

@ -5,33 +5,14 @@ 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
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
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)
class States():
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters

1
selfdrive/manager/process_config.py
Unescape 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}

Write Preview
Loading…
Cancel
Save