diff --git a/SConstruct b/SConstruct index fc1ddfd66b..9010217e4b 100644 --- a/SConstruct +++ b/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']) diff --git a/rednose_repo b/rednose_repo index 946a034574..fbf65ab4ae 160000 --- a/rednose_repo +++ b/rednose_repo @@ -1 +1 @@ -Subproject commit 946a0345740abe46241b2168262c01d7239d642e +Subproject commit fbf65ab4aeec9a6891eaf22ddf430e592ab67aa4 diff --git a/release/files_common b/release/files_common index bcaa1cc43b..ca1cf34b36 100644 --- a/release/files_common +++ b/release/files_common @@ -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 diff --git a/selfdrive/locationd/models/SConscript b/selfdrive/locationd/models/SConscript deleted file mode 100644 index 0a197ffda8..0000000000 --- a/selfdrive/locationd/models/SConscript +++ /dev/null @@ -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]) diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 6c5357f2dd..1efd955837 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -6,14 +6,19 @@ 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 s = slice(i, i + n) @@ -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__": diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index 3d8a1f65d1..9e194b6fcd 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -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): diff --git a/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py index 4e05c4c703..08ce672b57 100755 --- a/selfdrive/locationd/models/loc_kf.py +++ b/selfdrive/locationd/models/loc_kf.py @@ -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:] diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 96b3c26dc2..ba39bc0d70 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -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() diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 265e391ca7..4ebb772a56 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -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),