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