mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-21 14:02:05 +08:00
loc_local is dead
This commit is contained in:
Willem Melching
@@ -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)
|
||||
Reference in New Issue
Block a user