sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

paramsd improvements from branch

Browse Source 
old-commit-hash: a24d08e569
commatwo_master
Willem Melching 6 years ago
parent fedd440066
commit a3568be0b0
3 changed files with 87 additions and 38 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      selfdrive/locationd/kalman/helpers/ekf_sym.py
  2. 16
      selfdrive/locationd/kalman/models/car_kf.py
  3. 104
      selfdrive/locationd/paramsd.py

5
selfdrive/locationd/kalman/helpers/ekf_sym.py
Unescape View File

@ -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.

16
selfdrive/locationd/kalman/models/car_kf.py
Unescape View File

@ -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__":

104
selfdrive/locationd/paramsd.py
Unescape View File

@ -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
CP = CarInterface.get_params(CAR.COROLLA_TSS2)
learner = ParamsLearner(CP)
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")
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)

Write Preview
Loading…
Cancel
Save