From 7d0ed178dc6f41ed5d16f408e378d0ce107075d2 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 26 Feb 2020 16:19:02 -0800 Subject: [PATCH] Kalman filter to identify vehicle parameters (#1123) * full vehicle model simulator * Add vehicle model * Model compiles * Close enough * Simulation works * Add fast angle offset * Tune fast angle offset learner * Create live service for paramsd * Better clamping * Fix rotation matrix * Cleanup before merge * move debug script to debug/internal * revert plannerd change * switch vehicle model to corolla * fix valid flag * Bigger stiffness range * Lower process noise on steer ratio * Tuning * Decimation * No maha tests old-commit-hash: c9ecab2139cd0b15bba49dcc30e07c7b0f33c6ef --- release/files_common | 2 + selfdrive/debug/internal/test_paramsd.py | 129 ++++++++++++ selfdrive/locationd/kalman/SConscript | 1 + .../locationd/kalman/helpers/__init__.py | 60 ++++-- selfdrive/locationd/kalman/helpers/ekf_sym.py | 20 +- selfdrive/locationd/kalman/models/car_kf.py | 198 ++++++++++++++++++ selfdrive/locationd/paramsd.py | 109 ++++++++++ 7 files changed, 495 insertions(+), 24 deletions(-) create mode 100755 selfdrive/debug/internal/test_paramsd.py create mode 100755 selfdrive/locationd/kalman/models/car_kf.py create mode 100755 selfdrive/locationd/paramsd.py diff --git a/release/files_common b/release/files_common index 825b7912c4..b89477a870 100644 --- a/release/files_common +++ b/release/files_common @@ -244,6 +244,7 @@ selfdrive/locationd/ublox_msg.h selfdrive/locationd/test/*.py selfdrive/locationd/locationd.py +selfdrive/locationd/paramsd.py selfdrive/locationd/kalman/.gitignore selfdrive/locationd/kalman/__init__.py selfdrive/locationd/kalman/README.md @@ -251,6 +252,7 @@ selfdrive/locationd/kalman/SConscript selfdrive/locationd/kalman/templates/* selfdrive/locationd/kalman/helpers/* selfdrive/locationd/kalman/models/live_kf.py +selfdrive/locationd/kalman/models/car_kf.py selfdrive/locationd/calibrationd.py selfdrive/locationd/calibration_helpers.py diff --git a/selfdrive/debug/internal/test_paramsd.py b/selfdrive/debug/internal/test_paramsd.py new file mode 100755 index 0000000000..356edfaef6 --- /dev/null +++ b/selfdrive/debug/internal/test_paramsd.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +import numpy as np +import math +from tqdm import tqdm + +import seaborn as sns +import matplotlib.pyplot as plt + + +from selfdrive.car.honda.interface import CarInterface +from selfdrive.car.honda.values import CAR +from selfdrive.controls.lib.vehicle_model import VehicleModel, create_dyn_state_matrices +from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States + +T_SIM = 5 * 60 # s +DT = 0.01 + + +CP = CarInterface.get_params(CAR.CIVIC) +VM = VehicleModel(CP) + +x, y = 0, 0 # m, m +psi = math.radians(0) # rad + +# The state is x = [v, r]^T +# with v lateral speed [m/s], and r rotational speed [rad/s] +state = np.array([[0.0], [0.0]]) + + +ts = np.arange(0, T_SIM, DT) +speeds = 10 * np.sin(2 * np.pi * ts / 200.) + 25 + +angle_offsets = math.radians(1.0) * np.ones_like(ts) +angle_offsets[ts > 60] = 0 +steering_angles = np.radians(5 * np.cos(2 * np.pi * ts / 100.)) + +xs = [] +ys = [] +psis = [] +yaw_rates = [] +speed_ys = [] + + +kf_states = [] +kf_ps = [] + +kf = CarKalman() + +for i, t in tqdm(list(enumerate(ts))): + u = speeds[i] + sa = steering_angles[i] + ao = angle_offsets[i] + + A, B = create_dyn_state_matrices(u, VM) + + state += DT * (A.dot(state) + B.dot(sa + ao)) + + x += u * math.cos(psi) * DT + y += (float(state[0]) * math.sin(psi) + u * math.sin(psi)) * DT + psi += float(state[1]) * DT + + kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [float(state[1])]) + kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[u, float(state[0])]]) + kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, [sa]) + kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, [0]) + kf.predict(t) + + speed_ys.append(float(state[0])) + yaw_rates.append(float(state[1])) + kf_states.append(kf.x.copy()) + kf_ps.append(kf.P.copy()) + + xs.append(x) + ys.append(y) + psis.append(psi) + + +xs = np.asarray(xs) +ys = np.asarray(ys) +psis = np.asarray(psis) +speed_ys = np.asarray(speed_ys) +kf_states = np.asarray(kf_states) +kf_ps = np.asarray(kf_ps) + + +palette = sns.color_palette() + +def plot_with_bands(ts, state, label, ax, idx=1, converter=None): + mean = kf_states[:, state].flatten() + stds = np.sqrt(kf_ps[:, state, state].flatten()) + + if converter is not None: + mean = converter(mean) + stds = converter(stds) + + sns.lineplot(ts, mean, label=label, ax=ax) + ax.fill_between(ts, mean - stds, mean + stds, alpha=.2, color=palette[idx]) + + +print(kf.x) + +sns.set_context("paper") +f, axes = plt.subplots(6, 1) + +sns.lineplot(ts, np.degrees(steering_angles), label='Steering Angle [deg]', ax=axes[0]) +plot_with_bands(ts, States.STEER_ANGLE, 'Steering Angle kf [deg]', axes[0], converter=np.degrees) + +sns.lineplot(ts, np.degrees(yaw_rates), label='Yaw Rate [deg]', ax=axes[1]) +plot_with_bands(ts, States.YAW_RATE, 'Yaw Rate kf [deg]', axes[1], converter=np.degrees) + +sns.lineplot(ts, np.ones_like(ts) * VM.sR, label='Steer ratio [-]', ax=axes[2]) +plot_with_bands(ts, States.STEER_RATIO, 'Steer ratio kf [-]', axes[2]) +axes[2].set_ylim([10, 20]) + + +sns.lineplot(ts, np.ones_like(ts), label='Tire stiffness[-]', ax=axes[3]) +plot_with_bands(ts, States.STIFFNESS, 'Tire stiffness kf [-]', axes[3]) +axes[3].set_ylim([0.8, 1.2]) + + +sns.lineplot(ts, np.degrees(angle_offsets), label='Angle offset [deg]', ax=axes[4]) +plot_with_bands(ts, States.ANGLE_OFFSET, 'Angle offset kf deg', axes[4], converter=np.degrees) +plot_with_bands(ts, States.ANGLE_OFFSET_FAST, 'Fast Angle offset kf deg', axes[4], converter=np.degrees, idx=2) + +axes[4].set_ylim([-2, 2]) + +sns.lineplot(ts, speeds, ax=axes[5]) + +plt.show() diff --git a/selfdrive/locationd/kalman/SConscript b/selfdrive/locationd/kalman/SConscript index 2a67ad64fe..effa6a6b71 100644 --- a/selfdrive/locationd/kalman/SConscript +++ b/selfdrive/locationd/kalman/SConscript @@ -8,6 +8,7 @@ to_build = { 'pos_computer_4': 'helpers/lst_sq_computer.py', 'pos_computer_5': 'helpers/lst_sq_computer.py', 'feature_handler_5': 'helpers/feature_handler.py', + 'car': 'models/car_kf.py', 'gnss': 'models/gnss_kf.py', 'loc_4': 'models/loc_kf.py', 'live': 'models/live_kf.py', diff --git a/selfdrive/locationd/kalman/helpers/__init__.py b/selfdrive/locationd/kalman/helpers/__init__.py index 5ac7fb3f28..9c3ae53565 100644 --- a/selfdrive/locationd/kalman/helpers/__init__.py +++ b/selfdrive/locationd/kalman/helpers/__init__.py @@ -56,28 +56,44 @@ class ObservationKind(): PSEUDORANGE = 22 PSEUDORANGE_RATE = 23 - names = ['Unknown', - 'No observation', - 'GPS NED', - 'Odometric speed', - 'Phone gyro', - 'GPS velocity', - 'GPS pseudorange', - 'GPS pseudorange rate', - 'Speed', - 'No rotation', - 'Phone acceleration', - 'ORB point', - 'ECEF pos', - 'camera odometric translation', - 'camera odometric rotation', - 'ORB features', - 'MSCKF test', - 'Feature track test', - 'Lane ecef point', - 'imu frame eulers', - 'GLONASS pseudorange', - 'GLONASS pseudorange rate'] + CAL_DEVICE_FRAME_XY_SPEED = 24 # (x, y) [m/s] + CAL_DEVICE_FRAME_YAW_RATE = 25 # [rad/s] + STEER_ANGLE = 26 # [rad] + ANGLE_OFFSET_FAST = 27 # [rad] + STIFFNESS = 28 # [-] + STEER_RATIO = 29 # [-] + + names = [ + 'Unknown', + 'No observation', + 'GPS NED', + 'Odometric speed', + 'Phone gyro', + 'GPS velocity', + 'GPS pseudorange', + 'GPS pseudorange rate', + 'Speed', + 'No rotation', + 'Phone acceleration', + 'ORB point', + 'ECEF pos', + 'camera odometric translation', + 'camera odometric rotation', + 'ORB features', + 'MSCKF test', + 'Feature track test', + 'Lane ecef point', + 'imu frame eulers', + 'GLONASS pseudorange', + 'GLONASS pseudorange rate', + + 'Calibrated Device Frame x,y speed', + 'Calibrated Device Frame yaw rate', + 'Steer Angle', + 'Fast Angle Offset', + 'Stiffness', + 'Steer Ratio', + ] @classmethod def to_string(cls, kind): diff --git a/selfdrive/locationd/kalman/helpers/ekf_sym.py b/selfdrive/locationd/kalman/helpers/ekf_sym.py index 98d4a87b17..0078f3fcbf 100644 --- a/selfdrive/locationd/kalman/helpers/ekf_sym.py +++ b/selfdrive/locationd/kalman/helpers/ekf_sym.py @@ -74,8 +74,13 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No # linearize with jacobians F_sym = f_err_sym.jacobian(x_err_sym) - for sym in x_err_sym: - F_sym = F_sym.subs(sym, 0) + + if eskf_params: + for sym in x_err_sym: + F_sym = F_sym.subs(sym, 0) + + assert dt_sym in F_sym.free_symbols + for i in range(len(obs_eqs)): obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym)) if msckf and obs_eqs[i][1] in feature_track_kinds: @@ -336,6 +341,17 @@ class EKF_sym(): self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:] self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:] + def predict(self, t): + # initialize time + if self.filter_time is None: + self.filter_time = t + + # predict + dt = t - self.filter_time + assert dt >= 0 + self.x, self.P = self._predict(self.x, self.P, dt) + self.filter_time = t + def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False): # TODO handle rewinding at this level" diff --git a/selfdrive/locationd/kalman/models/car_kf.py b/selfdrive/locationd/kalman/models/car_kf.py new file mode 100755 index 0000000000..d457dbe20b --- /dev/null +++ b/selfdrive/locationd/kalman/models/car_kf.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 + +import math +import numpy as np +import sympy as sp + +from selfdrive.locationd.kalman.helpers import ObservationKind +from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code + +i = 0 + + +def _slice(n): + global i + s = slice(i, i + n) + i += n + + return s + + +class States(): + # Vehicle model params + STIFFNESS = _slice(1) # [-] + STEER_RATIO = _slice(1) # [-] + ANGLE_OFFSET = _slice(1) # [rad] + ANGLE_OFFSET_FAST = _slice(1) # [rad] + + VELOCITY = _slice(2) # (x, y) [m/s] + YAW_RATE = _slice(1) # [rad/s] + STEER_ANGLE = _slice(1) # [rad] + + +class CarKalman(): + name = 'car' + + x_initial = np.array([ + 1.0, + 15.0, + 0.0, + 0.0, + + 10.0, 0.0, + 0.0, + 0.0, + ]) + + # state covariance + P_initial = np.diag([ + .1**2, + .1**2, + math.radians(0.1)**2, + math.radians(0.1)**2, + + 10**2, 10**2, + 1**2, + 1**2, + ]) + + # process noise + Q = np.diag([ + (.05/10)**2, + .0001**2, + math.radians(0.01)**2, + math.radians(0.2)**2, + + .1**2, .1**2, + math.radians(0.1)**2, + math.radians(0.1)**2, + ]) + + obs_noise = { + ObservationKind.CAL_DEVICE_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]), + ObservationKind.CAL_DEVICE_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2), + ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**2), + ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2), + ObservationKind.STEER_RATIO: np.atleast_2d(50.0**2), + ObservationKind.STIFFNESS: np.atleast_2d(50.0**2), + } + + maha_test_kinds = [] # [ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED] + + @staticmethod + def generate_code(): + dim_state = CarKalman.x_initial.shape[0] + name = CarKalman.name + maha_test_kinds = CarKalman.maha_test_kinds + + # make functions and jacobians with sympy + # state variables + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + + # Vehicle model constants + # TODO: Read from car params at runtime + from selfdrive.controls.lib.vehicle_model import VehicleModel + from selfdrive.car.toyota.interface import CarInterface + from selfdrive.car.toyota.values import CAR + + CP = CarInterface.get_params(CAR.COROLLA_TSS2) + VM = VehicleModel(CP) + + m = VM.m + j = VM.j + aF = VM.aF + aR = VM.aR + + x = state[States.STIFFNESS, :][0, 0] + + cF, cR = x * VM.cF, x * VM.cR + angle_offset = state[States.ANGLE_OFFSET, :][0, 0] + angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0] + sa = state[States.STEER_ANGLE, :][0, 0] + + sR = state[States.STEER_RATIO, :][0, 0] + u, v = state[States.VELOCITY, :] + r = state[States.YAW_RATE, :][0, 0] + + A = sp.Matrix(np.zeros((2, 2))) + A[0, 0] = -(cF + cR) / (m * u) + A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u + A[1, 0] = -(cF * aF - cR * aR) / (j * u) + A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u) + + B = sp.Matrix(np.zeros((2, 1))) + B[0, 0] = cF / m / sR + B[1, 0] = (cF * aF) / j / sR + + x = sp.Matrix([v, r]) # lateral velocity, yaw rate + x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) + + dt = sp.Symbol('dt') + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.VELOCITY.start + 1, 0] = x_dot[0] + state_dot[States.YAW_RATE.start, 0] = x_dot[1] + + # Basic descretization, 1st order integrator + # Can be pretty bad if dt is big + f_sym = state + dt * state_dot + + # + # Observation functions + # + obs_eqs = [ + [sp.Matrix([r]), ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, None], + [sp.Matrix([u, v]), ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, None], + [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None], + [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None], + [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None], + [sp.Matrix([x]), ObservationKind.STIFFNESS, None], + ] + + gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds) + + def __init__(self): + self.dim_state = self.x_initial.shape[0] + + # init filter + self.filter = EKF_sym(self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, self.dim_state, maha_test_kinds=self.maha_test_kinds) + + @property + def x(self): + return self.filter.state() + + @property + def P(self): + return self.filter.covs() + + def predict(self, t): + return self.filter.predict(t) + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=False) + + def get_R(self, kind, n): + obs_noise = self.obs_noise[kind] + dim = obs_noise.shape[0] + R = np.zeros((n, dim, dim)) + for i in range(n): + R[i, :, :] = obs_noise + return R + + def init_state(self, state, covs_diag=None, covs=None, filter_time=None): + if covs_diag is not None: + P = np.diag(covs_diag) + elif covs is not None: + P = covs + else: + P = self.filter.covs() + self.filter.init_state(state, P, filter_time) + + def predict_and_observe(self, t, kind, data): + if len(data) > 0: + data = np.atleast_2d(data) + self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) + + +if __name__ == "__main__": + CarKalman.generate_code() diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py new file mode 100755 index 0000000000..96bfc1dcff --- /dev/null +++ b/selfdrive/locationd/paramsd.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +import math + +import cereal.messaging as messaging +import common.transformations.orientation as orient +from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States + +CARSTATE_DECIMATION = 5 + + +class ParamsLearner: + def __init__(self): + self.kf = CarKalman() + self.active = False + + self.speed = 0 + self.steering_pressed = False + self.steering_angle = 0 + self.carstate_counter = 0 + + def update_active(self): + self.active = (abs(self.steering_angle) < 45 or not self.steering_pressed) and self.speed > 5 + + def handle_log(self, t, which, msg): + if which == 'liveLocation': + roll, pitch, yaw = math.radians(msg.roll), math.radians(msg.pitch), math.radians(msg.heading) + v_device = orient.rot_from_euler([roll, pitch, yaw]).T.dot(msg.vNED) + self.speed = v_device[0] + + self.update_active() + if self.active: + self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [-msg.gyro[2]]) + self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]]) + + # Clamp values + x = self.kf.x + if not (10 < x[States.STEER_RATIO] < 25): + self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, [15.0]) + + if not (0.5 < x[States.STIFFNESS] < 3.0): + self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, [1.0]) + + else: + self.kf.filter.filter_time = t - 0.1 + + elif which == 'carState': + self.carstate_counter += 1 + if self.carstate_counter % CARSTATE_DECIMATION == 0: + self.steering_angle = msg.steeringAngle + self.steering_pressed = msg.steeringPressed + + self.update_active() + if self.active: + self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, [math.radians(msg.steeringAngle)]) + self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, [0]) + else: + self.kf.filter.filter_time = t - 0.1 + + +def main(sm=None, pm=None): + if sm is None: + sm = messaging.SubMaster(['liveLocation', 'carState']) + if pm is None: + pm = messaging.PubMaster(['liveParameters']) + + learner = ParamsLearner() + + while True: + sm.update() + + for which, updated in sm.updated.items(): + if not updated: + continue + t = sm.logMonoTime[which] * 1e-9 + learner.handle_log(t, which, sm[which]) + + # TODO: set valid to false when locationd stops sending + # TODO: make sure controlsd knows when there is no gyro + # TODO: move posenetValid somewhere else to show the model uncertainty alert + # TODO: Save and resume values from param + # TODO: Change KF to allow mass, etc to be inputs in predict step + + if sm.updated['carState']: + msg = messaging.new_message() + msg.logMonoTime = sm.logMonoTime['carState'] + + msg.init('liveParameters') + msg.liveParameters.valid = True # TODO: Check if learned values are sane + msg.liveParameters.posenetValid = True + msg.liveParameters.sensorValid = True + + x = learner.kf.x + msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) + msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) + msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET]) + msg.liveParameters.angleOffset = math.degrees(x[States.ANGLE_OFFSET_FAST]) + + # P = learner.kf.P + # print() + # print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5) + # print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5) + # print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5) + # print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5) + + pm.send('liveParameters', msg) + + +if __name__ == "__main__": + main()