Vehicle Researcher
6 years ago
parent
1bbcc51a36
commit
30f7a33535
102 changed files with 2763 additions and 966 deletions
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:94b4a6bc1f7fc720b9f638268ff2345368164efd8a4710616b3e9ad5fd473fcf |
||||
size 18388501 |
||||
oid sha256:e03ff9d6482c19f14ce565d64aab24433847208106e5a5e31947f5307e510ee5 |
||||
size 18376966 |
||||
|
||||
@ -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 |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:782e58e2fcbcbb39011011aa72c884ab3aa0cef22c434282df17ce5d64f2cba1 |
||||
size 428100 |
||||
oid sha256:0e880365b63e8d48bf8bee797dc96b483682a6b0b14e968e937c9109377b926e |
||||
size 427951 |
||||
|
||||
@ -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 |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:1d469e8cd75122d5996338da166919f3dda15c1ef72f5ce3e33c46096a168228 |
||||
size 2328 |
||||
oid sha256:b0ccf356c299a12f40fd99fa61361812e5fedff6f1836c8afad98b7b2f3a2a2a |
||||
size 2456 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:0cc8148b7105e645cd232ac0c088c4324a545328eb64781939762214ddf14f90 |
||||
oid sha256:849733e32eecd68112c4ab305c013025dbfa41ec3f13c54fbb9368388d835260 |
||||
size 532192 |
||||
|
||||
@ -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 |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:81c0f18ac2f84fa44a2afca0206173dd866605a0b7dbe60fd2640734d54ce3b9 |
||||
size 5480 |
||||
oid sha256:453ffa5ad5c533e6d6065167fb1a674ed9196e2ade87df4a55d163ff178eccf7 |
||||
size 5376 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:6d5761f29b51905c42910fdf9e7b5744f09f132c69caa8ac6e96e57ac1f17e6b |
||||
size 8832 |
||||
oid sha256:c50ba5b1353617c7a18d17f1417c4036d8e5c41db79f82cb9a9b4a19032e6cc6 |
||||
size 8752 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:26a4622ed83e6315cf34d55e46a3bb4af8db1d135f9c54c4e10dbd4d09a47dda |
||||
size 34770 |
||||
oid sha256:221b739b59f5a314fc832a29a7fcf49da960012acc22aed82e66331f950ac5f8 |
||||
size 11486 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:b6f5e9a2fc5f5765ce2f53c16d6ccbd1142cd514ab6acee0625a12fef8f11fd9 |
||||
size 8584 |
||||
oid sha256:1a5e0651c2c8c650b59421bb4c4beea5c3b666bbd1c6e440eb2b62e2a64119bb |
||||
size 3560 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:7aaa8e6766705d1cd22c65d418e10a4c4b54864809cc9ad5e5de09c98e72cfd0 |
||||
oid sha256:c4099e03a411a516b406ef62cf0d923fb9447b376ae7dcbebe680a29f6c217d9 |
||||
size 1948 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:383c2fa9974e0802dd86773aed20e068b39e0e5c505e6a641b30332f99f875b7 |
||||
oid sha256:3cc91e06813e2f18c87f0f2c798eb76a4e81e12c1027892a6c2b7d3451b03d54 |
||||
size 1821 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:21349c8535aa0c186edcf75fbf815822dfddbedc3f5e090c8bcd416dea5a1ced |
||||
oid sha256:d8648ab44187e2be8e7cdb128249dfac75338d98d0d93ca9878ea4a4b3f2ad3c |
||||
size 2992 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:c00034f1aa41c2f00e93c7d748cf5f663435360563107b6898441aac661a9bf2 |
||||
size 448071 |
||||
oid sha256:a58b02a90b3776ce308ab77094ebd8cdd23e788e1501a4c050862e65e827b185 |
||||
size 349038 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:6a618ebaa304fe2776942918ea1f007a44eb63afd5b5e2946082034073358c2d |
||||
size 307280 |
||||
oid sha256:16e942a5676e23d49e01b17ff664a55bddffa9e7d054caf4f6dbb03d16ec2e9e |
||||
size 274768 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:eb9f45801304976cb37dcfbcff9681244bf78e97284d8b8da7264871a827783f |
||||
oid sha256:a955478920067e11d3c1d40f96323bfb943196d66954911dfc62c5f673ecab06 |
||||
size 8048 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:0285ad4d884bf0bdcd3fa692ddc0814ec12b73ec0b0433b18b9072192fd503e0 |
||||
oid sha256:81feeda7638a908cd175f97addf62d16c4f47d3e38b020a0069838f3502ceff0 |
||||
size 8112 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:df47bdf5dba35ac627a72e0c58e7d1648a0759d17b852a3347cac5e183e4b046 |
||||
oid sha256:57289f47ce5d35735edc673351e496e32ec606028512ecd785b9ea4309005629 |
||||
size 2944 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:6f9c880eb9d53f6b86acc90166150bc96ad25c1554b3dc61e449217e8faabc0a |
||||
oid sha256:d1df0d4b3379b6daebd902add7e38a8f53b90c3b7c25b1d8855fd44a043fe650 |
||||
size 4488 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:5a3b55e323c19172587d0e58c619a1e4d2c95711473b071bff29c0d32dad442d |
||||
oid sha256:3ac57f4a4c5410bf29c12a32418e3b0ccd020d0362cc4a5316a915482cb4c99d |
||||
size 4496 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:acefa3c955a443246265289085a7fb9c096c5310bc456a8ccda785d94852e4ff |
||||
size 72928 |
||||
oid sha256:67f333a6f1d18e9372b0c9eb8588cf4d3db87d3f731ff56497fbcecd79294400 |
||||
size 72200 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:728d61fd9f2e72f9fca96e51ae58b23308469625f8e97a29918493e79e9b37b9 |
||||
size 37832 |
||||
oid sha256:250c5e3626bd753796225bca9d031751a9f21e73e7b7e7fc8c4821b59af96298 |
||||
size 37592 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:f6b12c121ee23318322af7882c07d37dd298068898160a8c1975fd5e9fb6d79f |
||||
oid sha256:ec3bcbb2a42f595b00d2b7ed13f08077c1c759366bf83fb553e397f5091dc60c |
||||
size 3856 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:79a968b415bc198402eac3b630b3087c1bc2b5a62883b9e39c8c43469ee9f388 |
||||
size 446096 |
||||
oid sha256:6c30e9afb75af10d72d0ea5e30b06c0d5310cce9312a441a2d260eb82d66f5ea |
||||
size 405136 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:b9352e718fb6a33b154e5ff749c65051dde02cc5bcf8ccf2f8a44c2bed85e8cc |
||||
size 3976 |
||||
oid sha256:3b76bbb87fa3725ded4f4372174c3c7e4e5a58b86b950cd8867222f4cf3c375b |
||||
size 3136 |
||||
|
||||
@ -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() |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:f4ce9c8b78e58c57b0f033354f1411811457b9e401c16fe5c5b130aa9fe86d3a |
||||
oid sha256:4e0f935ee35f4856cfc315209da7c0d869fe518c00740ccd3646c108ce4a7745 |
||||
size 1630952 |
||||
|
||||
@ -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" |
||||
} |
||||
} |
||||
] |
||||
} |
||||
} |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:a912e6346d7da8591acc72b5aa2d2382cb815d44f8567c656097ac180fe846b6 |
||||
oid sha256:b07f5055891b22f181023c132917416ebb8385201ffd172c5f9584ff8e8ff47b |
||||
size 1171544 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:09b15277bb78a39ef3734b4a4773e8fed6431541395385ea49b78fbb1c3f37c4 |
||||
oid sha256:b8a9e5e8d9e3ae4349b846f3f621aa0d67a114867ac0bc7c3ab891c425a56c2b |
||||
size 1159016 |
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue