|
|
|
|
import numpy as np
|
|
|
|
|
import sympy as sp
|
|
|
|
|
import os
|
|
|
|
|
|
getting ready for Python 3 (#619)
* tabs to spaces
python 2 to 3: https://portingguide.readthedocs.io/en/latest/syntax.html#tabs-and-spaces
* use the new except syntax
python 2 to 3: https://portingguide.readthedocs.io/en/latest/exceptions.html#the-new-except-syntax
* make relative imports absolute
python 2 to 3: https://portingguide.readthedocs.io/en/latest/imports.html#absolute-imports
* Queue renamed to queue in python 3
Use the six compatibility library to support both python 2 and 3: https://portingguide.readthedocs.io/en/latest/stdlib-reorg.html#renamed-modules
* replace dict.has_key() with in
python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#removed-dict-has-key
* make dict views compatible with python 3
python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#dict-views-and-iterators
Where needed, wrapping things that will be a view in python 3 with a list(). For example, if it's accessed with []
Python 3 has no iter*() methods, so just using the values() instead of itervalues() as long as it's not too performance intensive. Note that any minor performance hit of using a list instead of a view will go away when switching to python 3. If it is intensive, we could use the six version.
* Explicitly use truncating division
python 2 to 3: https://portingguide.readthedocs.io/en/latest/numbers.html#division
python 3 treats / as float division. When we want the result to be an integer, use //
* replace map() with list comprehension where a list result is needed.
In python 3, map() returns an iterator.
python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter
* replace filter() with list comprehension
In python 3, filter() returns an interatoooooooooooor.
python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter
* wrap zip() in list() where we need the result to be a list
python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-zip
* clean out some lint
Removes these pylint warnings:
************* Module selfdrive.car.chrysler.chryslercan
W: 15, 0: Unnecessary semicolon (unnecessary-semicolon)
W: 16, 0: Unnecessary semicolon (unnecessary-semicolon)
W: 25, 0: Unnecessary semicolon (unnecessary-semicolon)
************* Module common.dbc
W:101, 0: Anomalous backslash in string: '\?'. String constant might be missing an r prefix. (anomalous-backslash-in-string)
************* Module selfdrive.car.gm.interface
R:102, 6: Redefinition of ret.minEnableSpeed type from float to int (redefined-variable-type)
R:103, 6: Redefinition of ret.mass type from int to float (redefined-variable-type)
************* Module selfdrive.updated
R: 20, 6: Redefinition of r type from int to str (redefined-variable-type)
7 years ago
|
|
|
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
|
|
|
|
|
from selfdrive.locationd.kalman.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)
|
