Vehicle Researcher
6 years ago
parent
be5c2aef3a
commit
f74a201edc
102 changed files with 6419 additions and 5458 deletions
Binary file not shown.
@ -0,0 +1,81 @@ |
||||
#!/usr/bin/env python |
||||
import sympy as sp |
||||
import numpy as np |
||||
|
||||
def cross(x): |
||||
ret = sp.Matrix(np.zeros((3,3))) |
||||
ret[0,1], ret[0,2] = -x[2], x[1] |
||||
ret[1,0], ret[1,2] = x[2], -x[0] |
||||
ret[2,0], ret[2,1] = -x[1], x[0] |
||||
return ret |
||||
|
||||
def euler_rotate(roll, pitch, yaw): |
||||
# make symbolic rotation matrix from eulers |
||||
matrix_roll = sp.Matrix([[1, 0, 0], |
||||
[0, sp.cos(roll), -sp.sin(roll)], |
||||
[0, sp.sin(roll), sp.cos(roll)]]) |
||||
matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)], |
||||
[0, 1, 0], |
||||
[-sp.sin(pitch), 0, sp.cos(pitch)]]) |
||||
matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0], |
||||
[sp.sin(yaw), sp.cos(yaw), 0], |
||||
[0, 0, 1]]) |
||||
return matrix_yaw*matrix_pitch*matrix_roll |
||||
|
||||
def quat_rotate(q0, q1, q2, q3): |
||||
# make symbolic rotation matrix from quat |
||||
return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 + q0*q3), 2*(q1*q3 - q0*q2)], |
||||
[2*(q1*q2 - q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 + q0*q1)], |
||||
[2*(q1*q3 + q0*q2), 2*(q2*q3 - q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]).T |
||||
|
||||
def quat_matrix_l(p): |
||||
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]], |
||||
[p[1], p[0], -p[3], p[2]], |
||||
[p[2], p[3], p[0], -p[1]], |
||||
[p[3], -p[2], p[1], p[0]]]) |
||||
|
||||
def quat_matrix_r(p): |
||||
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]], |
||||
[p[1], p[0], p[3], -p[2]], |
||||
[p[2], -p[3], p[0], p[1]], |
||||
[p[3], p[2], -p[1], p[0]]]) |
||||
|
||||
|
||||
def sympy_into_c(sympy_functions): |
||||
from sympy.utilities import codegen |
||||
routines = [] |
||||
for name, expr, args in sympy_functions: |
||||
r = codegen.make_routine(name, expr, language="C99") |
||||
|
||||
# argument ordering input to sympy is broken with function with output arguments |
||||
nargs = [] |
||||
# reorder the input arguments |
||||
for aa in args: |
||||
if aa is None: |
||||
nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1,1])) |
||||
continue |
||||
found = False |
||||
for a in r.arguments: |
||||
if str(aa.name) == str(a.name): |
||||
nargs.append(a) |
||||
found = True |
||||
break |
||||
if not found: |
||||
# [1,1] is a hack for Matrices |
||||
nargs.append(codegen.InputArgument(aa, dimensions=[1,1])) |
||||
# add the output arguments |
||||
for a in r.arguments: |
||||
if type(a) == codegen.OutputArgument: |
||||
nargs.append(a) |
||||
|
||||
#assert len(r.arguments) == len(args)+1 |
||||
r.arguments = nargs |
||||
|
||||
# add routine to list |
||||
routines.append(r) |
||||
|
||||
[(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf") |
||||
c_code = '\n'.join(filter(lambda x: len(x) > 0 and x[0] != '#', c_code.split("\n"))) |
||||
c_header = '\n'.join(filter(lambda x: len(x) > 0 and x[0] != '#', c_header.split("\n"))) |
||||
|
||||
return c_header, c_code |
||||
Binary file not shown.
@ -0,0 +1,128 @@ |
||||
#!/usr/bin/env python2.7 |
||||
import json |
||||
import os |
||||
import random |
||||
import time |
||||
import threading |
||||
import traceback |
||||
import zmq |
||||
import Queue |
||||
from jsonrpc import JSONRPCResponseManager, dispatcher |
||||
from websocket import create_connection, WebSocketTimeoutException |
||||
|
||||
import selfdrive.crash as crash |
||||
import selfdrive.messaging as messaging |
||||
from common.params import Params |
||||
from selfdrive.services import service_list |
||||
from selfdrive.swaglog import cloudlog |
||||
from selfdrive.version import version, dirty |
||||
|
||||
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') |
||||
HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4) |
||||
|
||||
dispatcher["echo"] = lambda s: s |
||||
payload_queue = Queue.Queue() |
||||
response_queue = Queue.Queue() |
||||
|
||||
def handle_long_poll(ws): |
||||
end_event = threading.Event() |
||||
|
||||
threads = [ |
||||
threading.Thread(target=ws_recv, args=(ws, end_event)), |
||||
threading.Thread(target=ws_send, args=(ws, end_event)) |
||||
] + [ |
||||
threading.Thread(target=jsonrpc_handler, args=(end_event,)) |
||||
for x in xrange(HANDLER_THREADS) |
||||
] |
||||
|
||||
map(lambda thread: thread.start(), threads) |
||||
try: |
||||
while not end_event.is_set(): |
||||
time.sleep(0.1) |
||||
except (KeyboardInterrupt, SystemExit): |
||||
end_event.set() |
||||
raise |
||||
finally: |
||||
for i, thread in enumerate(threads): |
||||
thread.join() |
||||
|
||||
def jsonrpc_handler(end_event): |
||||
while not end_event.is_set(): |
||||
try: |
||||
data = payload_queue.get(timeout=1) |
||||
response = JSONRPCResponseManager.handle(data, dispatcher) |
||||
response_queue.put_nowait(response) |
||||
except Queue.Empty: |
||||
pass |
||||
except Exception as e: |
||||
cloudlog.exception("athena jsonrpc handler failed") |
||||
traceback.print_exc() |
||||
response_queue.put_nowait(json.dumps({"error": str(e)})) |
||||
|
||||
# security: user should be able to request any message from their car |
||||
# TODO: add service to, for example, start visiond and take a picture |
||||
@dispatcher.add_method |
||||
def getMessage(service=None, timeout=1000): |
||||
context = zmq.Context() |
||||
if service is None or service not in service_list: |
||||
raise Exception("invalid service") |
||||
socket = messaging.sub_sock(context, service_list[service].port) |
||||
socket.setsockopt(zmq.RCVTIMEO, timeout) |
||||
ret = messaging.recv_one(socket) |
||||
return ret.to_dict() |
||||
|
||||
def ws_recv(ws, end_event): |
||||
while not end_event.is_set(): |
||||
try: |
||||
data = ws.recv() |
||||
payload_queue.put_nowait(data) |
||||
except WebSocketTimeoutException: |
||||
pass |
||||
except Exception: |
||||
traceback.print_exc() |
||||
end_event.set() |
||||
|
||||
def ws_send(ws, end_event): |
||||
while not end_event.is_set(): |
||||
try: |
||||
response = response_queue.get(timeout=1) |
||||
ws.send(response.json) |
||||
except Queue.Empty: |
||||
pass |
||||
except Exception: |
||||
traceback.print_exc() |
||||
end_event.set() |
||||
|
||||
def backoff(retries): |
||||
return random.randrange(0, min(128, int(2 ** retries))) |
||||
|
||||
def main(gctx=None): |
||||
params = Params() |
||||
dongle_id = params.get("DongleId") |
||||
access_token = params.get("AccessToken") |
||||
ws_uri = ATHENA_HOST + "/ws/" + dongle_id |
||||
|
||||
crash.bind_user(id=dongle_id) |
||||
crash.bind_extra(version=version, dirty=dirty, is_eon=True) |
||||
crash.install() |
||||
|
||||
conn_retries = 0 |
||||
while 1: |
||||
try: |
||||
print("connecting to %s" % ws_uri) |
||||
ws = create_connection(ws_uri, |
||||
cookie="jwt=" + access_token, |
||||
enable_multithread=True) |
||||
ws.settimeout(1) |
||||
conn_retries = 0 |
||||
handle_long_poll(ws) |
||||
except (KeyboardInterrupt, SystemExit): |
||||
break |
||||
except Exception: |
||||
conn_retries += 1 |
||||
traceback.print_exc() |
||||
|
||||
time.sleep(backoff(conn_retries)) |
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
@ -0,0 +1 @@ |
||||
|
||||
@ -0,0 +1,104 @@ |
||||
import numpy as np |
||||
from common.kalman.simple_kalman import KF1D |
||||
from selfdrive.config import Conversions as CV |
||||
from selfdrive.can.parser import CANParser |
||||
from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD |
||||
|
||||
def get_powertrain_can_parser(CP): |
||||
# this function generates lists for signal, messages and initial values |
||||
signals = [ |
||||
# sig_name, sig_address, default |
||||
("Steer_Torque_Sensor", "Steering_Torque", 0), |
||||
("Steering_Angle", "Steering_Torque", 0), |
||||
("Cruise_On", "CruiseControl", 0), |
||||
("Cruise_Activated", "CruiseControl", 0), |
||||
("Brake_Pedal", "Brake_Pedal", 0), |
||||
("Throttle_Pedal", "Throttle", 0), |
||||
("LEFT_BLINKER", "Dashlights", 0), |
||||
("RIGHT_BLINKER", "Dashlights", 0), |
||||
("SEATBELT_FL", "Dashlights", 0), |
||||
("FL", "Wheel_Speeds", 0), |
||||
("FR", "Wheel_Speeds", 0), |
||||
("RL", "Wheel_Speeds", 0), |
||||
("RR", "Wheel_Speeds", 0), |
||||
("DOOR_OPEN_FR", "BodyInfo", 1), |
||||
("DOOR_OPEN_FL", "BodyInfo", 1), |
||||
("DOOR_OPEN_RR", "BodyInfo", 1), |
||||
("DOOR_OPEN_RL", "BodyInfo", 1), |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("Dashlights", 10), |
||||
("CruiseControl", 20), |
||||
("Wheel_Speeds", 50), |
||||
("Steering_Torque", 100), |
||||
("BodyInfo", 10), |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) |
||||
|
||||
class CarState(object): |
||||
def __init__(self, CP): |
||||
# initialize can parser |
||||
self.CP = CP |
||||
|
||||
self.car_fingerprint = CP.carFingerprint |
||||
self.left_blinker_on = False |
||||
self.prev_left_blinker_on = False |
||||
self.right_blinker_on = False |
||||
self.prev_right_blinker_on = False |
||||
self.steer_torque_driver = 0 |
||||
self.steer_not_allowed = False |
||||
self.main_on = False |
||||
|
||||
# vEgo kalman filter |
||||
dt = 0.01 |
||||
self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]), |
||||
A=np.matrix([[1., dt], [0., 1.]]), |
||||
C=np.matrix([1., 0.]), |
||||
K=np.matrix([[0.12287673], [0.29666309]])) |
||||
self.v_ego = 0. |
||||
|
||||
def update(self, cp): |
||||
|
||||
self.can_valid = True |
||||
self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal'] |
||||
self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal'] |
||||
self.user_gas_pressed = self.pedal_gas > 0 |
||||
self.brake_pressed = self.brake_pressure > 0 |
||||
self.brake_lights = bool(self.brake_pressed) |
||||
|
||||
self.v_wheel_fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS |
||||
self.v_wheel_fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS |
||||
self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS |
||||
self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS |
||||
|
||||
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. |
||||
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default |
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed |
||||
self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]]) |
||||
|
||||
self.v_ego_raw = v_wheel |
||||
v_ego_x = self.v_ego_kf.update(v_wheel) |
||||
|
||||
self.v_ego = float(v_ego_x[0]) |
||||
self.a_ego = float(v_ego_x[1]) |
||||
self.standstill = self.v_ego_raw < 0.01 |
||||
|
||||
self.prev_left_blinker_on = self.left_blinker_on |
||||
self.prev_right_blinker_on = self.right_blinker_on |
||||
self.left_blinker_on = cp.vl["Dashlights"]['LEFT_BLINKER'] == 1 |
||||
self.right_blinker_on = cp.vl["Dashlights"]['RIGHT_BLINKER'] == 1 |
||||
self.seatbelt_unlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1 |
||||
self.steer_torque_driver = cp.vl["Steering_Torque"]['Steer_Torque_Sensor'] |
||||
self.acc_active = cp.vl["CruiseControl"]['Cruise_Activated'] |
||||
self.main_on = cp.vl["CruiseControl"]['Cruise_On'] |
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.car_fingerprint] |
||||
self.angle_steers = cp.vl["Steering_Torque"]['Steering_Angle'] |
||||
self.door_open = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'], |
||||
cp.vl["BodyInfo"]['DOOR_OPEN_RL'], |
||||
cp.vl["BodyInfo"]['DOOR_OPEN_FR'], |
||||
cp.vl["BodyInfo"]['DOOR_OPEN_FL']]) |
||||
|
||||
|
||||
@ -0,0 +1,205 @@ |
||||
#!/usr/bin/env python |
||||
from cereal import car |
||||
from common.realtime import sec_since_boot |
||||
from selfdrive.config import Conversions as CV |
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET |
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel |
||||
from selfdrive.car.subaru.values import CAR |
||||
from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser |
||||
|
||||
try: |
||||
from selfdrive.car.subaru.carcontroller import CarController |
||||
except ImportError: |
||||
CarController = None |
||||
|
||||
|
||||
class CarInterface(object): |
||||
def __init__(self, CP, sendcan=None): |
||||
self.CP = CP |
||||
|
||||
self.frame = 0 |
||||
self.can_invalid_count = 0 |
||||
self.acc_active_prev = 0 |
||||
|
||||
# *** init the major players *** |
||||
self.CS = CarState(CP) |
||||
self.VM = VehicleModel(CP) |
||||
self.pt_cp = get_powertrain_can_parser(CP) |
||||
|
||||
# sending if read only is False |
||||
if sendcan is not None: |
||||
self.sendcan = sendcan |
||||
self.CC = CarController(CP.carFingerprint) |
||||
|
||||
@staticmethod |
||||
def compute_gb(accel, speed): |
||||
return float(accel) / 4.0 |
||||
|
||||
@staticmethod |
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target): |
||||
return 1.0 |
||||
|
||||
@staticmethod |
||||
def get_params(candidate, fingerprint): |
||||
ret = car.CarParams.new_message() |
||||
|
||||
ret.carName = "subaru" |
||||
ret.carFingerprint = candidate |
||||
ret.safetyModel = car.CarParams.SafetyModels.subaru |
||||
|
||||
ret.enableCruise = False |
||||
ret.steerLimitAlert = True |
||||
ret.enableCamera = True |
||||
|
||||
std_cargo = 136 |
||||
ret.steerRateCost = 0.7 |
||||
|
||||
if candidate in [CAR.IMPREZA]: |
||||
ret.mass = 1568 + std_cargo |
||||
ret.wheelbase = 2.67 |
||||
ret.centerToFront = ret.wheelbase * 0.5 |
||||
ret.steerRatio = 15 |
||||
tire_stiffness_factor = 1.0 |
||||
ret.steerActuatorDelay = 0.4 # end-to-end angle controller |
||||
ret.steerKf = 0.00005 |
||||
ret.steerKiBP, ret.steerKpBP = [[0., 20.], [0., 20.]] |
||||
ret.steerKpV, ret.steerKiV = [[0.2, 0.3], [0.02, 0.03]] |
||||
ret.steerMaxBP = [0.] # m/s |
||||
ret.steerMaxV = [1.] |
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.torque |
||||
ret.steerRatioRear = 0. |
||||
# testing tuning |
||||
|
||||
# No long control in subaru |
||||
ret.gasMaxBP = [0.] |
||||
ret.gasMaxV = [0.] |
||||
ret.brakeMaxBP = [0.] |
||||
ret.brakeMaxV = [0.] |
||||
ret.longPidDeadzoneBP = [0.] |
||||
ret.longPidDeadzoneV = [0.] |
||||
ret.longitudinalKpBP = [0.] |
||||
ret.longitudinalKpV = [0.] |
||||
ret.longitudinalKiBP = [0.] |
||||
ret.longitudinalKiV = [0.] |
||||
|
||||
# end from gm |
||||
|
||||
# hardcoding honda civic 2016 touring params so they can be used to |
||||
# scale unknown params for other cars |
||||
mass_civic = 2923./2.205 + std_cargo |
||||
wheelbase_civic = 2.70 |
||||
centerToFront_civic = wheelbase_civic * 0.4 |
||||
centerToRear_civic = wheelbase_civic - centerToFront_civic |
||||
rotationalInertia_civic = 2500 |
||||
tireStiffnessFront_civic = 192150 |
||||
tireStiffnessRear_civic = 202500 |
||||
centerToRear = ret.wheelbase - ret.centerToFront |
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for |
||||
# civic and scaling by mass and wheelbase |
||||
ret.rotationalInertia = rotationalInertia_civic * \ |
||||
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) |
||||
|
||||
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by |
||||
# mass and CG position, so all cars will have approximately similar dyn behaviors |
||||
ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ |
||||
ret.mass / mass_civic * \ |
||||
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) |
||||
ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ |
||||
ret.mass / mass_civic * \ |
||||
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) |
||||
|
||||
return ret |
||||
|
||||
# returns a car.CarState |
||||
def update(self, c): |
||||
|
||||
self.pt_cp.update(int(sec_since_boot() * 1e9), False) |
||||
self.CS.update(self.pt_cp) |
||||
|
||||
# create message |
||||
ret = car.CarState.new_message() |
||||
|
||||
# speeds |
||||
ret.vEgo = self.CS.v_ego |
||||
ret.aEgo = self.CS.a_ego |
||||
ret.vEgoRaw = self.CS.v_ego_raw |
||||
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) |
||||
ret.standstill = self.CS.standstill |
||||
ret.wheelSpeeds.fl = self.CS.v_wheel_fl |
||||
ret.wheelSpeeds.fr = self.CS.v_wheel_fr |
||||
ret.wheelSpeeds.rl = self.CS.v_wheel_rl |
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr |
||||
|
||||
# steering wheel |
||||
ret.steeringAngle = self.CS.angle_steers |
||||
|
||||
# torque and user override. Driver awareness |
||||
# timer resets when the user uses the steering wheel. |
||||
ret.steeringPressed = self.CS.steer_override |
||||
ret.steeringTorque = self.CS.steer_torque_driver |
||||
|
||||
# cruise state |
||||
ret.cruiseState.available = bool(self.CS.main_on) |
||||
ret.leftBlinker = self.CS.left_blinker_on |
||||
ret.rightBlinker = self.CS.right_blinker_on |
||||
ret.seatbeltUnlatched = self.CS.seatbelt_unlatched |
||||
|
||||
buttonEvents = [] |
||||
|
||||
# blinkers |
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: |
||||
be = car.CarState.ButtonEvent.new_message() |
||||
be.type = 'leftBlinker' |
||||
be.pressed = self.CS.left_blinker_on |
||||
buttonEvents.append(be) |
||||
|
||||
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: |
||||
be = car.CarState.ButtonEvent.new_message() |
||||
be.type = 'rightBlinker' |
||||
be.pressed = self.CS.right_blinker_on |
||||
buttonEvents.append(be) |
||||
|
||||
be = car.CarState.ButtonEvent.new_message() |
||||
be.type = 'accelCruise' |
||||
buttonEvents.append(be) |
||||
|
||||
|
||||
events = [] |
||||
if not self.CS.can_valid: |
||||
self.can_invalid_count += 1 |
||||
if self.can_invalid_count >= 5: |
||||
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) |
||||
else: |
||||
self.can_invalid_count = 0 |
||||
|
||||
if ret.seatbeltUnlatched: |
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
||||
|
||||
if self.CS.acc_active and not self.acc_active_prev: |
||||
events.append(create_event('pcmEnable', [ET.ENABLE])) |
||||
if not self.CS.acc_active: |
||||
events.append(create_event('pcmDisable', [ET.USER_DISABLE])) |
||||
|
||||
## handle button presses |
||||
#for b in ret.buttonEvents: |
||||
# # do enable on both accel and decel buttons |
||||
# if b.type in ["accelCruise", "decelCruise"] and not b.pressed: |
||||
# events.append(create_event('buttonEnable', [ET.ENABLE])) |
||||
# # do disable on button down |
||||
# if b.type == "cancel" and b.pressed: |
||||
# events.append(create_event('buttonCancel', [ET.USER_DISABLE])) |
||||
|
||||
ret.events = events |
||||
|
||||
# update previous brake/gas pressed |
||||
self.acc_active_prev = self.CS.acc_active |
||||
|
||||
|
||||
# cast to reader so it can't be modified |
||||
return ret.as_reader() |
||||
|
||||
def apply(self, c): |
||||
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators) |
||||
self.frame += 1 |
||||
@ -0,0 +1,24 @@ |
||||
#!/usr/bin/env python |
||||
from cereal import car |
||||
import time |
||||
|
||||
|
||||
class RadarInterface(object): |
||||
def __init__(self, CP): |
||||
# radar |
||||
self.pts = {} |
||||
self.delay = 0.1 |
||||
|
||||
def update(self): |
||||
|
||||
ret = car.RadarState.new_message() |
||||
time.sleep(0.05) # radard runs on RI updates |
||||
|
||||
return ret |
||||
|
||||
if __name__ == "__main__": |
||||
RI = RadarInterface(None) |
||||
while 1: |
||||
ret = RI.update() |
||||
print(chr(27) + "[2J") |
||||
print ret |
||||
@ -0,0 +1,18 @@ |
||||
from selfdrive.car import dbc_dict |
||||
|
||||
class CAR: |
||||
IMPREZA = "SUBARU IMPREZA LIMITED 2019" |
||||
|
||||
FINGERPRINTS = { |
||||
CAR.IMPREZA: [{ |
||||
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8 |
||||
}], |
||||
} |
||||
|
||||
STEER_THRESHOLD = { |
||||
CAR.IMPREZA: 80, |
||||
} |
||||
|
||||
DBC = { |
||||
CAR.IMPREZA: dbc_dict('subaru_global_2017', None), |
||||
} |
||||
@ -1 +1 @@ |
||||
#define COMMA_VERSION "0.5.9-release" |
||||
#define COMMA_VERSION "0.5.10-release" |
||||
|
||||
@ -0,0 +1,71 @@ |
||||
import numpy as np |
||||
from collections import defaultdict |
||||
|
||||
from common.numpy_fast import interp |
||||
|
||||
_FCW_A_ACT_V = [-3., -2.] |
||||
_FCW_A_ACT_BP = [0., 30.] |
||||
|
||||
|
||||
class FCWChecker(object): |
||||
def __init__(self): |
||||
self.reset_lead(0.0) |
||||
|
||||
def reset_lead(self, cur_time): |
||||
self.last_fcw_a = 0.0 |
||||
self.v_lead_max = 0.0 |
||||
self.lead_seen_t = cur_time |
||||
self.last_fcw_time = 0.0 |
||||
self.last_min_a = 0.0 |
||||
|
||||
self.counters = defaultdict(lambda: 0) |
||||
|
||||
@staticmethod |
||||
def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead): |
||||
max_ttc = 5.0 |
||||
|
||||
v_rel = v_ego - v_lead |
||||
a_rel = a_ego - a_lead |
||||
|
||||
# assuming that closing gap ARel comes from lead vehicle decel, |
||||
# then limit ARel so that v_lead will get to zero in no sooner than t_decel. |
||||
# This helps underweighting ARel when v_lead is close to zero. |
||||
t_decel = 2. |
||||
a_rel = np.minimum(a_rel, v_lead / t_decel) |
||||
|
||||
# delta of the quadratic equation to solve for ttc |
||||
delta = v_rel**2 + 2 * x_lead * a_rel |
||||
|
||||
# assign an arbitrary high ttc value if there is no solution to ttc |
||||
if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1): |
||||
ttc = max_ttc |
||||
else: |
||||
ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc) |
||||
return ttc |
||||
|
||||
def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers): |
||||
mpc_solution_a = list(mpc_solution[0].a_ego) |
||||
self.last_min_a = min(mpc_solution_a) |
||||
self.v_lead_max = max(self.v_lead_max, v_lead) |
||||
|
||||
if (fcw_lead > 0.99): |
||||
ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead) |
||||
self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0 |
||||
self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0 |
||||
self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0 |
||||
self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0 |
||||
self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33 |
||||
self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0 |
||||
self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0 |
||||
self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0 |
||||
|
||||
a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V) |
||||
a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego) |
||||
|
||||
fcw_allowed = all(c >= 10 for c in self.counters.values()) |
||||
if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time: |
||||
self.last_fcw_time = cur_time |
||||
self.last_fcw_a = self.last_min_a |
||||
return True |
||||
|
||||
return False |
||||
Binary file not shown.
Binary file not shown.
@ -0,0 +1,120 @@ |
||||
import numpy as np |
||||
|
||||
import selfdrive.messaging as messaging |
||||
from selfdrive.swaglog import cloudlog |
||||
from common.realtime import sec_since_boot |
||||
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU |
||||
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py |
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG |
||||
|
||||
|
||||
class LongitudinalMpc(object): |
||||
def __init__(self, mpc_id, live_longitudinal_mpc): |
||||
self.live_longitudinal_mpc = live_longitudinal_mpc |
||||
self.mpc_id = mpc_id |
||||
|
||||
self.setup_mpc() |
||||
self.v_mpc = 0.0 |
||||
self.v_mpc_future = 0.0 |
||||
self.a_mpc = 0.0 |
||||
self.v_cruise = 0.0 |
||||
self.prev_lead_status = False |
||||
self.prev_lead_x = 0.0 |
||||
self.new_lead = False |
||||
|
||||
self.last_cloudlog_t = 0.0 |
||||
|
||||
def send_mpc_solution(self, qp_iterations, calculation_time): |
||||
qp_iterations = max(0, qp_iterations) |
||||
dat = messaging.new_message() |
||||
dat.init('liveLongitudinalMpc') |
||||
dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego) |
||||
dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego) |
||||
dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) |
||||
dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l) |
||||
dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l) |
||||
dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost |
||||
dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau |
||||
dat.liveLongitudinalMpc.qpIterations = qp_iterations |
||||
dat.liveLongitudinalMpc.mpcId = self.mpc_id |
||||
dat.liveLongitudinalMpc.calculationTime = calculation_time |
||||
self.live_longitudinal_mpc.send(dat.to_bytes()) |
||||
|
||||
def setup_mpc(self): |
||||
ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id) |
||||
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, |
||||
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) |
||||
|
||||
self.mpc_solution = ffi.new("log_t *") |
||||
self.cur_state = ffi.new("state_t *") |
||||
self.cur_state[0].v_ego = 0 |
||||
self.cur_state[0].a_ego = 0 |
||||
self.a_lead_tau = _LEAD_ACCEL_TAU |
||||
|
||||
def set_cur_state(self, v, a): |
||||
self.cur_state[0].v_ego = v |
||||
self.cur_state[0].a_ego = a |
||||
|
||||
def update(self, CS, lead, v_cruise_setpoint): |
||||
v_ego = CS.carState.vEgo |
||||
|
||||
# Setup current mpc state |
||||
self.cur_state[0].x_ego = 0.0 |
||||
|
||||
if lead is not None and lead.status: |
||||
x_lead = lead.dRel |
||||
v_lead = max(0.0, lead.vLead) |
||||
a_lead = lead.aLeadK |
||||
|
||||
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): |
||||
v_lead = 0.0 |
||||
a_lead = 0.0 |
||||
|
||||
self.a_lead_tau = lead.aLeadTau |
||||
self.new_lead = False |
||||
if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: |
||||
self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau) |
||||
self.new_lead = True |
||||
|
||||
self.prev_lead_status = True |
||||
self.prev_lead_x = x_lead |
||||
self.cur_state[0].x_l = x_lead |
||||
self.cur_state[0].v_l = v_lead |
||||
else: |
||||
self.prev_lead_status = False |
||||
# Fake a fast lead car, so mpc keeps running |
||||
self.cur_state[0].x_l = 50.0 |
||||
self.cur_state[0].v_l = v_ego + 10.0 |
||||
a_lead = 0.0 |
||||
self.a_lead_tau = _LEAD_ACCEL_TAU |
||||
|
||||
# Calculate mpc |
||||
t = sec_since_boot() |
||||
n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) |
||||
duration = int((sec_since_boot() - t) * 1e9) |
||||
self.send_mpc_solution(n_its, duration) |
||||
|
||||
# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed |
||||
self.v_mpc = self.mpc_solution[0].v_ego[1] |
||||
self.a_mpc = self.mpc_solution[0].a_ego[1] |
||||
self.v_mpc_future = self.mpc_solution[0].v_ego[10] |
||||
|
||||
# Reset if NaN or goes through lead car |
||||
dls = np.array(list(self.mpc_solution[0].x_l)) - np.array(list(self.mpc_solution[0].x_ego)) |
||||
crashing = min(dls) < -50.0 |
||||
nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego))) |
||||
backwards = min(list(self.mpc_solution[0].v_ego)) < -0.01 |
||||
|
||||
if ((backwards or crashing) and self.prev_lead_status) or nans: |
||||
if t > self.last_cloudlog_t + 5.0: |
||||
self.last_cloudlog_t = t |
||||
cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % ( |
||||
self.mpc_id, backwards, crashing, nans)) |
||||
|
||||
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, |
||||
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) |
||||
self.cur_state[0].v_ego = v_ego |
||||
self.cur_state[0].a_ego = 0.0 |
||||
self.v_mpc = v_ego |
||||
self.a_mpc = CS.carState.aEgo |
||||
self.prev_lead_status = False |
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -0,0 +1,124 @@ |
||||
#include <eigen3/Eigen/Dense> |
||||
#include <iostream> |
||||
|
||||
typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM; |
||||
typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM; |
||||
typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM; |
||||
|
||||
void predict(double *in_x, double *in_P, double *in_Q, double dt) { |
||||
typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> RRM; |
||||
|
||||
double nx[DIM] = {0}; |
||||
double in_F[EDIM*EDIM] = {0}; |
||||
|
||||
// functions from sympy
|
||||
f_fun(in_x, dt, nx); |
||||
F_fun(in_x, dt, in_F); |
||||
|
||||
|
||||
EEM F(in_F); |
||||
EEM P(in_P); |
||||
EEM Q(in_Q); |
||||
|
||||
RRM F_main = F.topLeftCorner(MEDIM, MEDIM); |
||||
P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose(); |
||||
P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM); |
||||
P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose(); |
||||
|
||||
P = P + dt*Q; |
||||
|
||||
// copy out state
|
||||
memcpy(in_x, nx, DIM * sizeof(double)); |
||||
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); |
||||
} |
||||
|
||||
// note: extra_args dim only correct when null space projecting
|
||||
// otherwise 1
|
||||
template <int ZDIM, int EADIM, bool MAHA_TEST> |
||||
void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) { |
||||
typedef Eigen::Matrix<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM; |
||||
typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM; |
||||
typedef Eigen::Matrix<double, ZDIM, EDIM, Eigen::RowMajor> ZEM; |
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM; |
||||
typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM; |
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M; |
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXM; |
||||
|
||||
double in_hx[ZDIM] = {0}; |
||||
double in_H[ZDIM * DIM] = {0}; |
||||
double in_H_mod[EDIM * DIM] = {0}; |
||||
double delta_x[EDIM] = {0}; |
||||
double x_new[DIM] = {0}; |
||||
|
||||
|
||||
// state x, P
|
||||
Eigen::Matrix<double, ZDIM, 1> z(in_z); |
||||
EEM P(in_P); |
||||
ZZM pre_R(in_R); |
||||
|
||||
// functions from sympy
|
||||
h_fun(in_x, in_ea, in_hx); |
||||
H_fun(in_x, in_ea, in_H); |
||||
ZDM pre_H(in_H);
|
||||
|
||||
// get y (y = z - hx)
|
||||
Eigen::Matrix<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y; |
||||
X1M y; XXM H; XXM R; |
||||
if (Hea_fun){ |
||||
typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> ZAM; |
||||
double in_Hea[ZDIM * EADIM] = {0}; |
||||
Hea_fun(in_x, in_ea, in_Hea); |
||||
ZAM Hea(in_Hea); |
||||
XXM A = Hea.transpose().fullPivLu().kernel(); |
||||
|
||||
|
||||
y = A.transpose() * pre_y; |
||||
H = A.transpose() * pre_H; |
||||
R = A.transpose() * pre_R * A; |
||||
} else { |
||||
y = pre_y; |
||||
H = pre_H; |
||||
R = pre_R; |
||||
} |
||||
// get modified H
|
||||
H_mod_fun(in_x, in_H_mod); |
||||
DEM H_mod(in_H_mod); |
||||
XEM H_err = H * H_mod; |
||||
|
||||
// Do mahalobis distance test
|
||||
if (MAHA_TEST){ |
||||
XXM a = (H_err * P * H_err.transpose() + R).inverse(); |
||||
double maha_dist = y.transpose() * a * y; |
||||
if (maha_dist > MAHA_THRESHOLD){ |
||||
R = 1.0e16 * R; |
||||
} |
||||
} |
||||
|
||||
// Outlier resilient weighting
|
||||
double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
|
||||
|
||||
// kalman gains and I_KH
|
||||
XXM S = ((H_err * P) * H_err.transpose()) + R/weight; |
||||
XEM KT = S.fullPivLu().solve(H_err * P.transpose()); |
||||
//EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
|
||||
//EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
|
||||
//std::cout << "Here is the matrix rot:\n" << K << std::endl;
|
||||
EEM I_KH = Eigen::Matrix<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err); |
||||
|
||||
// update state by injecting dx
|
||||
Eigen::Matrix<double, EDIM, 1> dx(delta_x); |
||||
dx = (KT.transpose() * y); |
||||
memcpy(delta_x, dx.data(), EDIM * sizeof(double)); |
||||
err_fun(in_x, delta_x, x_new); |
||||
Eigen::Matrix<double, DIM, 1> x(x_new); |
||||
|
||||
// update cov
|
||||
P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT); |
||||
|
||||
// copy out state
|
||||
memcpy(in_x, x.data(), DIM * sizeof(double)); |
||||
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double)); |
||||
memcpy(in_z, y.data(), y.rows() * sizeof(double)); |
||||
} |
||||
|
||||
|
||||
@ -0,0 +1,564 @@ |
||||
import os |
||||
from bisect import bisect_right |
||||
import sympy as sp |
||||
import numpy as np |
||||
from numpy import dot |
||||
from common.ffi_wrapper import compile_code, wrap_compiled |
||||
from common.sympy_helpers import sympy_into_c |
||||
import scipy |
||||
from scipy.stats import chi2 |
||||
|
||||
|
||||
EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__)) |
||||
|
||||
def solve(a, b): |
||||
if a.shape[0] == 1 and a.shape[1] == 1: |
||||
#assert np.allclose(b/a[0][0], np.linalg.solve(a, b)) |
||||
return b/a[0][0] |
||||
else: |
||||
return np.linalg.solve(a, b) |
||||
|
||||
def null(H, eps=1e-12): |
||||
from scipy import linalg |
||||
u, s, vh = linalg.svd(H) |
||||
padding = max(0,np.shape(H)[1]-np.shape(s)[0]) |
||||
null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0) |
||||
null_space = scipy.compress(null_mask, vh, axis=0) |
||||
return scipy.transpose(null_space) |
||||
|
||||
def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]): |
||||
# optional state transition matrix, H modifier |
||||
# and err_function if an error-state kalman filter (ESKF) |
||||
# is desired. Best described in "Quaternion kinematics |
||||
# for the error-state Kalman filter" by Joan Sola |
||||
|
||||
if eskf_params: |
||||
err_eqs = eskf_params[0] |
||||
inv_err_eqs = eskf_params[1] |
||||
H_mod_sym = eskf_params[2] |
||||
f_err_sym = eskf_params[3] |
||||
x_err_sym = eskf_params[4] |
||||
else: |
||||
nom_x = sp.MatrixSymbol('nom_x',dim_x,1) |
||||
true_x = sp.MatrixSymbol('true_x',dim_x,1) |
||||
delta_x = sp.MatrixSymbol('delta_x',dim_x,1) |
||||
err_function_sym = sp.Matrix(nom_x + delta_x) |
||||
inv_err_function_sym = sp.Matrix(true_x - nom_x) |
||||
err_eqs = [err_function_sym, nom_x, delta_x] |
||||
inv_err_eqs = [inv_err_function_sym, nom_x, true_x] |
||||
|
||||
H_mod_sym = sp.Matrix(np.eye(dim_x)) |
||||
f_err_sym = f_sym |
||||
x_err_sym = x_sym |
||||
|
||||
# This configures the multi-state augmentation |
||||
# needed for EKF-SLAM with MSCKF (Mourikis et al 2007) |
||||
if msckf_params: |
||||
msckf = True |
||||
dim_main = msckf_params[0] # size of the main state |
||||
dim_augment = msckf_params[1] # size of one augment state chunk |
||||
dim_main_err = msckf_params[2] |
||||
dim_augment_err = msckf_params[3] |
||||
N = msckf_params[4] |
||||
feature_track_kinds = msckf_params[5] |
||||
assert dim_main + dim_augment*N == dim_x |
||||
assert dim_main_err + dim_augment_err*N == dim_err |
||||
else: |
||||
msckf = False |
||||
dim_main = dim_x |
||||
dim_augment = 0 |
||||
dim_main_err = dim_err |
||||
dim_augment_err = 0 |
||||
N = 0 |
||||
|
||||
# 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) |
||||
for i in xrange(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: |
||||
obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2])) |
||||
else: |
||||
obs_eqs[i].append(None) |
||||
|
||||
# collect sympy functions |
||||
sympy_functions = [] |
||||
|
||||
# error functions |
||||
sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]])) |
||||
sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]])) |
||||
|
||||
# H modifier for ESKF updates |
||||
sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym])) |
||||
|
||||
# state propagation function |
||||
sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym])) |
||||
sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym])) |
||||
|
||||
# observation functions |
||||
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs: |
||||
sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym])) |
||||
sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym])) |
||||
if msckf and kind in feature_track_kinds: |
||||
sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym])) |
||||
|
||||
# Generate and wrap all th c code |
||||
header, code = sympy_into_c(sympy_functions) |
||||
extra_header = "#define DIM %d\n" % dim_x |
||||
extra_header += "#define EDIM %d\n" % dim_err |
||||
extra_header += "#define MEDIM %d\n" % dim_main_err |
||||
extra_header += "typedef void (*Hfun)(double *, double *, double *);\n" |
||||
|
||||
extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);" |
||||
|
||||
extra_post = "" |
||||
|
||||
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs: |
||||
if msckf and kind in feature_track_kinds: |
||||
He_str = 'He_%d' % kind |
||||
# ea_dim = ea_sym.shape[0] |
||||
else: |
||||
He_str = 'NULL' |
||||
# ea_dim = 1 # not really dim of ea but makes c function work |
||||
maha_thresh = chi2.ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection |
||||
maha_test = kind in maha_test_kinds |
||||
extra_post += """ |
||||
void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { |
||||
update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d); |
||||
} |
||||
""" % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind) |
||||
extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh) |
||||
extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind |
||||
|
||||
code += "\n" + extra_header |
||||
code += "\n" + open(os.path.join(EXTERNAL_PATH, "ekf_c.c")).read() |
||||
code += "\n" + extra_post |
||||
header += "\n" + extra_header |
||||
compile_code(name, code, header, EXTERNAL_PATH) |
||||
|
||||
class EKF_sym(object): |
||||
def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err, |
||||
N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]): |
||||
''' |
||||
Generates process function and all |
||||
observation functions for the kalman |
||||
filter. |
||||
''' |
||||
if N > 0: |
||||
self.msckf = True |
||||
else: |
||||
self.msckf = False |
||||
self.N = N |
||||
self.dim_augment = dim_augment |
||||
self.dim_augment_err = dim_augment_err |
||||
self.dim_main = dim_main |
||||
self.dim_main_err = dim_main_err |
||||
|
||||
# state |
||||
x_initial = x_initial.reshape((-1, 1)) |
||||
self.dim_x = x_initial.shape[0] |
||||
self.dim_err = P_initial.shape[0] |
||||
assert dim_main + dim_augment*N == self.dim_x |
||||
assert dim_main_err + dim_augment_err*N == self.dim_err |
||||
|
||||
# kinds that should get mahalanobis distance |
||||
# tested for outlier rejection |
||||
self.maha_test_kinds = maha_test_kinds |
||||
|
||||
# process noise |
||||
self.Q = Q |
||||
|
||||
# rewind stuff |
||||
self.rewind_t = [] |
||||
self.rewind_states = [] |
||||
self.rewind_obscache = [] |
||||
self.init_state(x_initial, P_initial, None) |
||||
|
||||
ffi, lib = wrap_compiled(name, EXTERNAL_PATH) |
||||
kinds, self.feature_track_kinds = [], [] |
||||
for func in dir(lib): |
||||
if func[:2] == 'h_': |
||||
kinds.append(int(func[2:])) |
||||
if func[:3] == 'He_': |
||||
self.feature_track_kinds.append(int(func[3:])) |
||||
|
||||
# wrap all the sympy functions |
||||
def wrap_1lists(name): |
||||
func = eval("lib.%s" % name, {"lib":lib}) |
||||
def ret(lst1, out): |
||||
func(ffi.cast("double *", lst1.ctypes.data), |
||||
ffi.cast("double *", out.ctypes.data)) |
||||
return ret |
||||
def wrap_2lists(name): |
||||
func = eval("lib.%s" % name, {"lib":lib}) |
||||
def ret(lst1, lst2, out): |
||||
func(ffi.cast("double *", lst1.ctypes.data), |
||||
ffi.cast("double *", lst2.ctypes.data), |
||||
ffi.cast("double *", out.ctypes.data)) |
||||
return ret |
||||
def wrap_1list_1float(name): |
||||
func = eval("lib.%s" % name, {"lib":lib}) |
||||
def ret(lst1, fl, out): |
||||
func(ffi.cast("double *", lst1.ctypes.data), |
||||
ffi.cast("double", fl), |
||||
ffi.cast("double *", out.ctypes.data)) |
||||
return ret |
||||
|
||||
self.f = wrap_1list_1float("f_fun") |
||||
self.F = wrap_1list_1float("F_fun") |
||||
|
||||
self.err_function = wrap_2lists("err_fun") |
||||
self.inv_err_function = wrap_2lists("inv_err_fun") |
||||
self.H_mod = wrap_1lists("H_mod_fun") |
||||
|
||||
self.hs, self.Hs, self.Hes = {}, {}, {} |
||||
for kind in kinds: |
||||
self.hs[kind] = wrap_2lists("h_%d" % kind) |
||||
self.Hs[kind] = wrap_2lists("H_%d" % kind) |
||||
if self.msckf and kind in self.feature_track_kinds: |
||||
self.Hes[kind] = wrap_2lists("He_%d" % kind) |
||||
|
||||
# wrap the C++ predict function |
||||
def _predict_blas(x, P, dt): |
||||
lib.predict(ffi.cast("double *", x.ctypes.data), |
||||
ffi.cast("double *", P.ctypes.data), |
||||
ffi.cast("double *", self.Q.ctypes.data), |
||||
ffi.cast("double", dt)) |
||||
return x, P |
||||
|
||||
# wrap the C++ update function |
||||
def fun_wrapper(f, kind): |
||||
f = eval("lib.%s" % f, {"lib": lib}) |
||||
def _update_inner_blas(x, P, z, R, extra_args): |
||||
f(ffi.cast("double *", x.ctypes.data), |
||||
ffi.cast("double *", P.ctypes.data), |
||||
ffi.cast("double *", z.ctypes.data), |
||||
ffi.cast("double *", R.ctypes.data), |
||||
ffi.cast("double *", extra_args.ctypes.data)) |
||||
if self.msckf and kind in self.feature_track_kinds: |
||||
y = z[:-len(extra_args)] |
||||
else: |
||||
y = z |
||||
return x, P, y |
||||
return _update_inner_blas |
||||
|
||||
self._updates = {} |
||||
for kind in kinds: |
||||
self._updates[kind] = fun_wrapper("update_%d" % kind, kind) |
||||
|
||||
def _update_blas(x, P, kind, z, R, extra_args=[]): |
||||
return self._updates[kind](x, P, z, R, extra_args) |
||||
|
||||
# assign the functions |
||||
self._predict = _predict_blas |
||||
#self._predict = self._predict_python |
||||
self._update = _update_blas |
||||
#self._update = self._update_python |
||||
|
||||
|
||||
def init_state(self, state, covs, filter_time): |
||||
self.x = np.array(state.reshape((-1, 1))).astype(np.float64) |
||||
self.P = np.array(covs).astype(np.float64) |
||||
self.filter_time = filter_time |
||||
self.augment_times = [0]*self.N |
||||
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. |
||||
assert self.msckf |
||||
d1 = self.dim_main |
||||
d2 = self.dim_main_err |
||||
d3 = self.dim_augment |
||||
d4 = self.dim_augment_err |
||||
# push through augmented states |
||||
self.x[d1:-d3] = self.x[d1+d3:] |
||||
self.x[-d3:] = self.x[:d3] |
||||
assert self.x.shape == (self.dim_x, 1) |
||||
# push through augmented covs |
||||
assert self.P.shape == (self.dim_err, self.dim_err) |
||||
P_reduced = self.P |
||||
P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=1) |
||||
P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=0) |
||||
assert P_reduced.shape == (self.dim_err -d4, self.dim_err -d4) |
||||
to_mult = np.zeros((self.dim_err, self.dim_err - d4)) |
||||
to_mult[:-d4,:] = np.eye(self.dim_err - d4) |
||||
to_mult[-d4:,:d4] = np.eye(d4) |
||||
self.P = to_mult.dot(P_reduced.dot(to_mult.T)) |
||||
self.augment_times = self.augment_times[1:] |
||||
self.augment_times.append(self.filter_time) |
||||
assert self.P.shape == (self.dim_err, self.dim_err) |
||||
|
||||
def state(self): |
||||
return np.array(self.x).flatten() |
||||
|
||||
def covs(self): |
||||
return self.P |
||||
|
||||
def rewind(self, t): |
||||
# find where we are rewinding to |
||||
idx = bisect_right(self.rewind_t, t) |
||||
assert self.rewind_t[idx-1] <= t |
||||
assert self.rewind_t[idx] > t # must be true, or rewind wouldn't be called |
||||
|
||||
# set the state to the time right before that |
||||
self.filter_time = self.rewind_t[idx-1] |
||||
self.x[:] = self.rewind_states[idx-1][0] |
||||
self.P[:] = self.rewind_states[idx-1][1] |
||||
|
||||
# return the observations we rewound over for fast forwarding |
||||
ret = self.rewind_obscache[idx:] |
||||
|
||||
# throw away the old future |
||||
# TODO: is this making a copy? |
||||
self.rewind_t = self.rewind_t[:idx] |
||||
self.rewind_states = self.rewind_states[:idx] |
||||
self.rewind_obscache = self.rewind_obscache[:idx] |
||||
|
||||
return ret |
||||
|
||||
def checkpoint(self, obs): |
||||
# push to rewinder |
||||
self.rewind_t.append(self.filter_time) |
||||
self.rewind_states.append((np.copy(self.x), np.copy(self.P))) |
||||
self.rewind_obscache.append(obs) |
||||
|
||||
# only keep a certain number around |
||||
REWIND_TO_KEEP = 512 |
||||
self.rewind_t = self.rewind_t[-REWIND_TO_KEEP:] |
||||
self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:] |
||||
self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:] |
||||
|
||||
def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False): |
||||
# TODO handle rewinding at this level" |
||||
|
||||
# rewind |
||||
if t < self.filter_time: |
||||
if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] -1.0: |
||||
print "observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time) |
||||
return None |
||||
rewound = self.rewind(t) |
||||
else: |
||||
rewound = [] |
||||
|
||||
ret = self._predict_and_update_batch(t, kind, z, R, extra_args, augment) |
||||
|
||||
# optional fast forward |
||||
for r in rewound: |
||||
self._predict_and_update_batch(*r) |
||||
|
||||
return ret |
||||
|
||||
def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False): |
||||
"""The main kalman filter function |
||||
Predicts the state and then updates a batch of observations |
||||
|
||||
dim_x: dimensionality of the state space |
||||
dim_z: dimensionality of the observation and depends on kind |
||||
n: number of observations |
||||
|
||||
Args: |
||||
t (float): Time of observation |
||||
kind (int): Type of observation |
||||
z (vec [n,dim_z]): Measurements |
||||
R (mat [n,dim_z, dim_z]): Measurement Noise |
||||
extra_args (list, [n]): Values used in H computations |
||||
""" |
||||
# 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 |
||||
xk_km1, Pk_km1 = np.copy(self.x).flatten(), np.copy(self.P) |
||||
|
||||
# update batch |
||||
y = [] |
||||
for i in xrange(len(z)): |
||||
# these are from the user, so we canonicalize them |
||||
z_i = np.array(z[i], dtype=np.float64, order='F') |
||||
R_i = np.array(R[i], dtype=np.float64, order='F') |
||||
extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F') |
||||
# update |
||||
self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i) |
||||
y.append(y_i) |
||||
xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P) |
||||
|
||||
if augment: |
||||
self.augment() |
||||
|
||||
# checkpoint |
||||
self.checkpoint((t, kind, z, R, extra_args)) |
||||
|
||||
return xk_km1, xk_k, Pk_km1, Pk_k, t, kind, y, z, extra_args |
||||
|
||||
def _predict_python(self, x, P, dt): |
||||
x_new = np.zeros(x.shape, dtype=np.float64) |
||||
self.f(x, dt, x_new) |
||||
|
||||
F = np.zeros(P.shape, dtype=np.float64) |
||||
self.F(x, dt, F) |
||||
|
||||
if not self.msckf: |
||||
P = dot(dot(F, P), F.T) |
||||
else: |
||||
# Update the predicted state covariance: |
||||
# Pk+1|k = |F*Pii*FT + Q*dt F*Pij | |
||||
# |PijT*FT Pjj | |
||||
# Where F is the jacobian of the main state |
||||
# predict function, Pii is the main state's |
||||
# covariance and Q its process noise. Pij |
||||
# is the covariance between the augmented |
||||
# states and the main state. |
||||
# |
||||
d2 = self.dim_main_err # known at compile time |
||||
F_curr = F[:d2, :d2] |
||||
P[:d2, :d2] = (F_curr.dot(P[:d2, :d2])).dot(F_curr.T) |
||||
P[:d2, d2:] = F_curr.dot(P[:d2, d2:]) |
||||
P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T) |
||||
|
||||
P += dt*self.Q |
||||
return x_new, P |
||||
|
||||
def _update_python(self, x, P, kind, z, R, extra_args=[]): |
||||
# init vars |
||||
z = z.reshape((-1, 1)) |
||||
h = np.zeros(z.shape, dtype=np.float64) |
||||
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64) |
||||
|
||||
# C functions |
||||
self.hs[kind](x, extra_args, h) |
||||
self.Hs[kind](x, extra_args, H) |
||||
|
||||
# y is the "loss" |
||||
y = z - h |
||||
|
||||
# *** same above this line *** |
||||
|
||||
if self.msckf and kind in self.Hes: |
||||
# Do some algebraic magic to decorrelate |
||||
He = np.zeros((z.shape[0], len(extra_args)), dtype=np.float64) |
||||
self.Hes[kind](x, extra_args, He) |
||||
|
||||
# TODO: Don't call a function here, do projection locally |
||||
A = null(He.T) |
||||
|
||||
y = A.T.dot(y) |
||||
H = A.T.dot(H) |
||||
R = A.T.dot(R.dot(A)) |
||||
|
||||
# TODO If nullspace isn't the dimension we want |
||||
if A.shape[1] + He.shape[1] != A.shape[0]: |
||||
print 'Warning: null space projection failed, measurement ignored' |
||||
return x, P, np.zeros(A.shape[0] - He.shape[1]) |
||||
|
||||
# if using eskf |
||||
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64) |
||||
self.H_mod(x, H_mod) |
||||
H = H.dot(H_mod) |
||||
|
||||
# Do mahalobis distance test |
||||
# currently just runs on msckf observations |
||||
# could run on anything if needed |
||||
if self.msckf and kind in self.maha_test_kinds: |
||||
a = np.linalg.inv(H.dot(P).dot(H.T) + R) |
||||
maha_dist = y.T.dot(a.dot(y)) |
||||
if maha_dist > chi2.ppf(0.95, y.shape[0]): |
||||
R = 10e16*R |
||||
|
||||
# *** same below this line *** |
||||
|
||||
# Outlier resilient weighting as described in: |
||||
# "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..." |
||||
weight = 1 #(1.5)/(1 + np.sum(y**2)/np.sum(R)) |
||||
|
||||
S = dot(dot(H, P), H.T) + R/weight |
||||
K = solve(S, dot(H, P.T)).T |
||||
I_KH = np.eye(P.shape[0]) - dot(K, H) |
||||
|
||||
# update actual state |
||||
delta_x = dot(K, y) |
||||
P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T) |
||||
|
||||
# inject observed error into state |
||||
x_new = np.zeros(x.shape, dtype=np.float64) |
||||
self.err_function(x, delta_x, x_new) |
||||
return x_new, P, y.flatten() |
||||
|
||||
def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95): |
||||
# init vars |
||||
z = z.reshape((-1, 1)) |
||||
h = np.zeros(z.shape, dtype=np.float64) |
||||
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64) |
||||
|
||||
# C functions |
||||
self.hs[kind](x, extra_args, h) |
||||
self.Hs[kind](x, extra_args, H) |
||||
|
||||
# y is the "loss" |
||||
y = z - h |
||||
|
||||
# if using eskf |
||||
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64) |
||||
self.H_mod(x, H_mod) |
||||
H = H.dot(H_mod) |
||||
|
||||
a = np.linalg.inv(H.dot(P).dot(H.T) + R) |
||||
maha_dist = y.T.dot(a.dot(y)) |
||||
if maha_dist > chi2.ppf(maha_thresh, y.shape[0]): |
||||
return False |
||||
else: |
||||
return True |
||||
|
||||
|
||||
|
||||
|
||||
def rts_smooth(self, estimates, norm_quats=False): |
||||
''' |
||||
Returns rts smoothed results of |
||||
kalman filter estimates |
||||
|
||||
If the kalman state is augmented with |
||||
old states only the main state is smoothed |
||||
''' |
||||
xk_n = estimates[-1][0] |
||||
Pk_n = estimates[-1][2] |
||||
Fk_1 = np.zeros(Pk_n.shape, dtype=np.float64) |
||||
|
||||
states_smoothed = [xk_n] |
||||
covs_smoothed = [Pk_n] |
||||
for k in xrange(len(estimates) - 2, -1, -1): |
||||
xk1_n = xk_n |
||||
if norm_quats: |
||||
xk1_n[3:7] /= np.linalg.norm(xk1_n[3:7]) |
||||
Pk1_n = Pk_n |
||||
|
||||
xk1_k, _, Pk1_k, _, t2, _, _, _, _ = estimates[k + 1] |
||||
_, xk_k, _, Pk_k, t1, _, _, _, _ = estimates[k] |
||||
dt = t2 - t1 |
||||
self.F(xk_k, dt, Fk_1) |
||||
|
||||
d1 = self.dim_main |
||||
d2 = self.dim_main_err |
||||
Ck = np.linalg.solve(Pk1_k[:d2,:d2], Fk_1[:d2,:d2].dot(Pk_k[:d2,:d2].T)).T |
||||
xk_n = xk_k |
||||
delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64) |
||||
self.inv_err_function(xk1_k, xk1_n, delta_x) |
||||
delta_x[:d2] = Ck.dot(delta_x[:d2]) |
||||
x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64) |
||||
self.err_function(xk_k, delta_x, x_new) |
||||
xk_n[:d1] = x_new[:d1,0] |
||||
Pk_n = Pk_k |
||||
Pk_n[:d2,:d2] = Pk_k[:d2,:d2] + Ck.dot(Pk1_n[:d2,:d2] - Pk1_k[:d2,:d2]).dot(Ck.T) |
||||
states_smoothed.append(xk_n) |
||||
covs_smoothed.append(Pk_n) |
||||
|
||||
return np.flipud(np.vstack(states_smoothed)), np.stack(covs_smoothed, 0)[::-1] |
||||
@ -0,0 +1,165 @@ |
||||
import numpy as np |
||||
import os |
||||
from bisect import bisect |
||||
from tqdm import tqdm |
||||
|
||||
|
||||
class ObservationKind(object): |
||||
UNKNOWN = 0 |
||||
NO_OBSERVATION = 1 |
||||
GPS_NED = 2 |
||||
ODOMETRIC_SPEED = 3 |
||||
PHONE_GYRO = 4 |
||||
GPS_VEL = 5 |
||||
PSEUDORANGE_GPS = 6 |
||||
PSEUDORANGE_RATE_GPS = 7 |
||||
SPEED = 8 |
||||
NO_ROT = 9 |
||||
PHONE_ACCEL = 10 |
||||
ORB_POINT = 11 |
||||
ECEF_POS = 12 |
||||
CAMERA_ODO_TRANSLATION = 13 |
||||
CAMERA_ODO_ROTATION = 14 |
||||
ORB_FEATURES = 15 |
||||
MSCKF_TEST = 16 |
||||
FEATURE_TRACK_TEST = 17 |
||||
LANE_PT = 18 |
||||
IMU_FRAME = 19 |
||||
PSEUDORANGE_GLONASS = 20 |
||||
PSEUDORANGE_RATE_GLONASS = 21 |
||||
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'] |
||||
|
||||
@classmethod |
||||
def to_string(cls, kind): |
||||
return cls.names[kind] |
||||
|
||||
|
||||
|
||||
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS, |
||||
ObservationKind.PSEUDORANGE_RATE_GPS, |
||||
ObservationKind.PSEUDORANGE_GLONASS, |
||||
ObservationKind.PSEUDORANGE_RATE_GLONASS] |
||||
|
||||
|
||||
def run_car_ekf_offline(kf, observations_by_kind): |
||||
from laika.raw_gnss import GNSSMeasurement |
||||
observations = [] |
||||
# create list of observations with element format: [kind, time, data] |
||||
for kind in observations_by_kind: |
||||
for t, data in zip(observations_by_kind[kind][0], observations_by_kind[kind][1]): |
||||
observations.append([t, kind, data]) |
||||
observations.sort(key=lambda obs: obs[0]) |
||||
|
||||
times, estimates = run_observations_through_filter(kf, observations) |
||||
|
||||
forward_states = np.stack(e[1] for e in estimates) |
||||
forward_covs = np.stack(e[3] for e in estimates) |
||||
smoothed_states, smoothed_covs = kf.rts_smooth(estimates) |
||||
|
||||
observations_dict = {} |
||||
# TODO assuming observations and estimates |
||||
# are same length may not work with VO |
||||
for e in estimates: |
||||
t = e[4] |
||||
kind = str(int(e[5])) |
||||
res = e[6] |
||||
z = e[7] |
||||
ea = e[8] |
||||
if len(z) == 0: |
||||
continue |
||||
if kind not in observations_dict: |
||||
observations_dict[kind] = {} |
||||
observations_dict[kind]['t'] = np.array(len(z)*[t]) |
||||
observations_dict[kind]['z'] = np.array(z) |
||||
observations_dict[kind]['ea'] = np.array(ea) |
||||
observations_dict[kind]['residual'] = np.array(res) |
||||
else: |
||||
observations_dict[kind]['t'] = np.append(observations_dict[kind]['t'], np.array(len(z)*[t])) |
||||
observations_dict[kind]['z'] = np.vstack((observations_dict[kind]['z'], np.array(z))) |
||||
observations_dict[kind]['ea'] = np.vstack((observations_dict[kind]['ea'], np.array(ea))) |
||||
observations_dict[kind]['residual'] = np.vstack((observations_dict[kind]['residual'], np.array(res))) |
||||
|
||||
# add svIds to gnss data |
||||
for kind in map(str, SAT_OBS): |
||||
if int(kind) in observations_by_kind and kind in observations_dict: |
||||
observations_dict[kind]['svIds'] = np.array([]) |
||||
observations_dict[kind]['CNO'] = np.array([]) |
||||
observations_dict[kind]['std'] = np.array([]) |
||||
for obs in observations_by_kind[int(kind)][1]: |
||||
observations_dict[kind]['svIds'] = np.append(observations_dict[kind]['svIds'], |
||||
np.array([obs[:,GNSSMeasurement.PRN]])) |
||||
observations_dict[kind]['std'] = np.append(observations_dict[kind]['std'], |
||||
np.array([obs[:,GNSSMeasurement.PR_STD]])) |
||||
return smoothed_states, smoothed_covs, forward_states, forward_covs, times, observations_dict |
||||
|
||||
|
||||
def run_observations_through_filter(kf, observations, filter_time=None): |
||||
estimates = [] |
||||
|
||||
for obs in tqdm(observations): |
||||
t = obs[0] |
||||
kind = obs[1] |
||||
data = obs[2] |
||||
estimates.append(kf.predict_and_observe(t, kind, data)) |
||||
times = [x[4] for x in estimates] |
||||
return times, estimates |
||||
|
||||
|
||||
def save_residuals_plot(obs, save_path, data_name): |
||||
import matplotlib.pyplot as plt |
||||
import mpld3 |
||||
fig = plt.figure(figsize=(10,20)) |
||||
fig.suptitle('Residuals of ' + data_name, fontsize=24) |
||||
n = len(obs.keys()) |
||||
start_times = [obs[kind]['t'][0] for kind in obs] |
||||
start_time = min(start_times) |
||||
xlims = [start_time + 3, start_time + 60] |
||||
|
||||
for i, kind in enumerate(obs): |
||||
ax = fig.add_subplot(n, 1, i+1) |
||||
ax.set_xlim(xlims) |
||||
t = obs[kind]['t'] |
||||
res = obs[kind]['residual'] |
||||
start_idx = bisect(t, xlims[0]) |
||||
if len(res) == start_idx: |
||||
continue |
||||
ylim = max(np.linalg.norm(res[start_idx:], axis=1)) |
||||
ax.set_ylim([-ylim, ylim]) |
||||
if int(kind) in SAT_OBS: |
||||
svIds = obs[kind]['svIds'] |
||||
for svId in set(svIds): |
||||
svId_idx = (svIds == svId) |
||||
t = obs[kind]['t'][svId_idx] |
||||
res = obs[kind]['residual'][svId_idx] |
||||
ax.plot(t, res, label='SV ' + str(int(svId))) |
||||
ax.legend(loc='right') |
||||
else: |
||||
ax.plot(t, res) |
||||
plt.title('Residual of kind ' + ObservationKind.to_string(int(kind)), fontsize=20) |
||||
plt.tight_layout() |
||||
os.makedirs(save_path) |
||||
mpld3.save_html(fig, save_path + 'residuals_plot.html') |
||||
@ -0,0 +1,128 @@ |
||||
#!/usr/bin/env python |
||||
import numpy as np |
||||
import loc_local_model |
||||
|
||||
from kalman_helpers import ObservationKind |
||||
from ekf_sym import EKF_sym |
||||
|
||||
|
||||
|
||||
class States(object): |
||||
VELOCITY = slice(0,3) # device frame velocity in m/s |
||||
ANGULAR_VELOCITY = slice(3, 6) # roll, pitch and yaw rates in device frame in radians/s |
||||
GYRO_BIAS = slice(6, 9) # roll, pitch and yaw biases |
||||
ODO_SCALE = slice(9, 10) # odometer scale |
||||
ACCELERATION = slice(10, 13) # Acceleration in device frame in m/s**2 |
||||
|
||||
|
||||
class LocLocalKalman(object): |
||||
def __init__(self): |
||||
x_initial = np.array([0, 0, 0, |
||||
0, 0, 0, |
||||
0, 0, 0, |
||||
1, |
||||
0, 0, 0]) |
||||
|
||||
# state covariance |
||||
P_initial = np.diag([10**2, 10**2, 10**2, |
||||
1**2, 1**2, 1**2, |
||||
0.05**2, 0.05**2, 0.05**2, |
||||
0.02**2, |
||||
1**2, 1**2, 1**2]) |
||||
|
||||
# process noise |
||||
Q = np.diag([0.0**2, 0.0**2, 0.0**2, |
||||
.01**2, .01**2, .01**2, |
||||
(0.005/100)**2, (0.005/100)**2, (0.005/100)**2, |
||||
(0.02/100)**2, |
||||
3**2, 3**2, 3**2]) |
||||
|
||||
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), |
||||
ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2])} |
||||
|
||||
# MSCKF stuff |
||||
self.dim_state = len(x_initial) |
||||
self.dim_main = self.dim_state |
||||
|
||||
name = 'loc_local' |
||||
loc_local_model.gen_model(name, self.dim_state) |
||||
|
||||
# init filter |
||||
self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_main, self.dim_main) |
||||
|
||||
@property |
||||
def x(self): |
||||
return self.filter.state() |
||||
|
||||
@property |
||||
def t(self): |
||||
return self.filter.filter_time |
||||
|
||||
@property |
||||
def P(self): |
||||
return self.filter.covs() |
||||
|
||||
def predict(self, t): |
||||
if self.t: |
||||
# Does NOT modify filter state |
||||
return self.filter._predict(self.x, self.P, t - self.t)[0] |
||||
else: |
||||
raise RuntimeError("Request predict on filter with uninitialized time") |
||||
|
||||
def rts_smooth(self, estimates): |
||||
return self.filter.rts_smooth(estimates, norm_quats=True) |
||||
|
||||
|
||||
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) |
||||
if kind == ObservationKind.CAMERA_ODO_TRANSLATION: |
||||
r = self.predict_and_update_odo_trans(data, t, kind) |
||||
elif kind == ObservationKind.CAMERA_ODO_ROTATION: |
||||
r = self.predict_and_update_odo_rot(data, t, kind) |
||||
elif kind == ObservationKind.ODOMETRIC_SPEED: |
||||
r = self.predict_and_update_odo_speed(data, t, kind) |
||||
else: |
||||
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) |
||||
return r |
||||
|
||||
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 xrange(n): |
||||
R[i,:,:] = obs_noise |
||||
return R |
||||
|
||||
def predict_and_update_odo_speed(self, speed, t, kind): |
||||
z = np.array(speed) |
||||
R = np.zeros((len(speed), 1, 1)) |
||||
for i, _ in enumerate(z): |
||||
R[i,:,:] = np.diag([0.2**2]) |
||||
return self.filter.predict_and_update_batch(t, kind, z, R) |
||||
|
||||
def predict_and_update_odo_trans(self, trans, t, kind): |
||||
z = trans[:,:3] |
||||
R = np.zeros((len(trans), 3, 3)) |
||||
for i, _ in enumerate(z): |
||||
R[i,:,:] = np.diag(trans[i,3:]**2) |
||||
return self.filter.predict_and_update_batch(t, kind, z, R) |
||||
|
||||
def predict_and_update_odo_rot(self, rot, t, kind): |
||||
z = rot[:,:3] |
||||
R = np.zeros((len(rot), 3, 3)) |
||||
for i, _ in enumerate(z): |
||||
R[i,:,:] = np.diag(rot[i,3:]**2) |
||||
return self.filter.predict_and_update_batch(t, kind, z, R) |
||||
|
||||
if __name__ == "__main__": |
||||
LocLocalKalman() |
||||
@ -0,0 +1,80 @@ |
||||
import numpy as np |
||||
import sympy as sp |
||||
import os |
||||
|
||||
from kalman_helpers import ObservationKind |
||||
from ekf_sym import gen_code |
||||
|
||||
|
||||
def gen_model(name, dim_state): |
||||
|
||||
# check if rebuild is needed |
||||
try: |
||||
dir_path = os.path.dirname(__file__) |
||||
deps = [dir_path + '/' + 'ekf_c.c', |
||||
dir_path + '/' + 'ekf_sym.py', |
||||
dir_path + '/' + 'loc_local_model.py', |
||||
dir_path + '/' + 'loc_local_kf.py'] |
||||
|
||||
outs = [dir_path + '/' + name + '.o', |
||||
dir_path + '/' + name + '.so', |
||||
dir_path + '/' + name + '.cpp'] |
||||
out_times = map(os.path.getmtime, outs) |
||||
dep_times = map(os.path.getmtime, deps) |
||||
rebuild = os.getenv("REBUILD", False) |
||||
if min(out_times) > max(dep_times) and not rebuild: |
||||
return |
||||
map(os.remove, outs) |
||||
except OSError: |
||||
pass |
||||
|
||||
# make functions and jacobians with sympy |
||||
# state variables |
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1) |
||||
state = sp.Matrix(state_sym) |
||||
v = state[0:3,:] |
||||
omega = state[3:6,:] |
||||
vroll, vpitch, vyaw = omega |
||||
vx, vy, vz = v |
||||
roll_bias, pitch_bias, yaw_bias = state[6:9,:] |
||||
odo_scale = state[9,:] |
||||
accel = state[10:13,:] |
||||
|
||||
dt = sp.Symbol('dt') |
||||
|
||||
# Time derivative of the state as a function of state |
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1))) |
||||
state_dot[:3,:] = accel |
||||
|
||||
# Basic descretization, 1st order intergrator |
||||
# Can be pretty bad if dt is big |
||||
f_sym = sp.Matrix(state + dt*state_dot) |
||||
|
||||
# |
||||
# Observation functions |
||||
# |
||||
|
||||
# extra args |
||||
#imu_rot = euler_rotate(*imu_angles) |
||||
#h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias, |
||||
# vpitch + pitch_bias, |
||||
# vyaw + yaw_bias]) |
||||
h_gyro_sym = sp.Matrix([vroll + roll_bias, |
||||
vpitch + pitch_bias, |
||||
vyaw + yaw_bias]) |
||||
|
||||
speed = vx**2 + vy**2 + vz**2 |
||||
h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale]) |
||||
|
||||
h_relative_motion = sp.Matrix(v) |
||||
h_phone_rot_sym = sp.Matrix([vroll, |
||||
vpitch, |
||||
vyaw]) |
||||
|
||||
|
||||
obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None], |
||||
[h_gyro_sym, ObservationKind.PHONE_GYRO, None], |
||||
[h_phone_rot_sym, ObservationKind.NO_ROT, None], |
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], |
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None]] |
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state) |
||||
@ -0,0 +1,274 @@ |
||||
#!/usr/bin/env python |
||||
import os |
||||
import zmq |
||||
import math |
||||
import json |
||||
|
||||
os.environ["OMP_NUM_THREADS"] = "1" |
||||
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 selfdrive.messaging as messaging |
||||
from selfdrive.swaglog import cloudlog |
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel |
||||
from selfdrive.services import service_list |
||||
from selfdrive.locationd.kalman.loc_local_kf import LocLocalKalman |
||||
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind |
||||
|
||||
DEBUG = False |
||||
kf = LocLocalKalman() # Make sure that model is generated on import time |
||||
|
||||
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 |
||||
|
||||
LEARNING_RATE = 3 |
||||
|
||||
class Localizer(object): |
||||
def __init__(self, disabled_logs=None, dog=None): |
||||
self.kf = LocLocalKalman() |
||||
self.reset_kalman() |
||||
|
||||
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_car_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.carState.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.handle_sensors(log, current_time) |
||||
elif typ == "carState": |
||||
self.handle_car_state(log, current_time) |
||||
elif typ == "cameraOdometry": |
||||
self.handle_cam_odo(log, current_time) |
||||
|
||||
|
||||
class ParamsLearner(object): |
||||
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.00025 * 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: % .2f Avg. AO % .2f" % (math.degrees(self.ao), math.degrees(self.slow_ao)) |
||||
s5 = "Stiffnes: % .3f x" % self.x |
||||
print s4, s5 |
||||
|
||||
|
||||
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 |
||||
|
||||
|
||||
def locationd_thread(gctx, addr, disabled_logs): |
||||
ctx = zmq.Context() |
||||
poller = zmq.Poller() |
||||
|
||||
car_state_socket = messaging.sub_sock(ctx, service_list['carState'].port, poller, addr=addr, conflate=True) |
||||
sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True) |
||||
camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True) |
||||
|
||||
kalman_odometry_socket = messaging.pub_sock(ctx, service_list['kalmanOdometry'].port) |
||||
live_parameters_socket = messaging.pub_sock(ctx, service_list['liveParameters'].port) |
||||
|
||||
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: |
||||
cloudlog.info("Parameter learner found parameters for wrong car.") |
||||
params = None |
||||
|
||||
if params is None: |
||||
params = { |
||||
'carFingerprint': CP.carFingerprint, |
||||
'angleOffsetAverage': 0.0, |
||||
'stiffnessFactor': 1.0, |
||||
'steerRatio': VM.sR, |
||||
} |
||||
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 = 0 |
||||
while True: |
||||
for socket, event in poller.poll(timeout=1000): |
||||
log = messaging.recv_one(socket) |
||||
localizer.handle_log(log) |
||||
|
||||
if socket is car_state_socket: |
||||
if not localizer.kf.t: |
||||
continue |
||||
|
||||
if i % LEARNING_RATE == 0: |
||||
# carState 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.carState.steeringAngle) |
||||
params_valid = learner.update(yaw_rate, log.carState.vEgo, steering_angle) |
||||
|
||||
params = messaging.new_message() |
||||
params.init('liveParameters') |
||||
params.liveParameters.valid = bool(params_valid) |
||||
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_reader.put("LiveParameters", json.dumps(params)) |
||||
|
||||
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('carState') |
||||
if IN_CAR: |
||||
addr = "192.168.5.11" |
||||
|
||||
locationd_thread(gctx, addr, disabled_logs) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
Binary file not shown.
@ -1,454 +0,0 @@ |
||||
{ |
||||
"CA": { |
||||
"Default": [ |
||||
{ |
||||
"speed": "100", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "unclassified" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "40", |
||||
"tags": { |
||||
"highway": "residential" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "40", |
||||
"tags": { |
||||
"highway": "service" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "90", |
||||
"tags": { |
||||
"highway": "motorway_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "20", |
||||
"tags": { |
||||
"highway": "living_street" |
||||
} |
||||
} |
||||
] |
||||
}, |
||||
"DE": { |
||||
"Default": [ |
||||
{ |
||||
"speed": "none", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "10", |
||||
"tags": { |
||||
"highway": "living_street" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "100", |
||||
"tags": { |
||||
"zone:traffic": "DE:rural" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"zone:traffic": "DE:urban" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30", |
||||
"tags": { |
||||
"bicycle_road": "yes" |
||||
} |
||||
} |
||||
] |
||||
}, |
||||
"AU": { |
||||
"Default": [ |
||||
{ |
||||
"speed": "100", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "unclassified" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "residential" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "40", |
||||
"tags": { |
||||
"highway": "service" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "90", |
||||
"tags": { |
||||
"highway": "motorway_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "80", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "50", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30", |
||||
"tags": { |
||||
"highway": "living_street" |
||||
} |
||||
} |
||||
] |
||||
}, |
||||
"US": { |
||||
"South Dakota": [ |
||||
{ |
||||
"speed": "80 mph", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "70 mph", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "70 mph", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
} |
||||
], |
||||
"Wisconsin": [ |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "unclassified" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
} |
||||
], |
||||
"Default": [ |
||||
{ |
||||
"speed": "65 mph", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "trunk" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "unclassified" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "25 mph", |
||||
"tags": { |
||||
"highway": "residential" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "25 mph", |
||||
"tags": { |
||||
"highway": "service" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "motorway_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "trunk_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "15 mph", |
||||
"tags": { |
||||
"highway": "living_street" |
||||
} |
||||
} |
||||
], |
||||
"Michigan": [ |
||||
{ |
||||
"speed": "70 mph", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
} |
||||
], |
||||
"Oregon": [ |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "motorway" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30 mph", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "15 mph", |
||||
"tags": { |
||||
"highway": "service" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "35 mph", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30 mph", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
} |
||||
], |
||||
"New York": [ |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "primary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "secondary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "tertiary" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "30 mph", |
||||
"tags": { |
||||
"highway": "residential" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "45 mph", |
||||
"tags": { |
||||
"highway": "primary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "secondary_link" |
||||
} |
||||
}, |
||||
{ |
||||
"speed": "55 mph", |
||||
"tags": { |
||||
"highway": "tertiary_link" |
||||
} |
||||
} |
||||
] |
||||
} |
||||
} |
||||
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue