Willem Melching
5 years ago
parent
8611b1b648
commit
4bfe88a31a
2 changed files with 0 additions and 208 deletions
@ -1,128 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import numpy as np |
|
||||||
from selfdrive.locationd.kalman import loc_local_model |
|
||||||
|
|
||||||
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind |
|
||||||
from selfdrive.locationd.kalman.ekf_sym import EKF_sym |
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
class States(): |
|
||||||
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(): |
|
||||||
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 range(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() |
|
||||||
@ -1,80 +0,0 @@ |
|||||||
import numpy as np |
|
||||||
import sympy as sp |
|
||||||
import os |
|
||||||
|
|
||||||
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 = list(map(os.path.getmtime, outs)) |
|
||||||
dep_times = list(map(os.path.getmtime, deps)) |
|
||||||
rebuild = os.getenv("REBUILD", False) |
|
||||||
if min(out_times) > max(dep_times) and not rebuild: |
|
||||||
return |
|
||||||
list(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) |
|
||||||
Loading…
Reference in new issue