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
Willem Melching
5 years ago
committed by
GitHub
parent
a790892796
commit
47fd50ca60
28 changed files with 1454 additions and 1448 deletions
@ -1,5 +1 @@ |
|||||||
lane.cpp |
generated/ |
||||||
gnss.cpp |
|
||||||
loc*.cpp |
|
||||||
live.cpp |
|
||||||
pos_computer*.cpp |
|
||||||
|
|||||||
@ -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) |
|
||||||
@ -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() |
||||||
@ -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) |
||||||
Loading…
Reference in new issue