Kalman filter compilation cleanup (#1080)
* start cleanup * create generated dir if not exist * tests pass! * everything works again * also convert live_kf to new structure * Remove sympy helpers from file list * Add laika to docker container * Only build models that are presentpull/1084/head
parent
a790892796
commit
47fd50ca60
|
@ -81,6 +81,8 @@ COPY ./pyextra /tmp/openpilot/pyextra
|
|||
COPY ./panda /tmp/openpilot/panda
|
||||
COPY ./external /tmp/openpilot/external
|
||||
COPY ./tools /tmp/openpilot/tools
|
||||
COPY ./laika /tmp/openpilot/laika
|
||||
COPY ./laika_repo /tmp/openpilot/laika_repo
|
||||
|
||||
COPY SConstruct /tmp/openpilot/SConstruct
|
||||
|
||||
|
|
|
@ -228,5 +228,6 @@ if arch == "aarch64":
|
|||
SConscript(['selfdrive/clocksd/SConscript'])
|
||||
|
||||
SConscript(['selfdrive/locationd/SConscript'])
|
||||
SConscript(['selfdrive/locationd/kalman/SConscript'])
|
||||
|
||||
# TODO: finish cereal, dbcbuilder, MPC
|
||||
|
|
|
@ -28,7 +28,6 @@ common/profiler.py
|
|||
common/testing.py
|
||||
common/basedir.py
|
||||
common/filter_simple.py
|
||||
common/sympy_helpers.py
|
||||
common/stat_live.py
|
||||
common/spinner.py
|
||||
common/cython_hacks.py
|
||||
|
|
|
@ -1,5 +1 @@
|
|||
lane.cpp
|
||||
gnss.cpp
|
||||
loc*.cpp
|
||||
live.cpp
|
||||
pos_computer*.cpp
|
||||
generated/
|
||||
|
|
|
@ -0,0 +1,30 @@
|
|||
Import('env')
|
||||
|
||||
templates = Glob('templates/*')
|
||||
sympy_helpers = "helpers/sympy_helpers.py"
|
||||
ekf_sym = "helpers/ekf_sym.py"
|
||||
|
||||
to_build = {
|
||||
'pos_computer_4': 'helpers/lst_sq_computer.py',
|
||||
'feature_handler_5': 'helpers/feature_handler.py',
|
||||
'gnss': 'models/gnss_kf.py',
|
||||
'loc_4': 'models/loc_kf.py',
|
||||
'live': 'models/live_kf.py',
|
||||
'lane': '#xx/pipeline/lib/ekf/lane_kf.py',
|
||||
}
|
||||
|
||||
found = {}
|
||||
|
||||
for target, command in to_build.items():
|
||||
if File(command).exists():
|
||||
found[target] = command
|
||||
|
||||
for target, command in found.items():
|
||||
target_files = File([f'generated/{target}.cpp', f'generated/{target}.h'])
|
||||
command_file = File(command)
|
||||
env.Command(target_files,
|
||||
[templates, command_file, sympy_helpers, ekf_sym],
|
||||
command_file.get_abspath()
|
||||
)
|
||||
|
||||
env.SharedLibrary('generated/' + target, target_files[0])
|
|
@ -1,323 +0,0 @@
|
|||
import common.transformations.orientation as orient
|
||||
import numpy as np
|
||||
import scipy.optimize as opt
|
||||
import time
|
||||
import os
|
||||
from bisect import bisect_left
|
||||
from common.sympy_helpers import sympy_into_c, quat_matrix_l
|
||||
from common.ffi_wrapper import ffi_wrap, wrap_compiled, compile_code
|
||||
|
||||
EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
|
||||
|
||||
|
||||
def sane(track):
|
||||
img_pos = track[1:,2:4]
|
||||
diffs_x = abs(img_pos[1:,0] - img_pos[:-1,0])
|
||||
diffs_y = abs(img_pos[1:,1] - img_pos[:-1,1])
|
||||
for i in range(1, len(diffs_x)):
|
||||
if ((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and \
|
||||
(diffs_x[i] > 2*diffs_x[i-1] or \
|
||||
diffs_x[i] < .5*diffs_x[i-1])) or \
|
||||
((diffs_y[i] > 0.05 or diffs_y[i-1] > 0.05) and \
|
||||
(diffs_y[i] > 2*diffs_y[i-1] or \
|
||||
diffs_y[i] < .5*diffs_y[i-1])):
|
||||
return False
|
||||
return True
|
||||
|
||||
class FeatureHandler():
|
||||
def __init__(self, K):
|
||||
self.MAX_TRACKS=6000
|
||||
self.K = K
|
||||
#Array of tracks, each track
|
||||
#has K 5D features preceded
|
||||
#by 5 params that inidicate
|
||||
#[f_idx, last_idx, updated, complete, valid]
|
||||
# f_idx: idx of current last feature in track
|
||||
# idx of of last feature in frame
|
||||
# bool for whether this track has been update
|
||||
# bool for whether this track is complete
|
||||
# bool for whether this track is valid
|
||||
self.tracks = np.zeros((self.MAX_TRACKS, K+1, 5))
|
||||
self.tracks[:] = np.nan
|
||||
|
||||
# Wrap c code for slow matching
|
||||
c_header = "\nvoid merge_features(double *tracks, double *features, long long *empty_idxs);"
|
||||
c_code = "#define K %d\n" % K
|
||||
c_code += "\n" + open(os.path.join(EXTERNAL_PATH, "feature_handler.c")).read()
|
||||
ffi, lib = ffi_wrap('feature_handler', c_code, c_header)
|
||||
def merge_features_c(tracks, features, empty_idxs):
|
||||
lib.merge_features(ffi.cast("double *", tracks.ctypes.data),
|
||||
ffi.cast("double *", features.ctypes.data),
|
||||
ffi.cast("long long *", empty_idxs.ctypes.data))
|
||||
|
||||
#self.merge_features = self.merge_features_python
|
||||
self.merge_features = merge_features_c
|
||||
|
||||
def reset(self):
|
||||
self.tracks[:] = np.nan
|
||||
|
||||
def merge_features_python(self, tracks, features, empty_idxs):
|
||||
empty_idx = 0
|
||||
for f in features:
|
||||
match_idx = int(f[4])
|
||||
if tracks[match_idx, 0, 1] == match_idx and tracks[match_idx, 0 ,2] == 0:
|
||||
tracks[match_idx, 0, 0] += 1
|
||||
tracks[match_idx, 0, 1] = f[1]
|
||||
tracks[match_idx, 0, 2] = 1
|
||||
tracks[match_idx, int(tracks[match_idx, 0, 0])] = f
|
||||
if tracks[match_idx, 0, 0] == self.K:
|
||||
tracks[match_idx, 0, 3] = 1
|
||||
if sane(tracks[match_idx]):
|
||||
tracks[match_idx, 0, 4] = 1
|
||||
else:
|
||||
if empty_idx == len(empty_idxs):
|
||||
print('need more empty space')
|
||||
continue
|
||||
tracks[empty_idxs[empty_idx], 0, 0] = 1
|
||||
tracks[empty_idxs[empty_idx], 0, 1] = f[1]
|
||||
tracks[empty_idxs[empty_idx], 0, 2] = 1
|
||||
tracks[empty_idxs[empty_idx], 1] = f
|
||||
empty_idx += 1
|
||||
|
||||
def update_tracks(self, features):
|
||||
t0 = time.time()
|
||||
last_idxs = np.copy(self.tracks[:,0,1])
|
||||
real = np.isfinite(last_idxs)
|
||||
self.tracks[last_idxs[real].astype(int)] = self.tracks[real]
|
||||
mask = np.ones(self.MAX_TRACKS, np.bool)
|
||||
mask[last_idxs[real].astype(int)] = 0
|
||||
empty_idxs = np.arange(self.MAX_TRACKS)[mask]
|
||||
self.tracks[empty_idxs] = np.nan
|
||||
self.tracks[:,0,2] = 0
|
||||
self.merge_features(self.tracks, features, empty_idxs)
|
||||
|
||||
def handle_features(self, features):
|
||||
self.update_tracks(features)
|
||||
valid_idxs = self.tracks[:,0,4] == 1
|
||||
complete_idxs = self.tracks[:,0,3] == 1
|
||||
stale_idxs = self.tracks[:,0,2] == 0
|
||||
valid_tracks = self.tracks[valid_idxs]
|
||||
self.tracks[complete_idxs] = np.nan
|
||||
self.tracks[stale_idxs] = np.nan
|
||||
return valid_tracks[:,1:,:4].reshape((len(valid_tracks), self.K*4))
|
||||
|
||||
def generate_residual(K):
|
||||
import sympy as sp
|
||||
from common.sympy_helpers import quat_rotate
|
||||
x_sym = sp.MatrixSymbol('abr', 3,1)
|
||||
poses_sym = sp.MatrixSymbol('poses', 7*K,1)
|
||||
img_pos_sym = sp.MatrixSymbol('img_positions', 2*K,1)
|
||||
alpha, beta, rho = x_sym
|
||||
to_c = sp.Matrix(orient.rot_matrix(-np.pi/2, -np.pi/2, 0))
|
||||
pos_0 = sp.Matrix(np.array(poses_sym[K*7-7:K*7-4])[:,0])
|
||||
q = poses_sym[K*7-4:K*7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_0 = to_c*quat_rot.T
|
||||
rows = []
|
||||
for i in range(K):
|
||||
pos_i = sp.Matrix(np.array(poses_sym[i*7:i*7+3])[:,0])
|
||||
q = poses_sym[7*i+3:7*i+7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_i = to_c*quat_rot.T
|
||||
rot_0_to_i = rot_g_to_i*(rot_g_to_0.T)
|
||||
trans_0_to_i = rot_g_to_i*(pos_0 - pos_i)
|
||||
funct_vec = rot_0_to_i*sp.Matrix([alpha, beta, 1]) + rho*trans_0_to_i
|
||||
h1, h2, h3 = funct_vec
|
||||
rows.append(h1/h3 - img_pos_sym[i*2 +0])
|
||||
rows.append(h2/h3 - img_pos_sym[i*2 + 1])
|
||||
img_pos_residual_sym = sp.Matrix(rows)
|
||||
|
||||
# sympy into c
|
||||
sympy_functions = []
|
||||
sympy_functions.append(('res_fun', img_pos_residual_sym, [x_sym, poses_sym, img_pos_sym]))
|
||||
sympy_functions.append(('jac_fun', img_pos_residual_sym.jacobian(x_sym), [x_sym, poses_sym, img_pos_sym]))
|
||||
|
||||
return sympy_functions
|
||||
|
||||
|
||||
def generate_orient_error_jac(K):
|
||||
import sympy as sp
|
||||
from common.sympy_helpers import quat_rotate
|
||||
x_sym = sp.MatrixSymbol('abr', 3,1)
|
||||
dtheta = sp.MatrixSymbol('dtheta', 3,1)
|
||||
delta_quat = sp.Matrix(np.ones(4))
|
||||
delta_quat[1:,:] = sp.Matrix(0.5*dtheta[0:3,:])
|
||||
poses_sym = sp.MatrixSymbol('poses', 7*K,1)
|
||||
img_pos_sym = sp.MatrixSymbol('img_positions', 2*K,1)
|
||||
alpha, beta, rho = x_sym
|
||||
to_c = sp.Matrix(orient.rot_matrix(-np.pi/2, -np.pi/2, 0))
|
||||
pos_0 = sp.Matrix(np.array(poses_sym[K*7-7:K*7-4])[:,0])
|
||||
q = quat_matrix_l(poses_sym[K*7-4:K*7])*delta_quat
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_0 = to_c*quat_rot.T
|
||||
rows = []
|
||||
for i in range(K):
|
||||
pos_i = sp.Matrix(np.array(poses_sym[i*7:i*7+3])[:,0])
|
||||
q = quat_matrix_l(poses_sym[7*i+3:7*i+7])*delta_quat
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_i = to_c*quat_rot.T
|
||||
rot_0_to_i = rot_g_to_i*(rot_g_to_0.T)
|
||||
trans_0_to_i = rot_g_to_i*(pos_0 - pos_i)
|
||||
funct_vec = rot_0_to_i*sp.Matrix([alpha, beta, 1]) + rho*trans_0_to_i
|
||||
h1, h2, h3 = funct_vec
|
||||
rows.append(h1/h3 - img_pos_sym[i*2 +0])
|
||||
rows.append(h2/h3 - img_pos_sym[i*2 + 1])
|
||||
img_pos_residual_sym = sp.Matrix(rows)
|
||||
|
||||
# sympy into c
|
||||
sympy_functions = []
|
||||
sympy_functions.append(('orient_error_jac', img_pos_residual_sym.jacobian(dtheta), [x_sym, poses_sym, img_pos_sym, dtheta]))
|
||||
|
||||
return sympy_functions
|
||||
|
||||
|
||||
class LstSqComputer():
|
||||
def __init__(self, K, MIN_DEPTH=2, MAX_DEPTH=500, debug=False):
|
||||
self.to_c = orient.rot_matrix(-np.pi/2, -np.pi/2, 0)
|
||||
self.MAX_DEPTH = MAX_DEPTH
|
||||
self.MIN_DEPTH = MIN_DEPTH
|
||||
self.debug = debug
|
||||
self.name = 'pos_computer_' + str(K)
|
||||
if debug:
|
||||
self.name += '_debug'
|
||||
|
||||
try:
|
||||
dir_path = os.path.dirname(__file__)
|
||||
deps = [dir_path + '/' + 'feature_handler.py',
|
||||
dir_path + '/' + 'compute_pos.c']
|
||||
|
||||
outs = [dir_path + '/' + self.name + '.o',
|
||||
dir_path + '/' + self.name + '.so',
|
||||
dir_path + '/' + self.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) or rebuild:
|
||||
list(map(os.remove, outs))
|
||||
# raise the OSError if removing didnt
|
||||
# raise one to start the compilation
|
||||
raise OSError()
|
||||
except OSError as e:
|
||||
# gen c code for sympy functions
|
||||
sympy_functions = generate_residual(K)
|
||||
#if debug:
|
||||
# sympy_functions.extend(generate_orient_error_jac(K))
|
||||
header, code = sympy_into_c(sympy_functions)
|
||||
|
||||
# ffi wrap c code
|
||||
extra_header = "\nvoid compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);"
|
||||
code += "\n#define KDIM %d\n" % K
|
||||
header += "\n" + extra_header
|
||||
code += "\n" + open(os.path.join(EXTERNAL_PATH, "compute_pos.c")).read()
|
||||
compile_code(self.name, code, header, EXTERNAL_PATH)
|
||||
ffi, lib = wrap_compiled(self.name, EXTERNAL_PATH)
|
||||
|
||||
# wrap c functions
|
||||
#if debug:
|
||||
#def orient_error_jac(x, poses, img_positions, dtheta):
|
||||
# out = np.zeros(((K*2, 3)), dtype=np.float64)
|
||||
# lib.orient_error_jac(ffi.cast("double *", x.ctypes.data),
|
||||
# ffi.cast("double *", poses.ctypes.data),
|
||||
# ffi.cast("double *", img_positions.ctypes.data),
|
||||
# ffi.cast("double *", dtheta.ctypes.data),
|
||||
# ffi.cast("double *", out.ctypes.data))
|
||||
# return out
|
||||
#self.orient_error_jac = orient_error_jac
|
||||
def residual_jac(x, poses, img_positions):
|
||||
out = np.zeros(((K*2, 3)), dtype=np.float64)
|
||||
lib.jac_fun(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return out
|
||||
def residual(x, poses, img_positions):
|
||||
out = np.zeros((K*2), dtype=np.float64)
|
||||
lib.res_fun(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return out
|
||||
self.residual = residual
|
||||
self.residual_jac = residual_jac
|
||||
|
||||
def compute_pos_c(poses, img_positions):
|
||||
pos = np.zeros(3, dtype=np.float64)
|
||||
param = np.zeros(3, dtype=np.float64)
|
||||
# Can't be a view for the ctype
|
||||
img_positions = np.copy(img_positions)
|
||||
lib.compute_pos(ffi.cast("double *", self.to_c.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", param.ctypes.data),
|
||||
ffi.cast("double *", pos.ctypes.data))
|
||||
return pos, param
|
||||
self.compute_pos_c = compute_pos_c
|
||||
|
||||
def compute_pos(self, poses, img_positions, debug=False):
|
||||
pos, param = self.compute_pos_c(poses, img_positions)
|
||||
#pos, param = self.compute_pos_python(poses, img_positions)
|
||||
depth = 1/param[2]
|
||||
if debug:
|
||||
if not self.debug:
|
||||
raise NotImplementedError("This is not a debug computer")
|
||||
#orient_err_jac = self.orient_error_jac(param, poses, img_positions, np.zeros(3)).reshape((-1,2,3))
|
||||
jac = self.residual_jac(param, poses, img_positions).reshape((-1,2,3))
|
||||
res = self.residual(param, poses, img_positions).reshape((-1,2))
|
||||
return pos, param, res, jac #, orient_err_jac
|
||||
elif (self.MIN_DEPTH < depth < self.MAX_DEPTH):
|
||||
return pos
|
||||
else:
|
||||
return None
|
||||
|
||||
def gauss_newton(self, fun, jac, x, args):
|
||||
poses, img_positions = args
|
||||
delta = 1
|
||||
counter = 0
|
||||
while abs(np.linalg.norm(delta)) > 1e-4 and counter < 30:
|
||||
delta = np.linalg.pinv(jac(x, poses, img_positions)).dot(fun(x, poses, img_positions))
|
||||
x = x - delta
|
||||
counter += 1
|
||||
return [x]
|
||||
|
||||
def compute_pos_python(self, poses, img_positions, check_quality=False):
|
||||
# This procedure is also described
|
||||
# in the MSCKF paper (Mourikis et al. 2007)
|
||||
x = np.array([img_positions[-1][0],
|
||||
img_positions[-1][1], 0.1])
|
||||
res = opt.leastsq(self.residual, x, Dfun=self.residual_jac, args=(poses, img_positions)) # scipy opt
|
||||
#res = self.gauss_newton(self.residual, self.residual_jac, x, (poses, img_positions)) # diy gauss_newton
|
||||
|
||||
alpha, beta, rho = res[0]
|
||||
rot_0_to_g = (orient.rotations_from_quats(poses[-1,3:])).dot(self.to_c.T)
|
||||
return (rot_0_to_g.dot(np.array([alpha, beta, 1])))/rho + poses[-1,:3]
|
||||
|
||||
|
||||
|
||||
|
||||
'''
|
||||
EXPERIMENTAL CODE
|
||||
'''
|
||||
def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos):
|
||||
# only speed correction for now
|
||||
t_roll = 0.016 # 16ms rolling shutter?
|
||||
vroll, vpitch, vyaw = rot_rates
|
||||
A = 0.5*np.array([[-1, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A.dot(poses[-1][3:7])
|
||||
v = np.append(v, q_dot)
|
||||
v = np.array([v[0], v[1], v[2],0,0,0,0])
|
||||
current_pose = poses[-1] + v*0.05
|
||||
poses = np.vstack((current_pose, poses))
|
||||
dt = -img_positions[:,1]*t_roll/0.48
|
||||
errs = project(poses, ecef_pos) - project(poses + np.atleast_2d(dt).T.dot(np.atleast_2d(v)), ecef_pos)
|
||||
return img_positions - errs
|
||||
|
||||
def project(poses, ecef_pos):
|
||||
img_positions = np.zeros((len(poses), 2))
|
||||
for i, p in enumerate(poses):
|
||||
cam_frame = orient.rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3])
|
||||
img_positions[i] = np.array([cam_frame[1]/cam_frame[0], cam_frame[2]/cam_frame[0]])
|
||||
return img_positions
|
||||
|
|
@ -1,105 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from . import gnss_model
|
||||
|
||||
from .kalman_helpers import ObservationKind
|
||||
from .ekf_sym import EKF_sym
|
||||
from selfdrive.locationd.kalman.loc_kf import parse_pr, parse_prr
|
||||
|
||||
class States():
|
||||
ECEF_POS = slice(0,3) # x, y and z in ECEF in meters
|
||||
ECEF_VELOCITY = slice(3,6)
|
||||
CLOCK_BIAS = slice(6, 7) # clock bias in light-meters,
|
||||
CLOCK_DRIFT = slice(7, 8) # clock drift in light-meters/s,
|
||||
CLOCK_ACCELERATION = slice(8, 9) # clock acceleration in light-meters/s**2
|
||||
GLONASS_BIAS = slice(9, 10) # clock drift in light-meters/s,
|
||||
GLONASS_FREQ_SLOPE = slice(10, 11) # GLONASS bias in m expressed as bias + freq_num*freq_slope
|
||||
|
||||
|
||||
class GNSSKalman():
|
||||
def __init__(self, N=0, max_tracks=3000):
|
||||
x_initial = np.array([-2712700.6008, -4281600.6679, 3859300.1830,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0])
|
||||
|
||||
# state covariance
|
||||
P_initial = np.diag([10000**2, 10000**2, 10000**2,
|
||||
10**2, 10**2, 10**2,
|
||||
(2000000)**2, (100)**2, (0.5)**2,
|
||||
(10)**2, (1)**2])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([0.3**2, 0.3**2, 0.3**2,
|
||||
3**2, 3**2, 3**2,
|
||||
(.1)**2, (0)**2, (0.01)**2,
|
||||
.1**2, (.01)**2])
|
||||
|
||||
self.dim_state = x_initial.shape[0]
|
||||
|
||||
# mahalanobis outlier rejection
|
||||
maha_test_kinds = []#ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS]
|
||||
|
||||
name = 'gnss'
|
||||
gnss_model.gen_model(name, self.dim_state, maha_test_kinds)
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_state, self.dim_state, maha_test_kinds=maha_test_kinds)
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
return self.filter.state()
|
||||
|
||||
@property
|
||||
def P(self):
|
||||
return self.filter.covs()
|
||||
|
||||
def predict(self, t):
|
||||
return self.filter.predict(t)
|
||||
|
||||
def rts_smooth(self, estimates):
|
||||
return self.filter.rts_smooth(estimates, norm_quats=False)
|
||||
|
||||
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.PSEUDORANGE_GPS or kind == ObservationKind.PSEUDORANGE_GLONASS:
|
||||
r = self.predict_and_update_pseudorange(data, t, kind)
|
||||
elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS:
|
||||
r = self.predict_and_update_pseudorange_rate(data, t, kind)
|
||||
return r
|
||||
|
||||
def predict_and_update_pseudorange(self, meas, t, kind):
|
||||
R = np.zeros((len(meas), 1, 1))
|
||||
sat_pos_freq = np.zeros((len(meas), 4))
|
||||
z = np.zeros((len(meas), 1))
|
||||
for i, m in enumerate(meas):
|
||||
z_i, R_i, sat_pos_freq_i = parse_pr(m)
|
||||
sat_pos_freq[i,:] = sat_pos_freq_i
|
||||
z[i,:] = z_i
|
||||
R[i,:,:] = R_i
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq)
|
||||
|
||||
def predict_and_update_pseudorange_rate(self, meas, t, kind):
|
||||
R = np.zeros((len(meas), 1, 1))
|
||||
z = np.zeros((len(meas), 1))
|
||||
sat_pos_vel = np.zeros((len(meas), 6))
|
||||
for i, m in enumerate(meas):
|
||||
z_i, R_i, sat_pos_vel_i = parse_prr(m)
|
||||
sat_pos_vel[i] = sat_pos_vel_i
|
||||
R[i,:,:] = R_i
|
||||
z[i, :] = z_i
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
GNSSKalman()
|
|
@ -1,93 +0,0 @@
|
|||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
import os
|
||||
from .kalman_helpers import ObservationKind
|
||||
from .ekf_sym import gen_code
|
||||
from common.sympy_helpers import cross, euler_rotate, quat_rotate, quat_matrix_l, quat_matrix_r
|
||||
|
||||
def gen_model(name, dim_state, maha_test_kinds):
|
||||
|
||||
# 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 + '/' + 'gnss_model.py',
|
||||
dir_path + '/' + 'gnss_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 as e:
|
||||
pass
|
||||
|
||||
# make functions and jacobians with sympy
|
||||
# state variables
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x,y,z = state[0:3,:]
|
||||
v = state[3:6,:]
|
||||
vx, vy, vz = v
|
||||
cb, cd, ca = state[6:9,:]
|
||||
glonass_bias, glonass_freq_slope = state[9:11,:]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[:3,:] = v
|
||||
state_dot[6,0] = cd
|
||||
state_dot[7,0] = ca
|
||||
|
||||
# Basic descretization, 1st order intergrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt*state_dot
|
||||
|
||||
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
|
||||
# extra args
|
||||
sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1)
|
||||
sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1)
|
||||
sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1)
|
||||
orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1)
|
||||
|
||||
# expand extra args
|
||||
sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym
|
||||
sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:]
|
||||
los_x, los_y, los_z = sat_los_sym
|
||||
orb_x, orb_y, orb_z = orb_epos_sym
|
||||
|
||||
h_pseudorange_sym = sp.Matrix([sp.sqrt(
|
||||
(x - sat_x)**2 +
|
||||
(y - sat_y)**2 +
|
||||
(z - sat_z)**2) +
|
||||
cb])
|
||||
|
||||
h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt(
|
||||
(x - sat_x)**2 +
|
||||
(y - sat_y)**2 +
|
||||
(z - sat_z)**2) +
|
||||
cb + glonass_bias + glonass_freq_slope*glonass_freq])
|
||||
|
||||
los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
|
||||
los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2)
|
||||
h_pseudorange_rate_sym = sp.Matrix([los_vector[0]*(sat_vx - vx) +
|
||||
los_vector[1]*(sat_vy - vy) +
|
||||
los_vector[2]*(sat_vz - vz) +
|
||||
cd])
|
||||
|
||||
obs_eqs = [[h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym],
|
||||
[h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym],
|
||||
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym],
|
||||
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym]]
|
||||
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds)
|
|
@ -2,6 +2,28 @@ import numpy as np
|
|||
import os
|
||||
from bisect import bisect
|
||||
from tqdm import tqdm
|
||||
from cffi import FFI
|
||||
|
||||
TEMPLATE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'templates'))
|
||||
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'generated'))
|
||||
|
||||
|
||||
def write_code(name, code, header):
|
||||
if not os.path.exists(GENERATED_DIR):
|
||||
os.mkdir(GENERATED_DIR)
|
||||
|
||||
open(os.path.join(GENERATED_DIR, f"{name}.cpp"), 'w').write(code)
|
||||
open(os.path.join(GENERATED_DIR, f"{name}.h"), 'w').write(header)
|
||||
|
||||
|
||||
def load_code(name):
|
||||
shared_fn = os.path.join(GENERATED_DIR, f"lib{name}.so")
|
||||
header_fn = os.path.join(GENERATED_DIR, f"{name}.h")
|
||||
header = open(header_fn).read()
|
||||
|
||||
ffi = FFI()
|
||||
ffi.cdef(header)
|
||||
return (ffi, ffi.dlopen(shared_fn))
|
||||
|
||||
|
||||
class KalmanError(Exception):
|
|
@ -1,14 +1,16 @@
|
|||
import os
|
||||
from bisect import bisect_right
|
||||
import sympy as sp
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
from numpy import dot
|
||||
from common.ffi_wrapper import compile_code, wrap_compiled
|
||||
from common.sympy_helpers import sympy_into_c
|
||||
from .chi2_lookup import chi2_ppf
|
||||
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import sympy_into_c
|
||||
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
|
||||
write_code)
|
||||
|
||||
from selfdrive.locationd.kalman.helpers.chi2_lookup import chi2_ppf
|
||||
|
||||
EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
|
||||
|
||||
def solve(a, b):
|
||||
if a.shape[0] == 1 and a.shape[1] == 1:
|
||||
|
@ -17,6 +19,7 @@ def solve(a, b):
|
|||
else:
|
||||
return np.linalg.solve(a, b)
|
||||
|
||||
|
||||
def null(H, eps=1e-12):
|
||||
u, s, vh = np.linalg.svd(H)
|
||||
padding = max(0,np.shape(H)[1]-np.shape(s)[0])
|
||||
|
@ -24,6 +27,7 @@ def null(H, eps=1e-12):
|
|||
null_space = np.compress(null_mask, vh, axis=0)
|
||||
return np.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)
|
||||
|
@ -129,11 +133,13 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
|
|||
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
|
||||
code += '\nextern "C"{\n' + extra_header + "\n}\n"
|
||||
code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read()
|
||||
code += '\nextern "C"{\n' + extra_post + "\n}\n"
|
||||
header += "\n" + extra_header
|
||||
compile_code(name, code, header, EXTERNAL_PATH)
|
||||
|
||||
write_code(name, code, header)
|
||||
|
||||
|
||||
class EKF_sym():
|
||||
def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err,
|
||||
|
@ -174,7 +180,7 @@ class EKF_sym():
|
|||
self.rewind_obscache = []
|
||||
self.init_state(x_initial, P_initial, None)
|
||||
|
||||
ffi, lib = wrap_compiled(name, EXTERNAL_PATH)
|
||||
ffi, lib = load_code(name)
|
||||
kinds, self.feature_track_kinds = [], []
|
||||
for func in dir(lib):
|
||||
if func[:2] == 'h_':
|
||||
|
@ -517,9 +523,6 @@ class EKF_sym():
|
|||
else:
|
||||
return True
|
||||
|
||||
|
||||
|
||||
|
||||
def rts_smooth(self, estimates, norm_quats=False):
|
||||
'''
|
||||
Returns rts smoothed results of
|
|
@ -0,0 +1,159 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
import common.transformations.orientation as orient
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import quat_matrix_l
|
||||
from selfdrive.locationd.kalman.helpers import TEMPLATE_DIR, write_code, load_code
|
||||
|
||||
|
||||
def sane(track):
|
||||
img_pos = track[1:,2:4]
|
||||
diffs_x = abs(img_pos[1:,0] - img_pos[:-1,0])
|
||||
diffs_y = abs(img_pos[1:,1] - img_pos[:-1,1])
|
||||
for i in range(1, len(diffs_x)):
|
||||
if ((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and \
|
||||
(diffs_x[i] > 2*diffs_x[i-1] or \
|
||||
diffs_x[i] < .5*diffs_x[i-1])) or \
|
||||
((diffs_y[i] > 0.05 or diffs_y[i-1] > 0.05) and \
|
||||
(diffs_y[i] > 2*diffs_y[i-1] or \
|
||||
diffs_y[i] < .5*diffs_y[i-1])):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
class FeatureHandler():
|
||||
name = 'feature_handler'
|
||||
|
||||
@staticmethod
|
||||
def generate_code(K=5):
|
||||
# Wrap c code for slow matching
|
||||
c_header = "\nvoid merge_features(double *tracks, double *features, long long *empty_idxs);"
|
||||
|
||||
c_code = "#include <math.h>\n"
|
||||
c_code += "#include <string.h>\n"
|
||||
c_code += "#define K %d\n" % K
|
||||
c_code += "\n" + open(os.path.join(TEMPLATE_DIR, "feature_handler.c")).read()
|
||||
|
||||
filename = f"{FeatureHandler.name}_{K}"
|
||||
write_code(filename, c_code, c_header)
|
||||
|
||||
def __init__(self, K=5):
|
||||
self.MAX_TRACKS = 6000
|
||||
self.K = K
|
||||
|
||||
#Array of tracks, each track
|
||||
#has K 5D features preceded
|
||||
#by 5 params that inidicate
|
||||
#[f_idx, last_idx, updated, complete, valid]
|
||||
# f_idx: idx of current last feature in track
|
||||
# idx of of last feature in frame
|
||||
# bool for whether this track has been update
|
||||
# bool for whether this track is complete
|
||||
# bool for whether this track is valid
|
||||
self.tracks = np.zeros((self.MAX_TRACKS, K+1, 5))
|
||||
self.tracks[:] = np.nan
|
||||
|
||||
name = f"{FeatureHandler.name}_{K}"
|
||||
ffi, lib = load_code(name)
|
||||
|
||||
def merge_features_c(tracks, features, empty_idxs):
|
||||
lib.merge_features(ffi.cast("double *", tracks.ctypes.data),
|
||||
ffi.cast("double *", features.ctypes.data),
|
||||
ffi.cast("long long *", empty_idxs.ctypes.data))
|
||||
|
||||
#self.merge_features = self.merge_features_python
|
||||
self.merge_features = merge_features_c
|
||||
|
||||
def reset(self):
|
||||
self.tracks[:] = np.nan
|
||||
|
||||
def merge_features_python(self, tracks, features, empty_idxs):
|
||||
empty_idx = 0
|
||||
for f in features:
|
||||
match_idx = int(f[4])
|
||||
if tracks[match_idx, 0, 1] == match_idx and tracks[match_idx, 0 ,2] == 0:
|
||||
tracks[match_idx, 0, 0] += 1
|
||||
tracks[match_idx, 0, 1] = f[1]
|
||||
tracks[match_idx, 0, 2] = 1
|
||||
tracks[match_idx, int(tracks[match_idx, 0, 0])] = f
|
||||
if tracks[match_idx, 0, 0] == self.K:
|
||||
tracks[match_idx, 0, 3] = 1
|
||||
if sane(tracks[match_idx]):
|
||||
tracks[match_idx, 0, 4] = 1
|
||||
else:
|
||||
if empty_idx == len(empty_idxs):
|
||||
print('need more empty space')
|
||||
continue
|
||||
tracks[empty_idxs[empty_idx], 0, 0] = 1
|
||||
tracks[empty_idxs[empty_idx], 0, 1] = f[1]
|
||||
tracks[empty_idxs[empty_idx], 0, 2] = 1
|
||||
tracks[empty_idxs[empty_idx], 1] = f
|
||||
empty_idx += 1
|
||||
|
||||
def update_tracks(self, features):
|
||||
t0 = time.time()
|
||||
last_idxs = np.copy(self.tracks[:,0,1])
|
||||
real = np.isfinite(last_idxs)
|
||||
self.tracks[last_idxs[real].astype(int)] = self.tracks[real]
|
||||
mask = np.ones(self.MAX_TRACKS, np.bool)
|
||||
mask[last_idxs[real].astype(int)] = 0
|
||||
empty_idxs = np.arange(self.MAX_TRACKS)[mask]
|
||||
self.tracks[empty_idxs] = np.nan
|
||||
self.tracks[:,0,2] = 0
|
||||
self.merge_features(self.tracks, features, empty_idxs)
|
||||
|
||||
def handle_features(self, features):
|
||||
self.update_tracks(features)
|
||||
valid_idxs = self.tracks[:,0,4] == 1
|
||||
complete_idxs = self.tracks[:,0,3] == 1
|
||||
stale_idxs = self.tracks[:,0,2] == 0
|
||||
valid_tracks = self.tracks[valid_idxs]
|
||||
self.tracks[complete_idxs] = np.nan
|
||||
self.tracks[stale_idxs] = np.nan
|
||||
return valid_tracks[:,1:,:4].reshape((len(valid_tracks), self.K*4))
|
||||
|
||||
|
||||
|
||||
def generate_orient_error_jac(K):
|
||||
import sympy as sp
|
||||
from common.sympy_helpers import quat_rotate
|
||||
x_sym = sp.MatrixSymbol('abr', 3,1)
|
||||
dtheta = sp.MatrixSymbol('dtheta', 3,1)
|
||||
delta_quat = sp.Matrix(np.ones(4))
|
||||
delta_quat[1:,:] = sp.Matrix(0.5*dtheta[0:3,:])
|
||||
poses_sym = sp.MatrixSymbol('poses', 7*K,1)
|
||||
img_pos_sym = sp.MatrixSymbol('img_positions', 2*K,1)
|
||||
alpha, beta, rho = x_sym
|
||||
to_c = sp.Matrix(orient.rot_matrix(-np.pi/2, -np.pi/2, 0))
|
||||
pos_0 = sp.Matrix(np.array(poses_sym[K*7-7:K*7-4])[:,0])
|
||||
q = quat_matrix_l(poses_sym[K*7-4:K*7])*delta_quat
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_0 = to_c*quat_rot.T
|
||||
rows = []
|
||||
for i in range(K):
|
||||
pos_i = sp.Matrix(np.array(poses_sym[i*7:i*7+3])[:,0])
|
||||
q = quat_matrix_l(poses_sym[7*i+3:7*i+7])*delta_quat
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_i = to_c*quat_rot.T
|
||||
rot_0_to_i = rot_g_to_i*(rot_g_to_0.T)
|
||||
trans_0_to_i = rot_g_to_i*(pos_0 - pos_i)
|
||||
funct_vec = rot_0_to_i*sp.Matrix([alpha, beta, 1]) + rho*trans_0_to_i
|
||||
h1, h2, h3 = funct_vec
|
||||
rows.append(h1/h3 - img_pos_sym[i*2 +0])
|
||||
rows.append(h2/h3 - img_pos_sym[i*2 + 1])
|
||||
img_pos_residual_sym = sp.Matrix(rows)
|
||||
|
||||
# sympy into c
|
||||
sympy_functions = []
|
||||
sympy_functions.append(('orient_error_jac', img_pos_residual_sym.jacobian(dtheta), [x_sym, poses_sym, img_pos_sym, dtheta]))
|
||||
|
||||
return sympy_functions
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# TODO: get K from argparse
|
||||
FeatureHandler.generate_code()
|
|
@ -0,0 +1,177 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
import scipy.optimize as opt
|
||||
import sympy as sp
|
||||
|
||||
import common.transformations.orientation as orient
|
||||
from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
|
||||
write_code)
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import (quat_rotate,
|
||||
sympy_into_c)
|
||||
|
||||
|
||||
def generate_residual(K):
|
||||
x_sym = sp.MatrixSymbol('abr', 3,1)
|
||||
poses_sym = sp.MatrixSymbol('poses', 7*K,1)
|
||||
img_pos_sym = sp.MatrixSymbol('img_positions', 2*K,1)
|
||||
alpha, beta, rho = x_sym
|
||||
to_c = sp.Matrix(orient.rot_matrix(-np.pi/2, -np.pi/2, 0))
|
||||
pos_0 = sp.Matrix(np.array(poses_sym[K*7-7:K*7-4])[:,0])
|
||||
q = poses_sym[K*7-4:K*7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_0 = to_c*quat_rot.T
|
||||
rows = []
|
||||
for i in range(K):
|
||||
pos_i = sp.Matrix(np.array(poses_sym[i*7:i*7+3])[:,0])
|
||||
q = poses_sym[7*i+3:7*i+7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
rot_g_to_i = to_c*quat_rot.T
|
||||
rot_0_to_i = rot_g_to_i*(rot_g_to_0.T)
|
||||
trans_0_to_i = rot_g_to_i*(pos_0 - pos_i)
|
||||
funct_vec = rot_0_to_i*sp.Matrix([alpha, beta, 1]) + rho*trans_0_to_i
|
||||
h1, h2, h3 = funct_vec
|
||||
rows.append(h1/h3 - img_pos_sym[i*2 +0])
|
||||
rows.append(h2/h3 - img_pos_sym[i*2 + 1])
|
||||
img_pos_residual_sym = sp.Matrix(rows)
|
||||
|
||||
# sympy into c
|
||||
sympy_functions = []
|
||||
sympy_functions.append(('res_fun', img_pos_residual_sym, [x_sym, poses_sym, img_pos_sym]))
|
||||
sympy_functions.append(('jac_fun', img_pos_residual_sym.jacobian(x_sym), [x_sym, poses_sym, img_pos_sym]))
|
||||
|
||||
return sympy_functions
|
||||
|
||||
|
||||
class LstSqComputer():
|
||||
name = 'pos_computer'
|
||||
|
||||
@staticmethod
|
||||
def generate_code(K=4):
|
||||
sympy_functions = generate_residual(K)
|
||||
header, code = sympy_into_c(sympy_functions)
|
||||
|
||||
code += "\n#define KDIM %d\n" % K
|
||||
code += "\n" + open(os.path.join(TEMPLATE_DIR, "compute_pos.c")).read()
|
||||
|
||||
header += """
|
||||
void compute_pos(double *to_c, double *in_poses, double *in_img_positions, double *param, double *pos);
|
||||
"""
|
||||
|
||||
filename = f"{LstSqComputer.name}_{K}"
|
||||
write_code(filename, code, header)
|
||||
|
||||
def __init__(self, K=4, MIN_DEPTH=2, MAX_DEPTH=500):
|
||||
self.to_c = orient.rot_matrix(-np.pi/2, -np.pi/2, 0)
|
||||
self.MAX_DEPTH = MAX_DEPTH
|
||||
self.MIN_DEPTH = MIN_DEPTH
|
||||
|
||||
name = f"{LstSqComputer.name}_{K}"
|
||||
ffi, lib = load_code(name)
|
||||
|
||||
# wrap c functions
|
||||
def residual_jac(x, poses, img_positions):
|
||||
out = np.zeros(((K*2, 3)), dtype=np.float64)
|
||||
lib.jac_fun(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return out
|
||||
self.residual_jac = residual_jac
|
||||
|
||||
def residual(x, poses, img_positions):
|
||||
out = np.zeros((K*2), dtype=np.float64)
|
||||
lib.res_fun(ffi.cast("double *", x.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", out.ctypes.data))
|
||||
return out
|
||||
self.residual = residual
|
||||
|
||||
def compute_pos_c(poses, img_positions):
|
||||
pos = np.zeros(3, dtype=np.float64)
|
||||
param = np.zeros(3, dtype=np.float64)
|
||||
# Can't be a view for the ctype
|
||||
img_positions = np.copy(img_positions)
|
||||
lib.compute_pos(ffi.cast("double *", self.to_c.ctypes.data),
|
||||
ffi.cast("double *", poses.ctypes.data),
|
||||
ffi.cast("double *", img_positions.ctypes.data),
|
||||
ffi.cast("double *", param.ctypes.data),
|
||||
ffi.cast("double *", pos.ctypes.data))
|
||||
return pos, param
|
||||
self.compute_pos_c = compute_pos_c
|
||||
|
||||
def compute_pos(self, poses, img_positions, debug=False):
|
||||
pos, param = self.compute_pos_c(poses, img_positions)
|
||||
#pos, param = self.compute_pos_python(poses, img_positions)
|
||||
depth = 1/param[2]
|
||||
if debug:
|
||||
if not self.debug:
|
||||
raise NotImplementedError("This is not a debug computer")
|
||||
#orient_err_jac = self.orient_error_jac(param, poses, img_positions, np.zeros(3)).reshape((-1,2,3))
|
||||
jac = self.residual_jac(param, poses, img_positions).reshape((-1,2,3))
|
||||
res = self.residual(param, poses, img_positions).reshape((-1,2))
|
||||
return pos, param, res, jac #, orient_err_jac
|
||||
elif (self.MIN_DEPTH < depth < self.MAX_DEPTH):
|
||||
return pos
|
||||
else:
|
||||
return None
|
||||
|
||||
def gauss_newton(self, fun, jac, x, args):
|
||||
poses, img_positions = args
|
||||
delta = 1
|
||||
counter = 0
|
||||
while abs(np.linalg.norm(delta)) > 1e-4 and counter < 30:
|
||||
delta = np.linalg.pinv(jac(x, poses, img_positions)).dot(fun(x, poses, img_positions))
|
||||
x = x - delta
|
||||
counter += 1
|
||||
return [x]
|
||||
|
||||
def compute_pos_python(self, poses, img_positions, check_quality=False):
|
||||
# This procedure is also described
|
||||
# in the MSCKF paper (Mourikis et al. 2007)
|
||||
x = np.array([img_positions[-1][0],
|
||||
img_positions[-1][1], 0.1])
|
||||
res = opt.leastsq(self.residual, x, Dfun=self.residual_jac, args=(poses, img_positions)) # scipy opt
|
||||
#res = self.gauss_newton(self.residual, self.residual_jac, x, (poses, img_positions)) # diy gauss_newton
|
||||
|
||||
alpha, beta, rho = res[0]
|
||||
rot_0_to_g = (orient.rotations_from_quats(poses[-1,3:])).dot(self.to_c.T)
|
||||
return (rot_0_to_g.dot(np.array([alpha, beta, 1])))/rho + poses[-1,:3]
|
||||
|
||||
|
||||
|
||||
|
||||
'''
|
||||
EXPERIMENTAL CODE
|
||||
'''
|
||||
def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos):
|
||||
# only speed correction for now
|
||||
t_roll = 0.016 # 16ms rolling shutter?
|
||||
vroll, vpitch, vyaw = rot_rates
|
||||
A = 0.5*np.array([[-1, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A.dot(poses[-1][3:7])
|
||||
v = np.append(v, q_dot)
|
||||
v = np.array([v[0], v[1], v[2],0,0,0,0])
|
||||
current_pose = poses[-1] + v*0.05
|
||||
poses = np.vstack((current_pose, poses))
|
||||
dt = -img_positions[:,1]*t_roll/0.48
|
||||
errs = project(poses, ecef_pos) - project(poses + np.atleast_2d(dt).T.dot(np.atleast_2d(v)), ecef_pos)
|
||||
return img_positions - errs
|
||||
|
||||
def project(poses, ecef_pos):
|
||||
img_positions = np.zeros((len(poses), 2))
|
||||
for i, p in enumerate(poses):
|
||||
cam_frame = orient.rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3])
|
||||
img_positions[i] = np.array([cam_frame[1]/cam_frame[0], cam_frame[2]/cam_frame[0]])
|
||||
return img_positions
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# TODO: get K from argparse
|
||||
LstSqComputer.generate_code()
|
|
@ -78,4 +78,6 @@ def sympy_into_c(sympy_functions):
|
|||
c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
|
||||
c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
|
||||
|
||||
c_code = 'extern "C" {\n#include <math.h>\n' + c_code + "\n}\n"
|
||||
|
||||
return c_header, c_code
|
|
@ -1,142 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.locationd.kalman.live_model import gen_model, States
|
||||
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind, KalmanError
|
||||
from selfdrive.locationd.kalman.ekf_sym import EKF_sym
|
||||
|
||||
|
||||
initial_x = np.array([-2.7e6, 4.2e6, 3.8e6,
|
||||
1, 0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
1,
|
||||
0, 0, 0,
|
||||
0, 0, 0])
|
||||
|
||||
|
||||
# state covariance
|
||||
initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
|
||||
10**2, 10**2, 10**2,
|
||||
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,
|
||||
(0.01)**2, (0.01)**2, (0.01)**2])
|
||||
|
||||
|
||||
class LiveKalman():
|
||||
def __init__(self):
|
||||
# process noise
|
||||
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.1**2, 0.1**2, 0.1**2,
|
||||
(0.005/100)**2, (0.005/100)**2, (0.005/100)**2,
|
||||
(0.02/100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
(0.05/60)**2, (0.05/60)**2, (0.05/60)**2])
|
||||
|
||||
self.dim_state = initial_x.shape[0]
|
||||
self.dim_state_err = initial_P_diag.shape[0]
|
||||
|
||||
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]),
|
||||
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5*2]),
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
|
||||
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
|
||||
|
||||
name = 'live'
|
||||
gen_model(name, self.dim_state, self.dim_state_err, [])
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(name, Q, initial_x, np.diag(initial_P_diag), self.dim_state, self.dim_state_err)
|
||||
|
||||
@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):
|
||||
return self.filter.predict(t)
|
||||
|
||||
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)))
|
||||
|
||||
# Normalize quats
|
||||
quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
|
||||
|
||||
# Should not continue if the quats behave this weirdly
|
||||
if not (0.1 < quat_norm < 10):
|
||||
cloudlog.error("Kalman filter quaternions unstable")
|
||||
raise KalmanError
|
||||
|
||||
self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
|
||||
|
||||
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__":
|
||||
LiveKalman()
|
|
@ -1,178 +0,0 @@
|
|||
import numpy as np
|
||||
import sympy as sp
|
||||
import os
|
||||
import sysconfig
|
||||
|
||||
from laika.constants import EARTH_GM
|
||||
from common.sympy_helpers import euler_rotate, quat_rotate, quat_matrix_r
|
||||
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
|
||||
from selfdrive.locationd.kalman.ekf_sym import gen_code
|
||||
|
||||
|
||||
class States():
|
||||
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
|
||||
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
|
||||
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
|
||||
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
|
||||
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
|
||||
ODO_SCALE = slice(16, 17) # odometer scale
|
||||
ACCELERATION = slice(17, 20) # Acceleration in device frame in m/s**2
|
||||
IMU_OFFSET = slice(20, 23) # imu offset angles in radians
|
||||
|
||||
ECEF_POS_ERR = slice(0, 3)
|
||||
ECEF_ORIENTATION_ERR = slice(3, 6)
|
||||
ECEF_VELOCITY_ERR = slice(6, 9)
|
||||
ANGULAR_VELOCITY_ERR = slice(9, 12)
|
||||
GYRO_BIAS_ERR = slice(12, 15)
|
||||
ODO_SCALE_ERR = slice(15, 16)
|
||||
ACCELERATION_ERR = slice(16, 19)
|
||||
IMU_OFFSET_ERR = slice(19, 22)
|
||||
|
||||
|
||||
def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
|
||||
# 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 + '/' + name + '_model.py',
|
||||
dir_path + '/' + name + '_kf.py']
|
||||
|
||||
outs = [dir_path + '/' + name + '.o',
|
||||
dir_path + '/' + name + sysconfig.get_config_var('EXT_SUFFIX'),
|
||||
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 as e:
|
||||
print('HAHAHA')
|
||||
print(e)
|
||||
pass
|
||||
|
||||
# make functions and jacobians with sympy
|
||||
# state variables
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x,y,z = state[States.ECEF_POS,:]
|
||||
q = state[States.ECEF_ORIENTATION,:]
|
||||
v = state[States.ECEF_VELOCITY,:]
|
||||
vx, vy, vz = v
|
||||
omega = state[States.ANGULAR_VELOCITY,:]
|
||||
vroll, vpitch, vyaw = omega
|
||||
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS,:]
|
||||
odo_scale = state[16,:]
|
||||
acceleration = state[States.ACCELERATION,:]
|
||||
imu_angles= state[States.IMU_OFFSET,:]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
# calibration and attitude rotation matrices
|
||||
quat_rot = quat_rotate(*q)
|
||||
|
||||
# Got the quat predict equations from here
|
||||
# A New Quaternion-Based Kalman Filter for
|
||||
# Real-Time Attitude Estimation Using the Two-Step
|
||||
# Geometrically-Intuitive Correction Algorithm
|
||||
A = 0.5*sp.Matrix([[0, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A * q
|
||||
|
||||
# Time derivative of the state as a function of state
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[States.ECEF_POS,:] = v
|
||||
state_dot[States.ECEF_ORIENTATION,:] = q_dot
|
||||
state_dot[States.ECEF_VELOCITY,0] = quat_rot * acceleration
|
||||
|
||||
# Basic descretization, 1st order intergrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt*state_dot
|
||||
|
||||
state_err_sym = sp.MatrixSymbol('state_err',dim_state_err,1)
|
||||
state_err = sp.Matrix(state_err_sym)
|
||||
quat_err = state_err[States.ECEF_ORIENTATION_ERR,:]
|
||||
v_err = state_err[States.ECEF_VELOCITY_ERR,:]
|
||||
omega_err = state_err[States.ANGULAR_VELOCITY_ERR,:]
|
||||
acceleration_err = state_err[States.ACCELERATION_ERR,:]
|
||||
|
||||
# Time derivative of the state error as a function of state error and state
|
||||
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
|
||||
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
|
||||
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
state_err_dot[States.ECEF_POS_ERR,:] = v_err
|
||||
state_err_dot[States.ECEF_ORIENTATION_ERR,:] = q_err_dot
|
||||
state_err_dot[States.ECEF_VELOCITY_ERR,:] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
|
||||
f_err_sym = state_err + dt*state_err_dot
|
||||
|
||||
# Observation matrix modifier
|
||||
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
|
||||
H_mod_sym[0:3, 0:3] = np.eye(3)
|
||||
H_mod_sym[3:7,3:6] = 0.5*quat_matrix_r(state[3:7])[:,1:]
|
||||
H_mod_sym[7:, 6:] = np.eye(dim_state-7)
|
||||
|
||||
# these error functions are defined so that say there
|
||||
# is a nominal x and true x:
|
||||
# true x = err_function(nominal x, delta x)
|
||||
# delta x = inv_err_function(nominal x, true x)
|
||||
nom_x = sp.MatrixSymbol('nom_x',dim_state,1)
|
||||
true_x = sp.MatrixSymbol('true_x',dim_state,1)
|
||||
delta_x = sp.MatrixSymbol('delta_x',dim_state_err,1)
|
||||
|
||||
err_function_sym = sp.Matrix(np.zeros((dim_state,1)))
|
||||
delta_quat = sp.Matrix(np.ones((4)))
|
||||
delta_quat[1:,:] = sp.Matrix(0.5*delta_x[3:6,:])
|
||||
err_function_sym[0:3,:] = sp.Matrix(nom_x[0:3,:] + delta_x[0:3,:])
|
||||
err_function_sym[3:7,0] = quat_matrix_r(nom_x[3:7,0])*delta_quat
|
||||
err_function_sym[7:,:] = sp.Matrix(nom_x[7:,:] + delta_x[6:,:])
|
||||
|
||||
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err,1)))
|
||||
inv_err_function_sym[0:3,0] = sp.Matrix(-nom_x[0:3,0] + true_x[0:3,0])
|
||||
delta_quat = quat_matrix_r(nom_x[3:7,0]).T*true_x[3:7,0]
|
||||
inv_err_function_sym[3:6,0] = sp.Matrix(2*delta_quat[1:])
|
||||
inv_err_function_sym[6:,0] = sp.Matrix(-nom_x[7:,0] + true_x[7:,0])
|
||||
|
||||
eskf_params = [[err_function_sym, nom_x, delta_x],
|
||||
[inv_err_function_sym, nom_x, true_x],
|
||||
H_mod_sym, f_err_sym, state_err_sym]
|
||||
|
||||
|
||||
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
|
||||
|
||||
imu_rot = euler_rotate(*imu_angles)
|
||||
h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
|
||||
vpitch + pitch_bias,
|
||||
vyaw + yaw_bias])
|
||||
|
||||
pos = sp.Matrix([x, y, z])
|
||||
gravity = quat_rot.T * ((EARTH_GM/((x**2 + y**2 + z**2)**(3.0/2.0)))*pos)
|
||||
h_acc_sym = imu_rot*(gravity + acceleration)
|
||||
h_phone_rot_sym = sp.Matrix([vroll,
|
||||
vpitch,
|
||||
vyaw])
|
||||
speed = vx**2 + vy**2 + vz**2
|
||||
h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])
|
||||
|
||||
h_pos_sym = sp.Matrix([x, y, z])
|
||||
h_imu_frame_sym = sp.Matrix(imu_angles)
|
||||
|
||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||
|
||||
|
||||
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_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
|
||||
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
|
|
@ -1,323 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from . import loc_model
|
||||
|
||||
from .kalman_helpers import ObservationKind
|
||||
from .ekf_sym import EKF_sym
|
||||
from .feature_handler import LstSqComputer, unroll_shutter
|
||||
from laika.raw_gnss import GNSSMeasurement
|
||||
|
||||
|
||||
def parse_prr(m):
|
||||
sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
|
||||
m[GNSSMeasurement.SAT_VEL]))
|
||||
R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
|
||||
z_i = m[GNSSMeasurement.PRR]
|
||||
return z_i, R_i, sat_pos_vel_i
|
||||
|
||||
|
||||
def parse_pr(m):
|
||||
pseudorange = m[GNSSMeasurement.PR]
|
||||
pseudorange_stdev = m[GNSSMeasurement.PR_STD]
|
||||
sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
|
||||
np.array([m[GNSSMeasurement.GLONASS_FREQ]])))
|
||||
z_i = np.atleast_1d(pseudorange)
|
||||
R_i = np.atleast_2d(pseudorange_stdev**2)
|
||||
return z_i, R_i, sat_pos_freq_i
|
||||
|
||||
|
||||
class States():
|
||||
ECEF_POS = slice(0,3) # x, y and z in ECEF in meters
|
||||
ECEF_ORIENTATION = slice(3,7) # quat for pose of phone in ecef
|
||||
ECEF_VELOCITY = slice(7,10) # ecef velocity in m/s
|
||||
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
|
||||
CLOCK_BIAS = slice(13, 14) # clock bias in light-meters,
|
||||
CLOCK_DRIFT = slice(14, 15) # clock drift in light-meters/s,
|
||||
GYRO_BIAS = slice(15, 18) # roll, pitch and yaw biases
|
||||
ODO_SCALE = slice(18, 19) # odometer scale
|
||||
ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2
|
||||
FOCAL_SCALE = slice(22, 23) # focal length scale
|
||||
IMU_OFFSET = slice(23,26) # imu offset angles in radians
|
||||
GLONASS_BIAS = slice(26,27) # GLONASS bias in m expressed as bias + freq_num*freq_slope
|
||||
GLONASS_FREQ_SLOPE = slice(27, 28) # GLONASS bias in m expressed as bias + freq_num*freq_slope
|
||||
CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2,
|
||||
|
||||
|
||||
class LocKalman():
|
||||
def __init__(self, N=0, max_tracks=3000):
|
||||
x_initial = np.array([-2.7e6, 4.2e6, 3.8e6,
|
||||
1, 0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0,
|
||||
0, 0, 0,
|
||||
1,
|
||||
0, 0, 0,
|
||||
1,
|
||||
0, 0, 0,
|
||||
0, 0,
|
||||
0])
|
||||
|
||||
# state covariance
|
||||
P_initial = np.diag([10000**2, 10000**2, 10000**2,
|
||||
10**2, 10**2, 10**2,
|
||||
10**2, 10**2, 10**2,
|
||||
1**2, 1**2, 1**2,
|
||||
(200000)**2, (100)**2,
|
||||
0.05**2, 0.05**2, 0.05**2,
|
||||
0.02**2,
|
||||
1**2, 1**2, 1**2,
|
||||
0.01**2,
|
||||
(0.01)**2, (0.01)**2, (0.01)**2,
|
||||
10**2, 1**2,
|
||||
0.05**2])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.1**2, 0.1**2, 0.1**2,
|
||||
(.1)**2, (0.0)**2,
|
||||
(0.005/100)**2, (0.005/100)**2, (0.005/100)**2,
|
||||
(0.02/100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
0.001**2,
|
||||
(0.05/60)**2, (0.05/60)**2, (0.05/60)**2,
|
||||
(.1)**2, (.01)**2,
|
||||
0.005**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]),
|
||||
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5*2]),
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
|
||||
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
|
||||
|
||||
# MSCKF stuff
|
||||
self.N = N
|
||||
self.dim_main = x_initial.shape[0]
|
||||
self.dim_augment = 7
|
||||
self.dim_main_err = P_initial.shape[0]
|
||||
self.dim_augment_err = 6
|
||||
self.dim_state = self.dim_main + self.dim_augment*self.N
|
||||
self.dim_state_err = self.dim_main_err + self.dim_augment_err*self.N
|
||||
|
||||
# mahalanobis outlier rejection
|
||||
maha_test_kinds = [ObservationKind.ORB_FEATURES] #, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
|
||||
|
||||
name = 'loc_%d' % N
|
||||
loc_model.gen_model(name, N, self.dim_main, self.dim_main_err,
|
||||
self.dim_augment, self.dim_augment_err,
|
||||
self.dim_state, self.dim_state_err,
|
||||
maha_test_kinds)
|
||||
|
||||
if self.N > 0:
|
||||
x_initial, P_initial, Q = self.pad_augmented(x_initial, P_initial, Q)
|
||||
self.computer = LstSqComputer(N)
|
||||
self.max_tracks = max_tracks
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err,
|
||||
N, self.dim_augment, self.dim_augment_err, maha_test_kinds)
|
||||
|
||||
@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):
|
||||
return self.filter.predict(t)
|
||||
|
||||
def rts_smooth(self, estimates):
|
||||
return self.filter.rts_smooth(estimates, norm_quats=True)
|
||||
|
||||
def pad_augmented(self, x, P, Q=None):
|
||||
if x.shape[0] == self.dim_main and self.N > 0:
|
||||
x = np.pad(x, (0, self.N*self.dim_augment), mode='constant')
|
||||
x[self.dim_main+3::7] = 1
|
||||
if P.shape[0] == self.dim_main_err and self.N > 0:
|
||||
P = np.pad(P, [(0, self.N*self.dim_augment_err), (0, self.N*self.dim_augment_err)], mode='constant')
|
||||
P[self.dim_main_err:, self.dim_main_err:] = 10e20*np.eye(self.dim_augment_err *self.N)
|
||||
if Q is None:
|
||||
return x, P
|
||||
else:
|
||||
Q = np.pad(Q, [(0, self.N*self.dim_augment_err), (0, self.N*self.dim_augment_err)], mode='constant')
|
||||
return x, P, Q
|
||||
|
||||
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()
|
||||
state, P = self.pad_augmented(state, P)
|
||||
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.PSEUDORANGE_GPS or kind == ObservationKind.PSEUDORANGE_GLONASS:
|
||||
r = self.predict_and_update_pseudorange(data, t, kind)
|
||||
elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS:
|
||||
r = self.predict_and_update_pseudorange_rate(data, t, kind)
|
||||
elif kind == ObservationKind.ORB_POINT:
|
||||
r = self.predict_and_update_orb(data, t, kind)
|
||||
elif kind == ObservationKind.ORB_FEATURES:
|
||||
r = self.predict_and_update_orb_features(data, t, kind)
|
||||
elif kind == ObservationKind.MSCKF_TEST:
|
||||
r = self.predict_and_update_msckf_test(data, t, kind)
|
||||
elif kind == ObservationKind.FEATURE_TRACK_TEST:
|
||||
r = self.predict_and_update_feature_track_test(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)))
|
||||
# Normalize quats
|
||||
quat_norm = np.linalg.norm(self.filter.x[3:7,0])
|
||||
# Should not continue if the quats behave this weirdly
|
||||
if not 0.1 < quat_norm < 10:
|
||||
raise RuntimeError("Sir! The filter's gone all wobbly!")
|
||||
self.filter.x[3:7,0] = self.filter.x[3:7,0]/quat_norm
|
||||
for i in range(self.N):
|
||||
d1 = self.dim_main
|
||||
d3 = self.dim_augment
|
||||
self.filter.x[d1+d3*i+3:d1+d3*i+7] /= np.linalg.norm(self.filter.x[d1+i*d3 + 3:d1+i*d3 + 7,0])
|
||||
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_pseudorange(self, meas, t, kind):
|
||||
R = np.zeros((len(meas), 1, 1))
|
||||
sat_pos_freq = np.zeros((len(meas), 4))
|
||||
z = np.zeros((len(meas), 1))
|
||||
for i, m in enumerate(meas):
|
||||
z_i, R_i, sat_pos_freq_i = parse_pr(m)
|
||||
sat_pos_freq[i,:] = sat_pos_freq_i
|
||||
z[i,:] = z_i
|
||||
R[i,:,:] = R_i
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq)
|
||||
|
||||
|
||||
def predict_and_update_pseudorange_rate(self, meas, t, kind):
|
||||
R = np.zeros((len(meas), 1, 1))
|
||||
z = np.zeros((len(meas), 1))
|
||||
sat_pos_vel = np.zeros((len(meas), 6))
|
||||
for i, m in enumerate(meas):
|
||||
z_i, R_i, sat_pos_vel_i = parse_prr(m)
|
||||
sat_pos_vel[i] = sat_pos_vel_i
|
||||
R[i,:,:] = R_i
|
||||
z[i, :] = z_i
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel)
|
||||
|
||||
def predict_and_update_orb(self, orb, t, kind):
|
||||
true_pos = orb[:,2:]
|
||||
z = orb[:,:2]
|
||||
R = np.zeros((len(orb), 2, 2))
|
||||
for i, _ in enumerate(z):
|
||||
R[i,:,:] = np.diag([10**2, 10**2])
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, true_pos)
|
||||
|
||||
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)
|
||||
|
||||
def predict_and_update_orb_features(self, tracks, t, kind):
|
||||
k = 2*(self.N+1)
|
||||
R = np.zeros((len(tracks), k, k))
|
||||
z = np.zeros((len(tracks), k))
|
||||
ecef_pos = np.zeros((len(tracks), 3))
|
||||
ecef_pos[:] = np.nan
|
||||
poses = self.x[self.dim_main:].reshape((-1,7))
|
||||
times = tracks.reshape((len(tracks),self.N+1, 4))[:,:,0]
|
||||
good_counter = 0
|
||||
if times.any() and np.allclose(times[0,:-1], self.filter.augment_times, rtol=1e-6):
|
||||
for i, track in enumerate(tracks):
|
||||
img_positions = track.reshape((self.N+1, 4))[:,2:]
|
||||
# TODO not perfect as last pose not used
|
||||
#img_positions = unroll_shutter(img_positions, poses, self.filter.state()[7:10], self.filter.state()[10:13], ecef_pos[i])
|
||||
ecef_pos[i] = self.computer.compute_pos(poses, img_positions[:-1])
|
||||
z[i] = img_positions.flatten()
|
||||
R[i,:,:] = np.diag([0.005**2]*(k))
|
||||
if np.isfinite(ecef_pos[i][0]):
|
||||
good_counter += 1
|
||||
if good_counter > self.max_tracks:
|
||||
break
|
||||
good_idxs = np.all(np.isfinite(ecef_pos),axis=1)
|
||||
# have to do some weird stuff here to keep
|
||||
# to have the observations input from mesh3d
|
||||
# consistent with the outputs of the filter
|
||||
# Probably should be replaced, not sure how.
|
||||
ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], augment=True)
|
||||
if ret is None:
|
||||
return
|
||||
y_full = np.zeros((z.shape[0], z.shape[1] - 3))
|
||||
#print sum(good_idxs), len(tracks)
|
||||
if sum(good_idxs) > 0:
|
||||
y_full[good_idxs] = np.array(ret[6])
|
||||
ret = ret[:6] + (y_full, z, ecef_pos)
|
||||
return ret
|
||||
|
||||
def predict_and_update_msckf_test(self, test_data, t, kind):
|
||||
assert self.N > 0
|
||||
z = test_data
|
||||
R = np.zeros((len(test_data), len(z[0]), len(z[0])))
|
||||
ecef_pos = [self.x[:3]]
|
||||
for i, _ in enumerate(z):
|
||||
R[i,:,:] = np.diag([0.1**2]*len(z[0]))
|
||||
ret = self.filter.predict_and_update_batch(t, kind, z, R, ecef_pos)
|
||||
self.filter.augment()
|
||||
return ret
|
||||
|
||||
def maha_test_pseudorange(self, x, P, meas, kind, maha_thresh=.3):
|
||||
bools = []
|
||||
for i, m in enumerate(meas):
|
||||
z, R, sat_pos_freq = parse_pr(m)
|
||||
bools.append(self.filter.maha_test(x, P, kind, z, R, extra_args=sat_pos_freq, maha_thresh=maha_thresh))
|
||||
return np.array(bools)
|
||||
|
||||
def maha_test_pseudorange_rate(self, x, P, meas, kind, maha_thresh=.999):
|
||||
bools = []
|
||||
for i, m in enumerate(meas):
|
||||
z, R, sat_pos_vel = parse_prr(m)
|
||||
bools.append(self.filter.maha_test(x, P, kind, z, R, extra_args=sat_pos_vel, maha_thresh=maha_thresh))
|
||||
return np.array(bools)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
LocKalman(N=4)
|
|
@ -1,254 +0,0 @@
|
|||
import numpy as np
|
||||
import sympy as sp
|
||||
import os
|
||||
|
||||
from laika.constants import EARTH_GM
|
||||
from .kalman_helpers import ObservationKind
|
||||
from .ekf_sym import gen_code
|
||||
from common.sympy_helpers import cross, euler_rotate, quat_rotate, quat_matrix_l, quat_matrix_r
|
||||
|
||||
def gen_model(name, N, dim_main, dim_main_err,
|
||||
dim_augment, dim_augment_err,
|
||||
dim_state, dim_state_err,
|
||||
maha_test_kinds):
|
||||
|
||||
|
||||
# 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_model.py',
|
||||
dir_path + '/' + 'loc_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 as e:
|
||||
pass
|
||||
|
||||
# make functions and jacobians with sympy
|
||||
# state variables
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x,y,z = state[0:3,:]
|
||||
q = state[3:7,:]
|
||||
v = state[7:10,:]
|
||||
vx, vy, vz = v
|
||||
omega = state[10:13,:]
|
||||
vroll, vpitch, vyaw = omega
|
||||
cb, cd = state[13:15,:]
|
||||
roll_bias, pitch_bias, yaw_bias = state[15:18,:]
|
||||
odo_scale = state[18,:]
|
||||
acceleration = state[19:22,:]
|
||||
focal_scale = state[22,:]
|
||||
imu_angles= state[23:26,:]
|
||||
glonass_bias, glonass_freq_slope = state[26:28,:]
|
||||
ca = state[28,0]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
# calibration and attitude rotation matrices
|
||||
quat_rot = quat_rotate(*q)
|
||||
|
||||
# Got the quat predict equations from here
|
||||
# A New Quaternion-Based Kalman Filter for
|
||||
# Real-Time Attitude Estimation Using the Two-Step
|
||||
# Geometrically-Intuitive Correction Algorithm
|
||||
A = 0.5*sp.Matrix([[0, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A * q
|
||||
|
||||
# Time derivative of the state as a function of state
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[:3,:] = v
|
||||
state_dot[3:7,:] = q_dot
|
||||
state_dot[7:10,0] = quat_rot * acceleration
|
||||
state_dot[13,0] = cd
|
||||
state_dot[14,0] = ca
|
||||
|
||||
# Basic descretization, 1st order intergrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt*state_dot
|
||||
|
||||
state_err_sym = sp.MatrixSymbol('state_err',dim_state_err,1)
|
||||
state_err = sp.Matrix(state_err_sym)
|
||||
quat_err = state_err[3:6,:]
|
||||
v_err = state_err[6:9,:]
|
||||
omega_err = state_err[9:12,:]
|
||||
cd_err = state_err[13,:]
|
||||
acceleration_err = state_err[18:21,:]
|
||||
ca_err = state_err[27,:]
|
||||
|
||||
# Time derivative of the state error as a function of state error and state
|
||||
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
|
||||
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
|
||||
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
state_err_dot[:3,:] = v_err
|
||||
state_err_dot[3:6,:] = q_err_dot
|
||||
state_err_dot[6:9,:] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
|
||||
state_err_dot[12,:] = cd_err
|
||||
state_err_dot[13,:] = ca_err
|
||||
f_err_sym = state_err + dt*state_err_dot
|
||||
|
||||
# convenient indexing
|
||||
# q idxs are for quats and p idxs are for other
|
||||
q_idxs = [[3, dim_augment]] + [[dim_main + n*dim_augment + 3, dim_main + (n+1)*dim_augment] for n in range(N)]
|
||||
q_err_idxs = [[3, dim_augment_err]] + [[dim_main_err + n*dim_augment_err + 3, dim_main_err + (n+1)*dim_augment_err] for n in range(N)]
|
||||
p_idxs = [[0, 3]] + [[dim_augment, dim_main]] + [[dim_main + n*dim_augment , dim_main + n*dim_augment + 3] for n in range(N)]
|
||||
p_err_idxs = [[0, 3]] + [[dim_augment_err, dim_main_err]] + [[dim_main_err + n*dim_augment_err, dim_main_err + n*dim_augment_err + 3] for n in range(N)]
|
||||
|
||||
# Observation matrix modifier
|
||||
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
|
||||
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
|
||||
H_mod_sym[p_idx[0]:p_idx[1],p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1]-p_idx[0])
|
||||
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
|
||||
H_mod_sym[q_idx[0]:q_idx[1],q_err_idx[0]:q_err_idx[1]] = 0.5*quat_matrix_r(state[q_idx[0]:q_idx[1]])[:,1:]
|
||||
|
||||
|
||||
# these error functions are defined so that say there
|
||||
# is a nominal x and true x:
|
||||
# true x = err_function(nominal x, delta x)
|
||||
# delta x = inv_err_function(nominal x, true x)
|
||||
nom_x = sp.MatrixSymbol('nom_x',dim_state,1)
|
||||
true_x = sp.MatrixSymbol('true_x',dim_state,1)
|
||||
delta_x = sp.MatrixSymbol('delta_x',dim_state_err,1)
|
||||
|
||||
err_function_sym = sp.Matrix(np.zeros((dim_state,1)))
|
||||
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
|
||||
delta_quat = sp.Matrix(np.ones((4)))
|
||||
delta_quat[1:,:] = sp.Matrix(0.5*delta_x[q_err_idx[0]: q_err_idx[1],:])
|
||||
err_function_sym[q_idx[0]:q_idx[1],0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1],0])*delta_quat
|
||||
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
|
||||
err_function_sym[p_idx[0]:p_idx[1],:] = sp.Matrix(nom_x[p_idx[0]:p_idx[1],:] + delta_x[p_err_idx[0]:p_err_idx[1],:])
|
||||
|
||||
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err,1)))
|
||||
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
|
||||
inv_err_function_sym[p_err_idx[0]:p_err_idx[1],0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1],0] + true_x[p_idx[0]:p_idx[1],0])
|
||||
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
|
||||
delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1],0]).T*true_x[q_idx[0]:q_idx[1],0]
|
||||
inv_err_function_sym[q_err_idx[0]:q_err_idx[1],0] = sp.Matrix(2*delta_quat[1:])
|
||||
|
||||
eskf_params = [[err_function_sym, nom_x, delta_x],
|
||||
[inv_err_function_sym, nom_x, true_x],
|
||||
H_mod_sym, f_err_sym, state_err_sym]
|
||||
|
||||
|
||||
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
|
||||
# extra args
|
||||
sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1)
|
||||
sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1)
|
||||
sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1)
|
||||
orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1)
|
||||
|
||||
# expand extra args
|
||||
sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym
|
||||
sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:]
|
||||
los_x, los_y, los_z = sat_los_sym
|
||||
orb_x, orb_y, orb_z = orb_epos_sym
|
||||
|
||||
h_pseudorange_sym = sp.Matrix([sp.sqrt(
|
||||
(x - sat_x)**2 +
|
||||
(y - sat_y)**2 +
|
||||
(z - sat_z)**2) +
|
||||
cb])
|
||||
|
||||
h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt(
|
||||
(x - sat_x)**2 +
|
||||
(y - sat_y)**2 +
|
||||
(z - sat_z)**2) +
|
||||
cb + glonass_bias + glonass_freq_slope*glonass_freq])
|
||||
|
||||
los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
|
||||
los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2)
|
||||
h_pseudorange_rate_sym = sp.Matrix([los_vector[0]*(sat_vx - vx) +
|
||||
los_vector[1]*(sat_vy - vy) +
|
||||
los_vector[2]*(sat_vz - vz) +
|
||||
cd])
|
||||
|
||||
imu_rot = euler_rotate(*imu_angles)
|
||||
h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
|
||||
vpitch + pitch_bias,
|
||||
vyaw + yaw_bias])
|
||||
|
||||
pos = sp.Matrix([x, y, z])
|
||||
gravity = quat_rot.T * ((EARTH_GM/((x**2 + y**2 + z**2)**(3.0/2.0)))*pos)
|
||||
h_acc_sym = imu_rot*(gravity + acceleration)
|
||||
h_phone_rot_sym = sp.Matrix([vroll,
|
||||
vpitch,
|
||||
vyaw])
|
||||
speed = vx**2 + vy**2 + vz**2
|
||||
h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])
|
||||
|
||||
# orb stuff
|
||||
orb_pos_sym = sp.Matrix([orb_x - x, orb_y - y, orb_z - z])
|
||||
orb_pos_rot_sym = quat_rot.T * orb_pos_sym
|
||||
s = orb_pos_rot_sym[0]
|
||||
h_orb_point_sym = sp.Matrix([(1/s)*(orb_pos_rot_sym[1]),
|
||||
(1/s)*(orb_pos_rot_sym[2])])
|
||||
|
||||
h_pos_sym = sp.Matrix([x, y, z])
|
||||
h_imu_frame_sym = sp.Matrix(imu_angles)
|
||||
|
||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||
|
||||
|
||||
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_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||
[h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym],
|
||||
[h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym],
|
||||
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym],
|
||||
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym],
|
||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None],
|
||||
[h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym]]
|
||||
|
||||
# MSCKF configuration
|
||||
if N > 0:
|
||||
focal_scale =1
|
||||
# Add observation functions for orb feature tracks
|
||||
track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1)
|
||||
track_x, track_y, track_z = track_epos_sym
|
||||
h_track_sym = sp.Matrix(np.zeros(((1 + N)*2, 1)))
|
||||
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
|
||||
track_pos_rot_sym = quat_rot.T * track_pos_sym
|
||||
h_track_sym[-2:,:] = sp.Matrix([focal_scale*(track_pos_rot_sym[1]/track_pos_rot_sym[0]),
|
||||
focal_scale*(track_pos_rot_sym[2]/track_pos_rot_sym[0])])
|
||||
|
||||
h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N)*3, 1)))
|
||||
h_msckf_test_sym[-3:,:] = sp.Matrix([track_x - x,track_y - y , track_z - z])
|
||||
|
||||
for n in range(N):
|
||||
idx = dim_main + n*dim_augment
|
||||
err_idx = dim_main_err + n*dim_augment_err
|
||||
x, y, z = state[idx:idx+3]
|
||||
q = state[idx+3:idx+7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
|
||||
track_pos_rot_sym = quat_rot.T * track_pos_sym
|
||||
h_track_sym[n*2:n*2+2,:] = sp.Matrix([focal_scale*(track_pos_rot_sym[1]/track_pos_rot_sym[0]),
|
||||
focal_scale*(track_pos_rot_sym[2]/track_pos_rot_sym[0])])
|
||||
h_msckf_test_sym[n*3:n*3+3,:] = sp.Matrix([track_x - x, track_y - y, track_z - z])
|
||||
obs_eqs.append([h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym])
|
||||
obs_eqs.append([h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym])
|
||||
obs_eqs.append([h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym])
|
||||
msckf_params = [dim_main, dim_augment, dim_main_err, dim_augment_err, N, [ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES]]
|
||||
else:
|
||||
msckf_params = None
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds)
|
|
@ -0,0 +1,176 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
from selfdrive.locationd.kalman.helpers import ObservationKind
|
||||
from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from selfdrive.locationd.kalman.models.loc_kf import parse_pr, parse_prr
|
||||
|
||||
|
||||
class States():
|
||||
ECEF_POS = slice(0,3) # x, y and z in ECEF in meters
|
||||
ECEF_VELOCITY = slice(3,6)
|
||||
CLOCK_BIAS = slice(6, 7) # clock bias in light-meters,
|
||||
CLOCK_DRIFT = slice(7, 8) # clock drift in light-meters/s,
|
||||
CLOCK_ACCELERATION = slice(8, 9) # clock acceleration in light-meters/s**2
|
||||
GLONASS_BIAS = slice(9, 10) # clock drift in light-meters/s,
|
||||
GLONASS_FREQ_SLOPE = slice(10, 11) # GLONASS bias in m expressed as bias + freq_num*freq_slope
|
||||
|
||||
|
||||
class GNSSKalman():
|
||||
name = 'gnss'
|
||||
|
||||
x_initial = np.array([-2712700.6008, -4281600.6679, 3859300.1830,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0])
|
||||
|
||||
# state covariance
|
||||
P_initial = np.diag([10000**2, 10000**2, 10000**2,
|
||||
10**2, 10**2, 10**2,
|
||||
(2000000)**2, (100)**2, (0.5)**2,
|
||||
(10)**2, (1)**2])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([0.3**2, 0.3**2, 0.3**2,
|
||||
3**2, 3**2, 3**2,
|
||||
(.1)**2, (0)**2, (0.01)**2,
|
||||
.1**2, (.01)**2])
|
||||
|
||||
maha_test_kinds = [] #ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS]
|
||||
|
||||
|
||||
@staticmethod
|
||||
def generate_code():
|
||||
dim_state = GNSSKalman.x_initial.shape[0]
|
||||
name = GNSSKalman.name
|
||||
maha_test_kinds = GNSSKalman.maha_test_kinds
|
||||
|
||||
# make functions and jacobians with sympy
|
||||
# state variables
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x,y,z = state[0:3,:]
|
||||
v = state[3:6,:]
|
||||
vx, vy, vz = v
|
||||
cb, cd, ca = state[6:9,:]
|
||||
glonass_bias, glonass_freq_slope = state[9:11,:]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[:3,:] = v
|
||||
state_dot[6,0] = cd
|
||||
state_dot[7,0] = ca
|
||||
|
||||
# Basic descretization, 1st order integrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt*state_dot
|
||||
|
||||
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
|
||||
# extra args
|
||||
sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1)
|
||||
sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1)
|
||||
sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1)
|
||||
orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1)
|
||||
|
||||
# expand extra args
|
||||
sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym
|
||||
sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:]
|
||||
los_x, los_y, los_z = sat_los_sym
|
||||
orb_x, orb_y, orb_z = orb_epos_sym
|
||||
|
||||
h_pseudorange_sym = sp.Matrix([sp.sqrt(
|
||||
(x - sat_x)**2 +
|
||||
(y - sat_y)**2 +
|
||||
(z - sat_z)**2) +
|
||||
cb])
|
||||
|
||||
h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt(
|
||||
(x - sat_x)**2 +
|
||||
(y - sat_y)**2 +
|
||||
(z - sat_z)**2) +
|
||||
cb + glonass_bias + glonass_freq_slope*glonass_freq])
|
||||
|
||||
los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
|
||||
los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2)
|
||||
h_pseudorange_rate_sym = sp.Matrix([los_vector[0]*(sat_vx - vx) +
|
||||
los_vector[1]*(sat_vy - vy) +
|
||||
los_vector[2]*(sat_vz - vz) +
|
||||
cd])
|
||||
|
||||
obs_eqs = [[h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym],
|
||||
[h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym],
|
||||
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym],
|
||||
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym]]
|
||||
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds)
|
||||
|
||||
def __init__(self):
|
||||
self.dim_state = self.x_initial.shape[0]
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, self.dim_state, maha_test_kinds=self.maha_test_kinds)
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
return self.filter.state()
|
||||
|
||||
@property
|
||||
def P(self):
|
||||
return self.filter.covs()
|
||||
|
||||
def predict(self, t):
|
||||
return self.filter.predict(t)
|
||||
|
||||
def rts_smooth(self, estimates):
|
||||
return self.filter.rts_smooth(estimates, norm_quats=False)
|
||||
|
||||
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.PSEUDORANGE_GPS or kind == ObservationKind.PSEUDORANGE_GLONASS:
|
||||
r = self.predict_and_update_pseudorange(data, t, kind)
|
||||
elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS:
|
||||
r = self.predict_and_update_pseudorange_rate(data, t, kind)
|
||||
return r
|
||||
|
||||
def predict_and_update_pseudorange(self, meas, t, kind):
|
||||
R = np.zeros((len(meas), 1, 1))
|
||||
sat_pos_freq = np.zeros((len(meas), 4))
|
||||
z = np.zeros((len(meas), 1))
|
||||
for i, m in enumerate(meas):
|
||||
z_i, R_i, sat_pos_freq_i = parse_pr(m)
|
||||
sat_pos_freq[i,:] = sat_pos_freq_i
|
||||
z[i,:] = z_i
|
||||
R[i,:,:] = R_i
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq)
|
||||
|
||||
def predict_and_update_pseudorange_rate(self, meas, t, kind):
|
||||
R = np.zeros((len(meas), 1, 1))
|
||||
z = np.zeros((len(meas), 1))
|
||||
sat_pos_vel = np.zeros((len(meas), 6))
|
||||
for i, m in enumerate(meas):
|
||||
z_i, R_i, sat_pos_vel_i = parse_prr(m)
|
||||
sat_pos_vel[i] = sat_pos_vel_i
|
||||
R[i,:,:] = R_i
|
||||
z[i, :] = z_i
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
GNSSKalman.generate_code()
|
|
@ -0,0 +1,293 @@
|
|||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
from laika.constants import EARTH_GM
|
||||
from selfdrive.locationd.kalman.helpers import KalmanError, ObservationKind
|
||||
from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import (euler_rotate,
|
||||
quat_matrix_r,
|
||||
quat_rotate)
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
|
||||
class States():
|
||||
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
|
||||
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
|
||||
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
|
||||
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
|
||||
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
|
||||
ODO_SCALE = slice(16, 17) # odometer scale
|
||||
ACCELERATION = slice(17, 20) # Acceleration in device frame in m/s**2
|
||||
IMU_OFFSET = slice(20, 23) # imu offset angles in radians
|
||||
|
||||
ECEF_POS_ERR = slice(0, 3)
|
||||
ECEF_ORIENTATION_ERR = slice(3, 6)
|
||||
ECEF_VELOCITY_ERR = slice(6, 9)
|
||||
ANGULAR_VELOCITY_ERR = slice(9, 12)
|
||||
GYRO_BIAS_ERR = slice(12, 15)
|
||||
ODO_SCALE_ERR = slice(15, 16)
|
||||
ACCELERATION_ERR = slice(16, 19)
|
||||
IMU_OFFSET_ERR = slice(19, 22)
|
||||
|
||||
|
||||
class LiveKalman():
|
||||
name = 'live'
|
||||
|
||||
initial_x = np.array([-2.7e6, 4.2e6, 3.8e6,
|
||||
1, 0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
1,
|
||||
0, 0, 0,
|
||||
0, 0, 0])
|
||||
|
||||
|
||||
# state covariance
|
||||
initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
|
||||
10**2, 10**2, 10**2,
|
||||
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,
|
||||
(0.01)**2, (0.01)**2, (0.01)**2])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.1**2, 0.1**2, 0.1**2,
|
||||
(0.005/100)**2, (0.005/100)**2, (0.005/100)**2,
|
||||
(0.02/100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
(0.05/60)**2, (0.05/60)**2, (0.05/60)**2])
|
||||
|
||||
@staticmethod
|
||||
def generate_code():
|
||||
name = LiveKalman.name
|
||||
dim_state = LiveKalman.initial_x.shape[0]
|
||||
dim_state_err = LiveKalman.initial_P_diag.shape[0]
|
||||
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x,y,z = state[States.ECEF_POS,:]
|
||||
q = state[States.ECEF_ORIENTATION,:]
|
||||
v = state[States.ECEF_VELOCITY,:]
|
||||
vx, vy, vz = v
|
||||
omega = state[States.ANGULAR_VELOCITY,:]
|
||||
vroll, vpitch, vyaw = omega
|
||||
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS,:]
|
||||
odo_scale = state[16,:]
|
||||
acceleration = state[States.ACCELERATION,:]
|
||||
imu_angles= state[States.IMU_OFFSET,:]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
# calibration and attitude rotation matrices
|
||||
quat_rot = quat_rotate(*q)
|
||||
|
||||
# Got the quat predict equations from here
|
||||
# A New Quaternion-Based Kalman Filter for
|
||||
# Real-Time Attitude Estimation Using the Two-Step
|
||||
# Geometrically-Intuitive Correction Algorithm
|
||||
A = 0.5*sp.Matrix([[0, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A * q
|
||||
|
||||
# Time derivative of the state as a function of state
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[States.ECEF_POS,:] = v
|
||||
state_dot[States.ECEF_ORIENTATION,:] = q_dot
|
||||
state_dot[States.ECEF_VELOCITY,0] = quat_rot * acceleration
|
||||
|
||||
# Basic descretization, 1st order intergrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt*state_dot
|
||||
|
||||
state_err_sym = sp.MatrixSymbol('state_err',dim_state_err,1)
|
||||
state_err = sp.Matrix(state_err_sym)
|
||||
quat_err = state_err[States.ECEF_ORIENTATION_ERR,:]
|
||||
v_err = state_err[States.ECEF_VELOCITY_ERR,:]
|
||||
omega_err = state_err[States.ANGULAR_VELOCITY_ERR,:]
|
||||
acceleration_err = state_err[States.ACCELERATION_ERR,:]
|
||||
|
||||
# Time derivative of the state error as a function of state error and state
|
||||
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
|
||||
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
|
||||
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
state_err_dot[States.ECEF_POS_ERR,:] = v_err
|
||||
state_err_dot[States.ECEF_ORIENTATION_ERR,:] = q_err_dot
|
||||
state_err_dot[States.ECEF_VELOCITY_ERR,:] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
|
||||
f_err_sym = state_err + dt*state_err_dot
|
||||
|
||||
# Observation matrix modifier
|
||||
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
|
||||
H_mod_sym[0:3, 0:3] = np.eye(3)
|
||||
H_mod_sym[3:7,3:6] = 0.5*quat_matrix_r(state[3:7])[:,1:]
|
||||
H_mod_sym[7:, 6:] = np.eye(dim_state-7)
|
||||
|
||||
# these error functions are defined so that say there
|
||||
# is a nominal x and true x:
|
||||
# true x = err_function(nominal x, delta x)
|
||||
# delta x = inv_err_function(nominal x, true x)
|
||||
nom_x = sp.MatrixSymbol('nom_x',dim_state,1)
|
||||
true_x = sp.MatrixSymbol('true_x',dim_state,1)
|
||||
delta_x = sp.MatrixSymbol('delta_x',dim_state_err,1)
|
||||
|
||||
err_function_sym = sp.Matrix(np.zeros((dim_state,1)))
|
||||
delta_quat = sp.Matrix(np.ones((4)))
|
||||
delta_quat[1:,:] = sp.Matrix(0.5*delta_x[3:6,:])
|
||||
err_function_sym[0:3,:] = sp.Matrix(nom_x[0:3,:] + delta_x[0:3,:])
|
||||
err_function_sym[3:7,0] = quat_matrix_r(nom_x[3:7,0])*delta_quat
|
||||
err_function_sym[7:,:] = sp.Matrix(nom_x[7:,:] + delta_x[6:,:])
|
||||
|
||||
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err,1)))
|
||||
inv_err_function_sym[0:3,0] = sp.Matrix(-nom_x[0:3,0] + true_x[0:3,0])
|
||||
delta_quat = quat_matrix_r(nom_x[3:7,0]).T*true_x[3:7,0]
|
||||
inv_err_function_sym[3:6,0] = sp.Matrix(2*delta_quat[1:])
|
||||
inv_err_function_sym[6:,0] = sp.Matrix(-nom_x[7:,0] + true_x[7:,0])
|
||||
|
||||
eskf_params = [[err_function_sym, nom_x, delta_x],
|
||||
[inv_err_function_sym, nom_x, true_x],
|
||||
H_mod_sym, f_err_sym, state_err_sym]
|
||||
|
||||
|
||||
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
|
||||
|
||||
imu_rot = euler_rotate(*imu_angles)
|
||||
h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
|
||||
vpitch + pitch_bias,
|
||||
vyaw + yaw_bias])
|
||||
|
||||
pos = sp.Matrix([x, y, z])
|
||||
gravity = quat_rot.T * ((EARTH_GM/((x**2 + y**2 + z**2)**(3.0/2.0)))*pos)
|
||||
h_acc_sym = imu_rot*(gravity + acceleration)
|
||||
h_phone_rot_sym = sp.Matrix([vroll,
|
||||
vpitch,
|
||||
vyaw])
|
||||
speed = vx**2 + vy**2 + vz**2
|
||||
h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])
|
||||
|
||||
h_pos_sym = sp.Matrix([x, y, z])
|
||||
h_imu_frame_sym = sp.Matrix(imu_angles)
|
||||
|
||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||
|
||||
|
||||
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_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
|
||||
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
|
||||
|
||||
def __init__(self):
|
||||
self.dim_state = self.initial_x.shape[0]
|
||||
self.dim_state_err = self.initial_P_diag.shape[0]
|
||||
|
||||
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]),
|
||||
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5*2]),
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
|
||||
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
|
||||
|
||||
@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):
|
||||
return self.filter.predict(t)
|
||||
|
||||
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)))
|
||||
|
||||
# Normalize quats
|
||||
quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
|
||||
|
||||
# Should not continue if the quats behave this weirdly
|
||||
if not (0.1 < quat_norm < 10):
|
||||
cloudlog.error("Kalman filter quaternions unstable")
|
||||
raise KalmanError
|
||||
|
||||
self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
|
||||
|
||||
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__":
|
||||
LiveKalman.generate_code()
|
|
@ -0,0 +1,559 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
import sympy as sp
|
||||
|
||||
from laika.constants import EARTH_GM
|
||||
from laika.raw_gnss import GNSSMeasurement
|
||||
from selfdrive.locationd.kalman.helpers import ObservationKind
|
||||
from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code
|
||||
from selfdrive.locationd.kalman.helpers.lst_sq_computer import LstSqComputer
|
||||
from selfdrive.locationd.kalman.helpers.sympy_helpers import (euler_rotate,
|
||||
quat_matrix_r,
|
||||
quat_rotate)
|
||||
|
||||
|
||||
def parse_prr(m):
|
||||
sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
|
||||
m[GNSSMeasurement.SAT_VEL]))
|
||||
R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
|
||||
z_i = m[GNSSMeasurement.PRR]
|
||||
return z_i, R_i, sat_pos_vel_i
|
||||
|
||||
|
||||
def parse_pr(m):
|
||||
pseudorange = m[GNSSMeasurement.PR]
|
||||
pseudorange_stdev = m[GNSSMeasurement.PR_STD]
|
||||
sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
|
||||
np.array([m[GNSSMeasurement.GLONASS_FREQ]])))
|
||||
z_i = np.atleast_1d(pseudorange)
|
||||
R_i = np.atleast_2d(pseudorange_stdev**2)
|
||||
return z_i, R_i, sat_pos_freq_i
|
||||
|
||||
|
||||
class States():
|
||||
ECEF_POS = slice(0,3) # x, y and z in ECEF in meters
|
||||
ECEF_ORIENTATION = slice(3,7) # quat for pose of phone in ecef
|
||||
ECEF_VELOCITY = slice(7,10) # ecef velocity in m/s
|
||||
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
|
||||
CLOCK_BIAS = slice(13, 14) # clock bias in light-meters,
|
||||
CLOCK_DRIFT = slice(14, 15) # clock drift in light-meters/s,
|
||||
GYRO_BIAS = slice(15, 18) # roll, pitch and yaw biases
|
||||
ODO_SCALE = slice(18, 19) # odometer scale
|
||||
ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2
|
||||
FOCAL_SCALE = slice(22, 23) # focal length scale
|
||||
IMU_OFFSET = slice(23,26) # imu offset angles in radians
|
||||
GLONASS_BIAS = slice(26,27) # GLONASS bias in m expressed as bias + freq_num*freq_slope
|
||||
GLONASS_FREQ_SLOPE = slice(27, 28) # GLONASS bias in m expressed as bias + freq_num*freq_slope
|
||||
CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2,
|
||||
|
||||
|
||||
class LocKalman():
|
||||
name = "loc"
|
||||
x_initial = np.array([-2.7e6, 4.2e6, 3.8e6,
|
||||
1, 0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0,
|
||||
0, 0, 0,
|
||||
1,
|
||||
0, 0, 0,
|
||||
1,
|
||||
0, 0, 0,
|
||||
0, 0,
|
||||
0])
|
||||
|
||||
# state covariance
|
||||
P_initial = np.diag([10000**2, 10000**2, 10000**2,
|
||||
10**2, 10**2, 10**2,
|
||||
10**2, 10**2, 10**2,
|
||||
1**2, 1**2, 1**2,
|
||||
(200000)**2, (100)**2,
|
||||
0.05**2, 0.05**2, 0.05**2,
|
||||
0.02**2,
|
||||
1**2, 1**2, 1**2,
|
||||
0.01**2,
|
||||
(0.01)**2, (0.01)**2, (0.01)**2,
|
||||
10**2, 1**2,
|
||||
0.05**2])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.0**2, 0.0**2, 0.0**2,
|
||||
0.1**2, 0.1**2, 0.1**2,
|
||||
(.1)**2, (0.0)**2,
|
||||
(0.005/100)**2, (0.005/100)**2, (0.005/100)**2,
|
||||
(0.02/100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
0.001**2,
|
||||
(0.05/60)**2, (0.05/60)**2, (0.05/60)**2,
|
||||
(.1)**2, (.01)**2,
|
||||
0.005**2])
|
||||
|
||||
maha_test_kinds = [ObservationKind.ORB_FEATURES] #, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
|
||||
dim_augment = 7
|
||||
dim_augment_err = 6
|
||||
|
||||
@staticmethod
|
||||
def generate_code(N=4):
|
||||
dim_augment = LocKalman.dim_augment
|
||||
dim_augment_err = LocKalman.dim_augment_err
|
||||
|
||||
dim_main = LocKalman.x_initial.shape[0]
|
||||
dim_main_err = LocKalman.P_initial.shape[0]
|
||||
dim_state = dim_main + dim_augment * N
|
||||
dim_state_err = dim_main_err + dim_augment_err * N
|
||||
maha_test_kinds = LocKalman.maha_test_kinds
|
||||
|
||||
name = f"{LocKalman.name}_{N}"
|
||||
|
||||
# make functions and jacobians with sympy
|
||||
# state variables
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x,y,z = state[0:3,:]
|
||||
q = state[3:7,:]
|
||||
v = state[7:10,:]
|
||||
vx, vy, vz = v
|
||||
omega = state[10:13,:]
|
||||
vroll, vpitch, vyaw = omega
|
||||
cb, cd = state[13:15,:]
|
||||
roll_bias, pitch_bias, yaw_bias = state[15:18,:]
|
||||
odo_scale = state[18,:]
|
||||
acceleration = state[19:22,:]
|
||||
focal_scale = state[22,:]
|
||||
imu_angles= state[23:26,:]
|
||||
glonass_bias, glonass_freq_slope = state[26:28,:]
|
||||
ca = state[28,0]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
# calibration and attitude rotation matrices
|
||||
quat_rot = quat_rotate(*q)
|
||||
|
||||
# Got the quat predict equations from here
|
||||
# A New Quaternion-Based Kalman Filter for
|
||||
# Real-Time Attitude Estimation Using the Two-Step
|
||||
# Geometrically-Intuitive Correction Algorithm
|
||||
A = 0.5*sp.Matrix([[0, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A * q
|
||||
|
||||
# Time derivative of the state as a function of state
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[:3,:] = v
|
||||
state_dot[3:7,:] = q_dot
|
||||
state_dot[7:10,0] = quat_rot * acceleration
|
||||
state_dot[13,0] = cd
|
||||
state_dot[14,0] = ca
|
||||
|
||||
# Basic descretization, 1st order intergrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt*state_dot
|
||||
|
||||
state_err_sym = sp.MatrixSymbol('state_err',dim_state_err,1)
|
||||
state_err = sp.Matrix(state_err_sym)
|
||||
quat_err = state_err[3:6,:]
|
||||
v_err = state_err[6:9,:]
|
||||
omega_err = state_err[9:12,:]
|
||||
cd_err = state_err[13,:]
|
||||
acceleration_err = state_err[18:21,:]
|
||||
ca_err = state_err[27,:]
|
||||
|
||||
# Time derivative of the state error as a function of state error and state
|
||||
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
|
||||
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
|
||||
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
state_err_dot[:3,:] = v_err
|
||||
state_err_dot[3:6,:] = q_err_dot
|
||||
state_err_dot[6:9,:] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
|
||||
state_err_dot[12,:] = cd_err
|
||||
state_err_dot[13,:] = ca_err
|
||||
f_err_sym = state_err + dt*state_err_dot
|
||||
|
||||
# convenient indexing
|
||||
# q idxs are for quats and p idxs are for other
|
||||
q_idxs = [[3, dim_augment]] + [[dim_main + n*dim_augment + 3, dim_main + (n+1)*dim_augment] for n in range(N)]
|
||||
q_err_idxs = [[3, dim_augment_err]] + [[dim_main_err + n*dim_augment_err + 3, dim_main_err + (n+1)*dim_augment_err] for n in range(N)]
|
||||
p_idxs = [[0, 3]] + [[dim_augment, dim_main]] + [[dim_main + n*dim_augment , dim_main + n*dim_augment + 3] for n in range(N)]
|
||||
p_err_idxs = [[0, 3]] + [[dim_augment_err, dim_main_err]] + [[dim_main_err + n*dim_augment_err, dim_main_err + n*dim_augment_err + 3] for n in range(N)]
|
||||
|
||||
# Observation matrix modifier
|
||||
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
|
||||
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
|
||||
H_mod_sym[p_idx[0]:p_idx[1],p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1]-p_idx[0])
|
||||
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
|
||||
H_mod_sym[q_idx[0]:q_idx[1],q_err_idx[0]:q_err_idx[1]] = 0.5*quat_matrix_r(state[q_idx[0]:q_idx[1]])[:,1:]
|
||||
|
||||
|
||||
# these error functions are defined so that say there
|
||||
# is a nominal x and true x:
|
||||
# true x = err_function(nominal x, delta x)
|
||||
# delta x = inv_err_function(nominal x, true x)
|
||||
nom_x = sp.MatrixSymbol('nom_x',dim_state,1)
|
||||
true_x = sp.MatrixSymbol('true_x',dim_state,1)
|
||||
delta_x = sp.MatrixSymbol('delta_x',dim_state_err,1)
|
||||
|
||||
err_function_sym = sp.Matrix(np.zeros((dim_state,1)))
|
||||
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
|
||||
delta_quat = sp.Matrix(np.ones((4)))
|
||||
delta_quat[1:,:] = sp.Matrix(0.5*delta_x[q_err_idx[0]: q_err_idx[1],:])
|
||||
err_function_sym[q_idx[0]:q_idx[1],0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1],0])*delta_quat
|
||||
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
|
||||
err_function_sym[p_idx[0]:p_idx[1],:] = sp.Matrix(nom_x[p_idx[0]:p_idx[1],:] + delta_x[p_err_idx[0]:p_err_idx[1],:])
|
||||
|
||||
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err,1)))
|
||||
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
|
||||
inv_err_function_sym[p_err_idx[0]:p_err_idx[1],0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1],0] + true_x[p_idx[0]:p_idx[1],0])
|
||||
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
|
||||
delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1],0]).T*true_x[q_idx[0]:q_idx[1],0]
|
||||
inv_err_function_sym[q_err_idx[0]:q_err_idx[1],0] = sp.Matrix(2*delta_quat[1:])
|
||||
|
||||
eskf_params = [[err_function_sym, nom_x, delta_x],
|
||||
[inv_err_function_sym, nom_x, true_x],
|
||||
H_mod_sym, f_err_sym, state_err_sym]
|
||||
|
||||
|
||||
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
|
||||
# extra args
|
||||
sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1)
|
||||
sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1)
|
||||
sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1)
|
||||
orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1)
|
||||
|
||||
# expand extra args
|
||||
sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym
|
||||
sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:]
|
||||
los_x, los_y, los_z = sat_los_sym
|
||||
orb_x, orb_y, orb_z = orb_epos_sym
|
||||
|
||||
h_pseudorange_sym = sp.Matrix([sp.sqrt(
|
||||
(x - sat_x)**2 +
|
||||
(y - sat_y)**2 +
|
||||
(z - sat_z)**2) +
|
||||
cb])
|
||||
|
||||
h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt(
|
||||
(x - sat_x)**2 +
|
||||
(y - sat_y)**2 +
|
||||
(z - sat_z)**2) +
|
||||
cb + glonass_bias + glonass_freq_slope*glonass_freq])
|
||||
|
||||
los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
|
||||
los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2)
|
||||
h_pseudorange_rate_sym = sp.Matrix([los_vector[0]*(sat_vx - vx) +
|
||||
los_vector[1]*(sat_vy - vy) +
|
||||
los_vector[2]*(sat_vz - vz) +
|
||||
cd])
|
||||
|
||||
imu_rot = euler_rotate(*imu_angles)
|
||||
h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
|
||||
vpitch + pitch_bias,
|
||||
vyaw + yaw_bias])
|
||||
|
||||
pos = sp.Matrix([x, y, z])
|
||||
gravity = quat_rot.T * ((EARTH_GM/((x**2 + y**2 + z**2)**(3.0/2.0)))*pos)
|
||||
h_acc_sym = imu_rot*(gravity + acceleration)
|
||||
h_phone_rot_sym = sp.Matrix([vroll,
|
||||
vpitch,
|
||||
vyaw])
|
||||
speed = vx**2 + vy**2 + vz**2
|
||||
h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])
|
||||
|
||||
# orb stuff
|
||||
orb_pos_sym = sp.Matrix([orb_x - x, orb_y - y, orb_z - z])
|
||||
orb_pos_rot_sym = quat_rot.T * orb_pos_sym
|
||||
s = orb_pos_rot_sym[0]
|
||||
h_orb_point_sym = sp.Matrix([(1/s)*(orb_pos_rot_sym[1]),
|
||||
(1/s)*(orb_pos_rot_sym[2])])
|
||||
|
||||
h_pos_sym = sp.Matrix([x, y, z])
|
||||
h_imu_frame_sym = sp.Matrix(imu_angles)
|
||||
|
||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||
|
||||
|
||||
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_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||
[h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym],
|
||||
[h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym],
|
||||
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym],
|
||||
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym],
|
||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None],
|
||||
[h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym]]
|
||||
|
||||
# MSCKF configuration
|
||||
if N > 0:
|
||||
focal_scale =1
|
||||
# Add observation functions for orb feature tracks
|
||||
track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1)
|
||||
track_x, track_y, track_z = track_epos_sym
|
||||
h_track_sym = sp.Matrix(np.zeros(((1 + N)*2, 1)))
|
||||
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
|
||||
track_pos_rot_sym = quat_rot.T * track_pos_sym
|
||||
h_track_sym[-2:,:] = sp.Matrix([focal_scale*(track_pos_rot_sym[1]/track_pos_rot_sym[0]),
|
||||
focal_scale*(track_pos_rot_sym[2]/track_pos_rot_sym[0])])
|
||||
|
||||
h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N)*3, 1)))
|
||||
h_msckf_test_sym[-3:,:] = sp.Matrix([track_x - x,track_y - y , track_z - z])
|
||||
|
||||
for n in range(N):
|
||||
idx = dim_main + n*dim_augment
|
||||
err_idx = dim_main_err + n*dim_augment_err
|
||||
x, y, z = state[idx:idx+3]
|
||||
q = state[idx+3:idx+7]
|
||||
quat_rot = quat_rotate(*q)
|
||||
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
|
||||
track_pos_rot_sym = quat_rot.T * track_pos_sym
|
||||
h_track_sym[n*2:n*2+2,:] = sp.Matrix([focal_scale*(track_pos_rot_sym[1]/track_pos_rot_sym[0]),
|
||||
focal_scale*(track_pos_rot_sym[2]/track_pos_rot_sym[0])])
|
||||
h_msckf_test_sym[n*3:n*3+3,:] = sp.Matrix([track_x - x, track_y - y, track_z - z])
|
||||
obs_eqs.append([h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym])
|
||||
obs_eqs.append([h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym])
|
||||
obs_eqs.append([h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym])
|
||||
msckf_params = [dim_main, dim_augment, dim_main_err, dim_augment_err, N, [ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES]]
|
||||
else:
|
||||
msckf_params = None
|
||||
gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds)
|
||||
|
||||
def __init__(self, N=4, max_tracks=3000):
|
||||
name = f"{self.name}_{N}"
|
||||
|
||||
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]),
|
||||
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5*2]),
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
|
||||
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
|
||||
|
||||
# MSCKF stuff
|
||||
self.N = N
|
||||
self.dim_main = LocKalman.x_initial.shape[0]
|
||||
self.dim_main_err = LocKalman.P_initial.shape[0]
|
||||
self.dim_state = self.dim_main + self.dim_augment*self.N
|
||||
self.dim_state_err = self.dim_main_err + self.dim_augment_err*self.N
|
||||
|
||||
if self.N > 0:
|
||||
x_initial, P_initial, Q = self.pad_augmented(self.x_initial, self.P_initial, self.Q)
|
||||
self.computer = LstSqComputer(N)
|
||||
self.max_tracks = max_tracks
|
||||
|
||||
# init filter
|
||||
self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err,
|
||||
N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds)
|
||||
|
||||
@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):
|
||||
return self.filter.predict(t)
|
||||
|
||||
def rts_smooth(self, estimates):
|
||||
return self.filter.rts_smooth(estimates, norm_quats=True)
|
||||
|
||||
def pad_augmented(self, x, P, Q=None):
|
||||
if x.shape[0] == self.dim_main and self.N > 0:
|
||||
x = np.pad(x, (0, self.N*self.dim_augment), mode='constant')
|
||||
x[self.dim_main+3::7] = 1
|
||||
if P.shape[0] == self.dim_main_err and self.N > 0:
|
||||
P = np.pad(P, [(0, self.N*self.dim_augment_err), (0, self.N*self.dim_augment_err)], mode='constant')
|
||||
P[self.dim_main_err:, self.dim_main_err:] = 10e20*np.eye(self.dim_augment_err *self.N)
|
||||
if Q is None:
|
||||
return x, P
|
||||
else:
|
||||
Q = np.pad(Q, [(0, self.N*self.dim_augment_err), (0, self.N*self.dim_augment_err)], mode='constant')
|
||||
return x, P, Q
|
||||
|
||||
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()
|
||||
state, P = self.pad_augmented(state, P)
|
||||
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.PSEUDORANGE_GPS or kind == ObservationKind.PSEUDORANGE_GLONASS:
|
||||
r = self.predict_and_update_pseudorange(data, t, kind)
|
||||
elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS:
|
||||
r = self.predict_and_update_pseudorange_rate(data, t, kind)
|
||||
elif kind == ObservationKind.ORB_POINT:
|
||||
r = self.predict_and_update_orb(data, t, kind)
|
||||
elif kind == ObservationKind.ORB_FEATURES:
|
||||
r = self.predict_and_update_orb_features(data, t, kind)
|
||||
elif kind == ObservationKind.MSCKF_TEST:
|
||||
r = self.predict_and_update_msckf_test(data, t, kind)
|
||||
elif kind == ObservationKind.FEATURE_TRACK_TEST:
|
||||
r = self.predict_and_update_feature_track_test(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)))
|
||||
# Normalize quats
|
||||
quat_norm = np.linalg.norm(self.filter.x[3:7,0])
|
||||
# Should not continue if the quats behave this weirdly
|
||||
if not 0.1 < quat_norm < 10:
|
||||
raise RuntimeError("Sir! The filter's gone all wobbly!")
|
||||
self.filter.x[3:7,0] = self.filter.x[3:7,0]/quat_norm
|
||||
for i in range(self.N):
|
||||
d1 = self.dim_main
|
||||
d3 = self.dim_augment
|
||||
self.filter.x[d1+d3*i+3:d1+d3*i+7] /= np.linalg.norm(self.filter.x[d1+i*d3 + 3:d1+i*d3 + 7,0])
|
||||
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_pseudorange(self, meas, t, kind):
|
||||
R = np.zeros((len(meas), 1, 1))
|
||||
sat_pos_freq = np.zeros((len(meas), 4))
|
||||
z = np.zeros((len(meas), 1))
|
||||
for i, m in enumerate(meas):
|
||||
z_i, R_i, sat_pos_freq_i = parse_pr(m)
|
||||
sat_pos_freq[i,:] = sat_pos_freq_i
|
||||
z[i,:] = z_i
|
||||
R[i,:,:] = R_i
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq)
|
||||
|
||||
|
||||
def predict_and_update_pseudorange_rate(self, meas, t, kind):
|
||||
R = np.zeros((len(meas), 1, 1))
|
||||
z = np.zeros((len(meas), 1))
|
||||
sat_pos_vel = np.zeros((len(meas), 6))
|
||||
for i, m in enumerate(meas):
|
||||
z_i, R_i, sat_pos_vel_i = parse_prr(m)
|
||||
sat_pos_vel[i] = sat_pos_vel_i
|
||||
R[i,:,:] = R_i
|
||||
z[i, :] = z_i
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel)
|
||||
|
||||
def predict_and_update_orb(self, orb, t, kind):
|
||||
true_pos = orb[:,2:]
|
||||
z = orb[:,:2]
|
||||
R = np.zeros((len(orb), 2, 2))
|
||||
for i, _ in enumerate(z):
|
||||
R[i,:,:] = np.diag([10**2, 10**2])
|
||||
return self.filter.predict_and_update_batch(t, kind, z, R, true_pos)
|
||||
|
||||
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)
|
||||
|
||||
def predict_and_update_orb_features(self, tracks, t, kind):
|
||||
k = 2*(self.N+1)
|
||||
R = np.zeros((len(tracks), k, k))
|
||||
z = np.zeros((len(tracks), k))
|
||||
ecef_pos = np.zeros((len(tracks), 3))
|
||||
ecef_pos[:] = np.nan
|
||||
poses = self.x[self.dim_main:].reshape((-1,7))
|
||||
times = tracks.reshape((len(tracks),self.N+1, 4))[:,:,0]
|
||||
good_counter = 0
|
||||
if times.any() and np.allclose(times[0,:-1], self.filter.augment_times, rtol=1e-6):
|
||||
for i, track in enumerate(tracks):
|
||||
img_positions = track.reshape((self.N+1, 4))[:,2:]
|
||||
# TODO not perfect as last pose not used
|
||||
#img_positions = unroll_shutter(img_positions, poses, self.filter.state()[7:10], self.filter.state()[10:13], ecef_pos[i])
|
||||
ecef_pos[i] = self.computer.compute_pos(poses, img_positions[:-1])
|
||||
z[i] = img_positions.flatten()
|
||||
R[i,:,:] = np.diag([0.005**2]*(k))
|
||||
if np.isfinite(ecef_pos[i][0]):
|
||||
good_counter += 1
|
||||
if good_counter > self.max_tracks:
|
||||
break
|
||||
good_idxs = np.all(np.isfinite(ecef_pos),axis=1)
|
||||
# have to do some weird stuff here to keep
|
||||
# to have the observations input from mesh3d
|
||||
# consistent with the outputs of the filter
|
||||
# Probably should be replaced, not sure how.
|
||||
ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], augment=True)
|
||||
if ret is None:
|
||||
return
|
||||
y_full = np.zeros((z.shape[0], z.shape[1] - 3))
|
||||
#print sum(good_idxs), len(tracks)
|
||||
if sum(good_idxs) > 0:
|
||||
y_full[good_idxs] = np.array(ret[6])
|
||||
ret = ret[:6] + (y_full, z, ecef_pos)
|
||||
return ret
|
||||
|
||||
def predict_and_update_msckf_test(self, test_data, t, kind):
|
||||
assert self.N > 0
|
||||
z = test_data
|
||||
R = np.zeros((len(test_data), len(z[0]), len(z[0])))
|
||||
ecef_pos = [self.x[:3]]
|
||||
for i, _ in enumerate(z):
|
||||
R[i,:,:] = np.diag([0.1**2]*len(z[0]))
|
||||
ret = self.filter.predict_and_update_batch(t, kind, z, R, ecef_pos)
|
||||
self.filter.augment()
|
||||
return ret
|
||||
|
||||
def maha_test_pseudorange(self, x, P, meas, kind, maha_thresh=.3):
|
||||
bools = []
|
||||
for i, m in enumerate(meas):
|
||||
z, R, sat_pos_freq = parse_pr(m)
|
||||
bools.append(self.filter.maha_test(x, P, kind, z, R, extra_args=sat_pos_freq, maha_thresh=maha_thresh))
|
||||
return np.array(bools)
|
||||
|
||||
def maha_test_pseudorange_rate(self, x, P, meas, kind, maha_thresh=.999):
|
||||
bools = []
|
||||
for i, m in enumerate(meas):
|
||||
z, R, sat_pos_vel = parse_prr(m)
|
||||
bools.append(self.filter.maha_test(x, P, kind, z, R, extra_args=sat_pos_vel, maha_thresh=maha_thresh))
|
||||
return np.array(bools)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
LocKalman.generate_code(N=4)
|
|
@ -1,13 +1,15 @@
|
|||
#include <eigen3/Eigen/QR>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
#include <Eigen/QR>
|
||||
#include <Eigen/Dense>
|
||||
#include <iostream>
|
||||
|
||||
typedef Eigen::Matrix<double, KDIM*2, 3, Eigen::RowMajor> R3M;
|
||||
typedef Eigen::Matrix<double, KDIM*2, 1> R1M;
|
||||
typedef Eigen::Matrix<double, 3, 1> O1M;
|
||||
typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> M3D;
|
||||
|
||||
extern "C" {
|
||||
void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) {
|
||||
|
||||
|
||||
double res[KDIM*2] = {0};
|
||||
double jac[KDIM*6] = {0};
|
||||
|
||||
|
@ -28,7 +30,7 @@ void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) {
|
|||
|
||||
void compute_pos(double *to_c, double *poses, double *img_positions, double *param, double *pos) {
|
||||
param[0] = img_positions[KDIM*2-2];
|
||||
param[1] = img_positions[KDIM*2-1];
|
||||
param[1] = img_positions[KDIM*2-1];
|
||||
param[2] = 0.1;
|
||||
gauss_newton(param, poses, img_positions);
|
||||
|
||||
|
@ -49,3 +51,4 @@ void compute_pos(double *to_c, double *poses, double *img_positions, double *par
|
|||
ecef_output = rot*ecef_output + ecef_offset;
|
||||
memcpy(pos, ecef_output.data(), 3 * sizeof(double));
|
||||
}
|
||||
}
|
|
@ -1,3 +1,4 @@
|
|||
extern "C"{
|
||||
bool sane(double track [K + 1][5]) {
|
||||
double diffs_x [K-1];
|
||||
double diffs_y [K-1];
|
||||
|
@ -14,7 +15,7 @@ bool sane(double track [K + 1][5]) {
|
|||
(diffs_y[i] > 2*diffs_y[i-1] or
|
||||
diffs_y[i] < .5*diffs_y[i-1]))){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
@ -40,9 +41,9 @@ void merge_features(double *tracks, double *features, long long *empty_idxs) {
|
|||
track_arr[match][0][3] = 1;
|
||||
if (sane(track_arr[match])){
|
||||
// label valid
|
||||
track_arr[match][0][4] = 1;
|
||||
track_arr[match][0][4] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// gen new track with this feature
|
||||
track_arr[empty_idxs[empty_idx]][0][0] = 1;
|
||||
|
@ -50,7 +51,8 @@ void merge_features(double *tracks, double *features, long long *empty_idxs) {
|
|||
track_arr[empty_idxs[empty_idx]][0][2] = 1;
|
||||
memcpy(track_arr[empty_idxs[empty_idx]][1], feature_arr[i], 5 * sizeof(double));
|
||||
empty_idx = empty_idx + 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
memcpy(tracks, track_arr, (K+1) * 6000 * 5 * sizeof(double));
|
||||
}
|
||||
}
|
|
@ -3,7 +3,7 @@
|
|||
|
||||
#include <capnp/message.h>
|
||||
#include <capnp/serialize-packed.h>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <Eigen/Dense>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#define DEGREES_TO_RADIANS 0.017453292519943295
|
||||
|
|
Loading…
Reference in New Issue