Willem Melching
5 years ago
parent
2d69a5a335
commit
b26d45ce04
2 changed files with 0 additions and 290 deletions
@ -1,206 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
import zmq |
||||
import math |
||||
import json |
||||
|
||||
import numpy as np |
||||
from bisect import bisect_right |
||||
|
||||
from cereal import car |
||||
from common.params import Params |
||||
from common.numpy_fast import clip |
||||
import cereal.messaging as messaging |
||||
from selfdrive.swaglog import cloudlog |
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel |
||||
from cereal.services import service_list |
||||
from selfdrive.locationd.kalman.loc_local_kf import LocLocalKalman |
||||
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind |
||||
from selfdrive.locationd.params_learner import ParamsLearner |
||||
|
||||
DEBUG = False |
||||
kf = LocLocalKalman() # Make sure that model is generated on import time |
||||
|
||||
|
||||
LEARNING_RATE = 3 |
||||
|
||||
|
||||
class Localizer(): |
||||
def __init__(self, disabled_logs=None, dog=None): |
||||
self.kf = LocLocalKalman() |
||||
self.reset_kalman() |
||||
|
||||
self.sensor_data_t = 0.0 |
||||
self.max_age = .2 # seconds |
||||
self.calibration_valid = False |
||||
|
||||
if disabled_logs is None: |
||||
self.disabled_logs = list() |
||||
else: |
||||
self.disabled_logs = disabled_logs |
||||
|
||||
def reset_kalman(self): |
||||
self.filter_time = None |
||||
self.observation_buffer = [] |
||||
self.converter = None |
||||
self.speed_counter = 0 |
||||
self.sensor_counter = 0 |
||||
|
||||
def liveLocationMsg(self, time): |
||||
fix = messaging.log.KalmanOdometry.new_message() |
||||
|
||||
predicted_state = self.kf.x |
||||
fix.trans = [float(predicted_state[0]), float(predicted_state[1]), float(predicted_state[2])] |
||||
fix.rot = [float(predicted_state[3]), float(predicted_state[4]), float(predicted_state[5])] |
||||
|
||||
return fix |
||||
|
||||
def update_kalman(self, time, kind, meas): |
||||
idx = bisect_right([x[0] for x in self.observation_buffer], time) |
||||
self.observation_buffer.insert(idx, (time, kind, meas)) |
||||
while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age: |
||||
self.kf.predict_and_observe(*self.observation_buffer.pop(0)) |
||||
|
||||
def handle_cam_odo(self, log, current_time): |
||||
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot, |
||||
log.cameraOdometry.rotStd])) |
||||
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans, |
||||
log.cameraOdometry.transStd])) |
||||
|
||||
def handle_controls_state(self, log, current_time): |
||||
self.speed_counter += 1 |
||||
if self.speed_counter % 5 == 0: |
||||
self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.controlsState.vEgo])) |
||||
|
||||
def handle_sensors(self, log, current_time): |
||||
for sensor_reading in log.sensorEvents: |
||||
# TODO does not yet account for double sensor readings in the log |
||||
if sensor_reading.type == 4: |
||||
self.sensor_counter += 1 |
||||
if self.sensor_counter % LEARNING_RATE == 0: |
||||
self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]]) |
||||
|
||||
def handle_log(self, log): |
||||
current_time = 1e-9 * log.logMonoTime |
||||
typ = log.which |
||||
if typ in self.disabled_logs: |
||||
return |
||||
if typ == "sensorEvents": |
||||
self.sensor_data_t = current_time |
||||
self.handle_sensors(log, current_time) |
||||
elif typ == "controlsState": |
||||
self.handle_controls_state(log, current_time) |
||||
elif typ == "cameraOdometry": |
||||
self.handle_cam_odo(log, current_time) |
||||
|
||||
|
||||
def locationd_thread(gctx, addr, disabled_logs): |
||||
poller = zmq.Poller() |
||||
|
||||
controls_state_socket = messaging.sub_sock('controlsState', poller, addr=addr, conflate=True) |
||||
sensor_events_socket = messaging.sub_sock('sensorEvents', poller, addr=addr, conflate=True) |
||||
camera_odometry_socket = messaging.sub_sock('cameraOdometry', poller, addr=addr, conflate=True) |
||||
|
||||
kalman_odometry_socket = messaging.pub_sock('kalmanOdometry') |
||||
live_parameters_socket = messaging.pub_sock('liveParameters') |
||||
|
||||
params_reader = Params() |
||||
cloudlog.info("Parameter learner is waiting for CarParams") |
||||
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True)) |
||||
VM = VehicleModel(CP) |
||||
cloudlog.info("Parameter learner got CarParams: %s" % CP.carFingerprint) |
||||
|
||||
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) or (params.get('carVin', CP.carVin) != CP.carVin): |
||||
cloudlog.info("Parameter learner found parameters for wrong car.") |
||||
params = None |
||||
|
||||
if params is None: |
||||
params = { |
||||
'carFingerprint': CP.carFingerprint, |
||||
'carVin': CP.carVin, |
||||
'angleOffsetAverage': 0.0, |
||||
'stiffnessFactor': 1.0, |
||||
'steerRatio': VM.sR, |
||||
} |
||||
params_reader.put("LiveParameters", json.dumps(params)) |
||||
cloudlog.info("Parameter learner resetting to default values") |
||||
|
||||
cloudlog.info("Parameter starting with: %s" % str(params)) |
||||
localizer = Localizer(disabled_logs=disabled_logs) |
||||
|
||||
learner = ParamsLearner(VM, |
||||
angle_offset=params['angleOffsetAverage'], |
||||
stiffness_factor=params['stiffnessFactor'], |
||||
steer_ratio=params['steerRatio'], |
||||
learning_rate=LEARNING_RATE) |
||||
|
||||
i = 1 |
||||
while True: |
||||
for socket, event in poller.poll(timeout=1000): |
||||
log = messaging.recv_one(socket) |
||||
localizer.handle_log(log) |
||||
|
||||
if socket is controls_state_socket: |
||||
if not localizer.kf.t: |
||||
continue |
||||
|
||||
if i % LEARNING_RATE == 0: |
||||
# controlsState is not updating the Kalman Filter, so update KF manually |
||||
localizer.kf.predict(1e-9 * log.logMonoTime) |
||||
|
||||
predicted_state = localizer.kf.x |
||||
yaw_rate = -float(predicted_state[5]) |
||||
|
||||
steering_angle = math.radians(log.controlsState.angleSteers) |
||||
params_valid = learner.update(yaw_rate, log.controlsState.vEgo, steering_angle) |
||||
|
||||
log_t = 1e-9 * log.logMonoTime |
||||
sensor_data_age = log_t - localizer.sensor_data_t |
||||
|
||||
params = messaging.new_message() |
||||
params.init('liveParameters') |
||||
params.liveParameters.valid = bool(params_valid) |
||||
params.liveParameters.sensorValid = bool(sensor_data_age < 5.0) |
||||
params.liveParameters.angleOffset = float(math.degrees(learner.ao)) |
||||
params.liveParameters.angleOffsetAverage = float(math.degrees(learner.slow_ao)) |
||||
params.liveParameters.stiffnessFactor = float(learner.x) |
||||
params.liveParameters.steerRatio = float(learner.sR) |
||||
live_parameters_socket.send(params.to_bytes()) |
||||
|
||||
if i % 6000 == 0: # once a minute |
||||
params = learner.get_values() |
||||
params['carFingerprint'] = CP.carFingerprint |
||||
params['carVin'] = CP.carVin |
||||
params_reader.put("LiveParameters", json.dumps(params)) |
||||
params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.controlsState.angleModelBias})) |
||||
|
||||
i += 1 |
||||
elif socket is camera_odometry_socket: |
||||
msg = messaging.new_message() |
||||
msg.init('kalmanOdometry') |
||||
msg.logMonoTime = log.logMonoTime |
||||
msg.kalmanOdometry = localizer.liveLocationMsg(log.logMonoTime * 1e-9) |
||||
kalman_odometry_socket.send(msg.to_bytes()) |
||||
elif socket is sensor_events_socket: |
||||
pass |
||||
|
||||
|
||||
def main(gctx=None, addr="127.0.0.1"): |
||||
IN_CAR = os.getenv("IN_CAR", False) |
||||
disabled_logs = os.getenv("DISABLED_LOGS", "").split(",") |
||||
|
||||
# No speed for now |
||||
disabled_logs.append('controlsState') |
||||
if IN_CAR: |
||||
addr = "192.168.5.11" |
||||
|
||||
locationd_thread(gctx, addr, disabled_logs) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
@ -1,84 +0,0 @@ |
||||
import math |
||||
from common.numpy_fast import clip |
||||
|
||||
MAX_ANGLE_OFFSET = math.radians(10.) |
||||
MAX_ANGLE_OFFSET_TH = math.radians(9.) |
||||
MIN_STIFFNESS = 0.5 |
||||
MAX_STIFFNESS = 2.0 |
||||
MIN_SR = 0.5 |
||||
MAX_SR = 2.0 |
||||
MIN_SR_TH = 0.55 |
||||
MAX_SR_TH = 1.9 |
||||
|
||||
DEBUG = False |
||||
|
||||
|
||||
class ParamsLearner(): |
||||
def __init__(self, VM, angle_offset=0., stiffness_factor=1.0, steer_ratio=None, learning_rate=1.0): |
||||
self.VM = VM |
||||
|
||||
self.ao = math.radians(angle_offset) |
||||
self.slow_ao = math.radians(angle_offset) |
||||
self.x = stiffness_factor |
||||
self.sR = VM.sR if steer_ratio is None else steer_ratio |
||||
self.MIN_SR = MIN_SR * self.VM.sR |
||||
self.MAX_SR = MAX_SR * self.VM.sR |
||||
self.MIN_SR_TH = MIN_SR_TH * self.VM.sR |
||||
self.MAX_SR_TH = MAX_SR_TH * self.VM.sR |
||||
|
||||
self.alpha1 = 0.01 * learning_rate |
||||
self.alpha2 = 0.0005 * learning_rate |
||||
self.alpha3 = 0.1 * learning_rate |
||||
self.alpha4 = 1.0 * learning_rate |
||||
|
||||
def get_values(self): |
||||
return { |
||||
'angleOffsetAverage': math.degrees(self.slow_ao), |
||||
'stiffnessFactor': self.x, |
||||
'steerRatio': self.sR, |
||||
} |
||||
|
||||
def update(self, psi, u, sa): |
||||
cF0 = self.VM.cF |
||||
cR0 = self.VM.cR |
||||
aR = self.VM.aR |
||||
aF = self.VM.aF |
||||
l = self.VM.l |
||||
m = self.VM.m |
||||
|
||||
x = self.x |
||||
ao = self.ao |
||||
sR = self.sR |
||||
|
||||
# Gradient descent: learn angle offset, tire stiffness and steer ratio. |
||||
if u > 10.0 and abs(math.degrees(sa)) < 15.: |
||||
self.ao -= self.alpha1 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2) |
||||
|
||||
ao = self.slow_ao |
||||
self.slow_ao -= self.alpha2 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2) |
||||
|
||||
self.x -= self.alpha3 * -2.0*cF0*cR0*l*m*u**3*(ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**3) |
||||
|
||||
self.sR -= self.alpha4 * -2.0*cF0*cR0*l*u*x*(ao - sa)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**3*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2) |
||||
|
||||
if DEBUG: |
||||
# s1 = "Measured yaw rate % .6f" % psi |
||||
# ao = 0. |
||||
# s2 = "Uncompensated yaw % .6f" % (1.0*u*(-ao + sa)/(l*sR*(1 - m*u**2*(aF*cF0*x - aR*cR0*x)/(cF0*cR0*l**2*x**2)))) |
||||
# instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa |
||||
s4 = "Instant AO: % .6f Avg. AO % .6f" % (math.degrees(self.ao), math.degrees(self.slow_ao)) |
||||
s5 = "Stiffnes: % .6f x" % self.x |
||||
s6 = "sR: %.4f" % self.sR |
||||
print("{0} {1} {2}".format(s4, s5, s6)) |
||||
|
||||
|
||||
self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET) |
||||
self.slow_ao = clip(self.slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET) |
||||
self.x = clip(self.x, MIN_STIFFNESS, MAX_STIFFNESS) |
||||
self.sR = clip(self.sR, self.MIN_SR, self.MAX_SR) |
||||
|
||||
# don't check stiffness for validity, as it can change quickly if sR is off |
||||
valid = abs(self.slow_ao) < MAX_ANGLE_OFFSET_TH and \ |
||||
self.sR > self.MIN_SR_TH and self.sR < self.MAX_SR_TH |
||||
|
||||
return valid |
||||
Loading…
Reference in new issue