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 testspull/1163/head
Willem Melching
5 years ago
committed by
GitHub
parent
63b52b60cb
commit
c9ecab2139
7 changed files with 495 additions and 24 deletions
@ -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() |
||||
@ -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() |
||||
@ -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() |
||||
Loading…
Reference in new issue