locationd and paramsd using cython version of ekfsym (#20610)
* Locationd live_kf using c++ kalman filter * use both cpp and python live_kf to compare * Locationd using ekfsym cpp * Paramsd using c++ ekf_sym * Other building method * Cleanup * cleanup * Single sconscript for rednose and locationd/models * CI * CI * CI fix * renamed scons config * Fix lib loading * bump rednose * update cpu usage testalbatross
Joost Wooning
GitHub
parent
5bb4879b87
commit
e6a8157916
23
SConstruct
23
SConstruct
|
|
@ -356,6 +356,28 @@ else:
|
|||
|
||||
Export('common', 'gpucommon', 'visionipc')
|
||||
|
||||
# Build rednose library and ekf models
|
||||
|
||||
rednose_config = {
|
||||
'generated_folder': '#selfdrive/locationd/models/generated',
|
||||
'to_build': {
|
||||
'live': ('#selfdrive/locationd/models/live_kf.py', True),
|
||||
'car': ('#selfdrive/locationd/models/car_kf.py', True),
|
||||
},
|
||||
}
|
||||
|
||||
if arch != "aarch64":
|
||||
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),
|
||||
'feature_handler_5': ('#rednose/helpers/feature_handler.py', False),
|
||||
'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True),
|
||||
})
|
||||
|
||||
Export('rednose_config')
|
||||
SConscript(['rednose/SConscript'])
|
||||
|
||||
# Build openpilot
|
||||
|
||||
|
|
@ -384,7 +406,6 @@ SConscript(['selfdrive/clocksd/SConscript'])
|
|||
SConscript(['selfdrive/loggerd/SConscript'])
|
||||
|
||||
SConscript(['selfdrive/locationd/SConscript'])
|
||||
SConscript(['selfdrive/locationd/models/SConscript'])
|
||||
SConscript(['selfdrive/sensord/SConscript'])
|
||||
SConscript(['selfdrive/ui/SConscript'])
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit 946a0345740abe46241b2168262c01d7239d642e
|
||||
Subproject commit fbf65ab4aeec9a6891eaf22ddf430e592ab67aa4
|
||||
|
|
@ -288,7 +288,6 @@ selfdrive/locationd/ublox_msg.h
|
|||
selfdrive/locationd/locationd.py
|
||||
selfdrive/locationd/paramsd.py
|
||||
selfdrive/locationd/models/.gitignore
|
||||
selfdrive/locationd/models/SConscript
|
||||
selfdrive/locationd/models/live_kf.py
|
||||
selfdrive/locationd/models/car_kf.py
|
||||
selfdrive/locationd/models/constants.py
|
||||
|
|
|
|||
|
|
@ -1,37 +0,0 @@
|
|||
Import('env', 'arch')
|
||||
|
||||
templates = Glob('#rednose/templates/*')
|
||||
|
||||
sympy_helpers = "#rednose/helpers/sympy_helpers.py"
|
||||
ekf_sym = "#rednose/helpers/ekf_sym.py"
|
||||
|
||||
to_build = {
|
||||
'live': ('live_kf.py', 'generated'),
|
||||
'car': ('car_kf.py', 'generated'),
|
||||
}
|
||||
|
||||
if arch != "aarch64":
|
||||
to_build.update({
|
||||
'gnss': ('gnss_kf.py', 'generated'),
|
||||
'loc_4': ('loc_kf.py', 'generated'),
|
||||
'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', 'generated'),
|
||||
'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', 'generated'),
|
||||
'feature_handler_5': ('#rednose/helpers/feature_handler.py', 'generated'),
|
||||
'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', 'generated'),
|
||||
})
|
||||
|
||||
found = {}
|
||||
|
||||
for target, (command, generated_folder) in to_build.items():
|
||||
if File(command).exists():
|
||||
found[target] = (command, generated_folder)
|
||||
|
||||
for target, (command, generated_folder) in found.items():
|
||||
target_files = File([f'{generated_folder}/{target}.cpp', f'{generated_folder}/{target}.h'])
|
||||
command_file = File(command)
|
||||
|
||||
env.Command(target_files,
|
||||
[templates, command_file, sympy_helpers, ekf_sym],
|
||||
command_file.get_abspath() + " " + target + " " + Dir(generated_folder).get_abspath())
|
||||
|
||||
env.SharedLibrary(f'{generated_folder}/' + target, target_files[0])
|
||||
|
|
@ -6,13 +6,18 @@ from typing import Any, Dict
|
|||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
from rednose import KalmanFilter
|
||||
from rednose.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from selfdrive.locationd.models.constants import ObservationKind
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
i = 0
|
||||
from rednose.helpers.kalmanfilter import KalmanFilter
|
||||
|
||||
if __name__ == '__main__': # Generating sympy
|
||||
from rednose.helpers.ekf_sym import gen_code
|
||||
else:
|
||||
from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module, import-error
|
||||
|
||||
|
||||
i = 0
|
||||
|
||||
def _slice(n):
|
||||
global i
|
||||
|
|
@ -149,7 +154,8 @@ class CarKalman(KalmanFilter):
|
|||
x_init[States.ANGLE_OFFSET] = angle_offset
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
|
||||
global_var_names = [x.name for x in self.global_vars] # pylint: disable=no-member
|
||||
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=global_var_names, logger=cloudlog)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -1,14 +1,17 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.locationd.models.constants import ObservationKind
|
||||
from rednose.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
|
||||
|
||||
if __name__ == '__main__': # Generating sympy
|
||||
import sympy as sp
|
||||
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
|
||||
from rednose.helpers.ekf_sym import gen_code
|
||||
else:
|
||||
from rednose.helpers.ekf_sym_pyx import EKF_sym # pylint: disable=no-name-in-module, import-error
|
||||
|
||||
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
|
|
@ -215,7 +218,7 @@ class LiveKalman():
|
|||
|
||||
@property
|
||||
def t(self):
|
||||
return self.filter.filter_time
|
||||
return self.filter.get_filter_time()
|
||||
|
||||
@property
|
||||
def P(self):
|
||||
|
|
@ -249,10 +252,7 @@ class LiveKalman():
|
|||
R = R[None]
|
||||
r = self.filter.predict_and_update_batch(t, kind, meas, R)
|
||||
|
||||
# Normalize quats
|
||||
quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
|
||||
self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
|
||||
|
||||
self.filter.normalize_state(States.ECEF_ORIENTATION.start, States.ECEF_ORIENTATION.stop)
|
||||
return r
|
||||
|
||||
def get_R(self, kind, n):
|
||||
|
|
|
|||
|
|
@ -392,7 +392,7 @@ class LocKalman():
|
|||
|
||||
@property
|
||||
def t(self):
|
||||
return self.filter.filter_time
|
||||
return self.filter.get_filter_time()
|
||||
|
||||
@property
|
||||
def P(self):
|
||||
|
|
@ -449,15 +449,15 @@ class LocKalman():
|
|||
else:
|
||||
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
|
||||
# Normalize quats
|
||||
quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
|
||||
quat_norm = np.linalg.norm(self.filter.state()[3:7])
|
||||
# Should not continue if the quats behave this weirdly
|
||||
if not 0.1 < quat_norm < 10:
|
||||
raise RuntimeError("Sir! The filter's gone all wobbly!")
|
||||
self.filter.x[3:7, 0] = self.filter.x[3:7, 0] / quat_norm
|
||||
self.filter.normalize_state(3, 7)
|
||||
for i in range(self.N):
|
||||
d1 = self.dim_main
|
||||
d3 = self.dim_augment
|
||||
self.filter.x[d1 + d3 * i + 3:d1 + d3 * i + 7] /= np.linalg.norm(self.filter.x[d1 + i * d3 + 3:d1 + i * d3 + 7, 0])
|
||||
self.filter.normalize_state(d1 + d3 * i + 3, d1 + d3 * i + 7)
|
||||
return r
|
||||
|
||||
def get_R(self, kind, n):
|
||||
|
|
@ -528,7 +528,7 @@ class LocKalman():
|
|||
poses = self.x[self.dim_main:].reshape((-1, 7))
|
||||
times = tracks.reshape((len(tracks), self.N + 1, 4))[:, :, 0]
|
||||
good_counter = 0
|
||||
if times.any() and np.allclose(times[0, :-1], self.filter.augment_times, rtol=1e-6):
|
||||
if times.any() and np.allclose(times[0, :-1], self.filter.get_augment_times(), rtol=1e-6):
|
||||
for i, track in enumerate(tracks):
|
||||
img_positions = track.reshape((self.N + 1, 4))[:, 2:]
|
||||
|
||||
|
|
|
|||
|
|
@ -16,12 +16,12 @@ class ParamsLearner:
|
|||
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
|
||||
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset)
|
||||
|
||||
self.kf.filter.set_mass(CP.mass) # pylint: disable=no-member
|
||||
self.kf.filter.set_rotational_inertia(CP.rotationalInertia) # pylint: disable=no-member
|
||||
self.kf.filter.set_center_to_front(CP.centerToFront) # pylint: disable=no-member
|
||||
self.kf.filter.set_center_to_rear(CP.wheelbase - CP.centerToFront) # pylint: disable=no-member
|
||||
self.kf.filter.set_stiffness_front(CP.tireStiffnessFront) # pylint: disable=no-member
|
||||
self.kf.filter.set_stiffness_rear(CP.tireStiffnessRear) # pylint: disable=no-member
|
||||
self.kf.filter.set_global("mass", CP.mass)
|
||||
self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
|
||||
self.kf.filter.set_global("center_to_front", CP.centerToFront)
|
||||
self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront)
|
||||
self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
|
||||
self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
|
||||
|
||||
self.active = False
|
||||
|
||||
|
|
@ -42,9 +42,9 @@ class ParamsLearner:
|
|||
if msg.inputsOK and msg.posenetOK and yaw_rate_valid:
|
||||
self.kf.predict_and_observe(t,
|
||||
ObservationKind.ROAD_FRAME_YAW_RATE,
|
||||
np.array([[[-yaw_rate]]]),
|
||||
np.array([[-yaw_rate]]),
|
||||
np.array([np.atleast_2d(yaw_rate_std**2)]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
|
||||
|
||||
elif which == 'carState':
|
||||
self.steering_angle = msg.steeringAngleDeg
|
||||
|
|
@ -55,12 +55,12 @@ class ParamsLearner:
|
|||
self.active = self.speed > 5 and in_linear_region
|
||||
|
||||
if self.active:
|
||||
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngleDeg)]]]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
|
||||
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]]))
|
||||
|
||||
if not self.active:
|
||||
# Reset time when stopped so uncertainty doesn't grow
|
||||
self.kf.filter.filter_time = t
|
||||
self.kf.filter.set_filter_time(t)
|
||||
self.kf.filter.reset_rewind()
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -16,10 +16,10 @@ from tools.lib.logreader import LogReader
|
|||
PROCS = [
|
||||
("selfdrive.controls.controlsd", 47.0),
|
||||
("./loggerd", 45.0),
|
||||
("selfdrive.locationd.locationd", 35.0),
|
||||
("selfdrive.locationd.locationd", 32.8),
|
||||
("selfdrive.controls.plannerd", 20.0),
|
||||
("./_ui", 15.0),
|
||||
("selfdrive.locationd.paramsd", 12.0),
|
||||
("selfdrive.locationd.paramsd", 9.1),
|
||||
("./camerad", 7.07),
|
||||
("./_sensord", 6.17),
|
||||
("selfdrive.controls.radard", 5.67),
|
||||
|
|
|
|||
Loading…
Reference in New Issue