paramsd improvements from branch
Willem Melching
parent
f0779d86e4
commit
a24d08e569
|
|
@ -291,6 +291,11 @@ class EKF_sym():
|
|||
self.rewind_t = []
|
||||
self.rewind_states = []
|
||||
|
||||
def reset_rewind(self):
|
||||
self.rewind_obscache = []
|
||||
self.rewind_t = []
|
||||
self.rewind_states = []
|
||||
|
||||
def augment(self):
|
||||
# TODO this is not a generalized way of doing this and implies that the augmented states
|
||||
# are simply the first (dim_augment_state) elements of the main state.
|
||||
|
|
|
|||
|
|
@ -69,8 +69,6 @@ class CarKalman():
|
|||
])
|
||||
|
||||
obs_noise = {
|
||||
ObservationKind.ROAD_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]),
|
||||
ObservationKind.ROAD_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),
|
||||
|
|
@ -149,8 +147,12 @@ class CarKalman():
|
|||
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds, global_vars=CarKalman.global_vars)
|
||||
|
||||
def __init__(self):
|
||||
def __init__(self, steer_ratio=15, stiffness_factor=1, angle_offset=0):
|
||||
self.dim_state = self.x_initial.shape[0]
|
||||
x_init = self.x_initial
|
||||
x_init[States.STEER_RATIO] = steer_ratio
|
||||
x_init[States.STIFFNESS] = stiffness_factor
|
||||
x_init[States.ANGLE_OFFSET] = angle_offset
|
||||
|
||||
# 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, global_vars=self.global_vars)
|
||||
|
|
@ -186,10 +188,14 @@ class CarKalman():
|
|||
P = self.filter.covs()
|
||||
self.filter.init_state(state, P, filter_time)
|
||||
|
||||
def predict_and_observe(self, t, kind, data):
|
||||
def predict_and_observe(self, t, kind, data, R=None):
|
||||
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 R is None:
|
||||
R = self.get_R(kind, len(data))
|
||||
|
||||
self.filter.predict_and_update_batch(t, kind, data, R)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -1,15 +1,22 @@
|
|||
#!/usr/bin/env python3
|
||||
import math
|
||||
|
||||
import json
|
||||
import numpy as np
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States
|
||||
from cereal import car
|
||||
from common.params import Params, put_nonblocking
|
||||
from selfdrive.locationd.kalman.models.car_kf import (CarKalman,
|
||||
ObservationKind, States)
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
CARSTATE_DECIMATION = 5
|
||||
|
||||
|
||||
class ParamsLearner:
|
||||
def __init__(self, CP):
|
||||
self.kf = CarKalman()
|
||||
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
|
||||
self.kf = CarKalman(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
|
||||
|
|
@ -25,34 +32,37 @@ class ParamsLearner:
|
|||
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 == 'liveLocationKalman':
|
||||
|
||||
v_calibrated = msg.velocityCalibrated.value
|
||||
# v_calibrated_std = msg.velocityCalibrated.std
|
||||
self.speed = v_calibrated[0]
|
||||
v_calibrated_std = msg.velocityCalibrated.std
|
||||
|
||||
yaw_rate = msg.angularVelocityCalibrated.value[2]
|
||||
# yaw_rate_std = msg.angularVelocityCalibrated.std[2]
|
||||
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
|
||||
|
||||
self.update_active()
|
||||
if self.active:
|
||||
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, [-yaw_rate])
|
||||
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_XY_SPEED, [[v_calibrated[0], -v_calibrated[1]]])
|
||||
self.active = v_calibrated[0] > 5
|
||||
in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
|
||||
|
||||
if self.active and in_linear_region:
|
||||
self.kf.predict_and_observe(t,
|
||||
ObservationKind.ROAD_FRAME_YAW_RATE,
|
||||
np.array([[[-yaw_rate]]]),
|
||||
np.array([np.atleast_2d(yaw_rate_std**2)]))
|
||||
self.kf.predict_and_observe(t,
|
||||
ObservationKind.ROAD_FRAME_XY_SPEED,
|
||||
np.array([[[v_calibrated[0], -v_calibrated[1]]]]),
|
||||
np.array([np.diag([v_calibrated_std[0]**2, v_calibrated_std[1]**2])]))
|
||||
|
||||
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
|
||||
|
||||
# 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])
|
||||
self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[[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
|
||||
self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.0]]]))
|
||||
|
||||
elif which == 'carState':
|
||||
self.carstate_counter += 1
|
||||
|
|
@ -60,12 +70,13 @@ class ParamsLearner:
|
|||
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
|
||||
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
|
||||
|
||||
if not self.active:
|
||||
# Reset time when stopped so uncertainty doesn't grow
|
||||
self.kf.filter.filter_time = t
|
||||
self.kf.filter.reset_rewind()
|
||||
|
||||
|
||||
def main(sm=None, pm=None):
|
||||
|
|
@ -74,13 +85,33 @@ def main(sm=None, pm=None):
|
|||
if pm is None:
|
||||
pm = messaging.PubMaster(['liveParameters'])
|
||||
|
||||
# TODO: Read from car params at runtime
|
||||
from selfdrive.car.toyota.interface import CarInterface
|
||||
from selfdrive.car.toyota.values import CAR
|
||||
params_reader = Params()
|
||||
# wait for stats about the car to come in from controls
|
||||
cloudlog.info("paramsd is waiting for CarParams")
|
||||
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
|
||||
cloudlog.info("paramsd got CarParams")
|
||||
|
||||
CP = CarInterface.get_params(CAR.COROLLA_TSS2)
|
||||
learner = ParamsLearner(CP)
|
||||
params = params_reader.get("LiveParameters")
|
||||
|
||||
# Check if car model matches
|
||||
if params is not None:
|
||||
params = json.loads(params)
|
||||
if params.get('carFingerprint', None) != CP.carFingerprint:
|
||||
cloudlog.info("Parameter learner found parameters for wrong car.")
|
||||
params = None
|
||||
|
||||
if params is None:
|
||||
params = {
|
||||
'carFingerprint': CP.carFingerprint,
|
||||
'steerRatio': CP.steerRatio,
|
||||
'stiffnessFactor': 1.0,
|
||||
'angleOffsetAverage': 0.0,
|
||||
}
|
||||
cloudlog.info("Parameter learner resetting to default values")
|
||||
|
||||
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage']))
|
||||
|
||||
i = 0
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
|
|
@ -92,9 +123,6 @@ def main(sm=None, pm=None):
|
|||
|
||||
# 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('liveParameters')
|
||||
|
|
@ -110,6 +138,16 @@ def main(sm=None, pm=None):
|
|||
msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET])
|
||||
msg.liveParameters.angleOffset = math.degrees(x[States.ANGLE_OFFSET_FAST])
|
||||
|
||||
i += 1
|
||||
if i % 6000 == 0: # once a minute
|
||||
params = {
|
||||
'carFingerprint': CP.carFingerprint,
|
||||
'steerRatio': msg.liveParameters.steerRatio,
|
||||
'stiffnessFactor': msg.liveParameters.stiffnessFactor,
|
||||
'angleOffsetAverage': msg.liveParameters.angleOffsetAverage,
|
||||
}
|
||||
put_nonblocking("LiveParameters", json.dumps(params))
|
||||
|
||||
# P = learner.kf.P
|
||||
# print()
|
||||
# print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5)
|
||||
|
|
|
|||
Loading…
Reference in New Issue