diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 71fc00091b..2f1e575073 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -81,6 +81,8 @@ COPY ./pyextra /tmp/openpilot/pyextra COPY ./panda /tmp/openpilot/panda COPY ./external /tmp/openpilot/external COPY ./tools /tmp/openpilot/tools +COPY ./laika /tmp/openpilot/laika +COPY ./laika_repo /tmp/openpilot/laika_repo COPY SConstruct /tmp/openpilot/SConstruct diff --git a/SConstruct b/SConstruct index 73c603ad55..5944541b5b 100644 --- a/SConstruct +++ b/SConstruct @@ -228,5 +228,6 @@ if arch == "aarch64": SConscript(['selfdrive/clocksd/SConscript']) SConscript(['selfdrive/locationd/SConscript']) +SConscript(['selfdrive/locationd/kalman/SConscript']) # TODO: finish cereal, dbcbuilder, MPC diff --git a/release/files_common b/release/files_common index 85036bdb1c..67b67d348f 100644 --- a/release/files_common +++ b/release/files_common @@ -28,7 +28,6 @@ common/profiler.py common/testing.py common/basedir.py common/filter_simple.py -common/sympy_helpers.py common/stat_live.py common/spinner.py common/cython_hacks.py diff --git a/selfdrive/locationd/kalman/.gitignore b/selfdrive/locationd/kalman/.gitignore index 06e03ea484..9ab870da89 100644 --- a/selfdrive/locationd/kalman/.gitignore +++ b/selfdrive/locationd/kalman/.gitignore @@ -1,5 +1 @@ -lane.cpp -gnss.cpp -loc*.cpp -live.cpp -pos_computer*.cpp +generated/ diff --git a/selfdrive/locationd/kalman/SConscript b/selfdrive/locationd/kalman/SConscript new file mode 100644 index 0000000000..1fca255c3c --- /dev/null +++ b/selfdrive/locationd/kalman/SConscript @@ -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]) diff --git a/selfdrive/locationd/kalman/feature_handler.py b/selfdrive/locationd/kalman/feature_handler.py deleted file mode 100644 index 7489aefd79..0000000000 --- a/selfdrive/locationd/kalman/feature_handler.py +++ /dev/null @@ -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 - diff --git a/selfdrive/locationd/kalman/gnss_kf.py b/selfdrive/locationd/kalman/gnss_kf.py deleted file mode 100755 index 4701e80a8d..0000000000 --- a/selfdrive/locationd/kalman/gnss_kf.py +++ /dev/null @@ -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() diff --git a/selfdrive/locationd/kalman/gnss_model.py b/selfdrive/locationd/kalman/gnss_model.py deleted file mode 100644 index 4a5f6fd2f6..0000000000 --- a/selfdrive/locationd/kalman/gnss_model.py +++ /dev/null @@ -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) diff --git a/selfdrive/locationd/kalman/kalman_helpers.py b/selfdrive/locationd/kalman/helpers/__init__.py similarity index 88% rename from selfdrive/locationd/kalman/kalman_helpers.py rename to selfdrive/locationd/kalman/helpers/__init__.py index c4dd4fdd86..b2e3cbb430 100644 --- a/selfdrive/locationd/kalman/kalman_helpers.py +++ b/selfdrive/locationd/kalman/helpers/__init__.py @@ -2,6 +2,28 @@ import numpy as np import os from bisect import bisect from tqdm import tqdm +from cffi import FFI + +TEMPLATE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'templates')) +GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'generated')) + + +def write_code(name, code, header): + if not os.path.exists(GENERATED_DIR): + os.mkdir(GENERATED_DIR) + + open(os.path.join(GENERATED_DIR, f"{name}.cpp"), 'w').write(code) + open(os.path.join(GENERATED_DIR, f"{name}.h"), 'w').write(header) + + +def load_code(name): + shared_fn = os.path.join(GENERATED_DIR, f"lib{name}.so") + header_fn = os.path.join(GENERATED_DIR, f"{name}.h") + header = open(header_fn).read() + + ffi = FFI() + ffi.cdef(header) + return (ffi, ffi.dlopen(shared_fn)) class KalmanError(Exception): diff --git a/selfdrive/locationd/kalman/chi2_lookup.py b/selfdrive/locationd/kalman/helpers/chi2_lookup.py similarity index 100% rename from selfdrive/locationd/kalman/chi2_lookup.py rename to selfdrive/locationd/kalman/helpers/chi2_lookup.py diff --git a/selfdrive/locationd/kalman/chi2_lookup_table.npy b/selfdrive/locationd/kalman/helpers/chi2_lookup_table.npy similarity index 100% rename from selfdrive/locationd/kalman/chi2_lookup_table.npy rename to selfdrive/locationd/kalman/helpers/chi2_lookup_table.npy diff --git a/selfdrive/locationd/kalman/ekf_sym.py b/selfdrive/locationd/kalman/helpers/ekf_sym.py similarity index 97% rename from selfdrive/locationd/kalman/ekf_sym.py rename to selfdrive/locationd/kalman/helpers/ekf_sym.py index 36d09cb86a..f328f0935d 100644 --- a/selfdrive/locationd/kalman/ekf_sym.py +++ b/selfdrive/locationd/kalman/helpers/ekf_sym.py @@ -1,14 +1,16 @@ import os from bisect import bisect_right -import sympy as sp + import numpy as np +import sympy as sp from numpy import dot -from common.ffi_wrapper import compile_code, wrap_compiled -from common.sympy_helpers import sympy_into_c -from .chi2_lookup import chi2_ppf +from selfdrive.locationd.kalman.helpers.sympy_helpers import sympy_into_c +from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code, + write_code) + +from selfdrive.locationd.kalman.helpers.chi2_lookup import chi2_ppf -EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__)) def solve(a, b): if a.shape[0] == 1 and a.shape[1] == 1: @@ -17,6 +19,7 @@ def solve(a, b): else: return np.linalg.solve(a, b) + def null(H, eps=1e-12): u, s, vh = np.linalg.svd(H) padding = max(0,np.shape(H)[1]-np.shape(s)[0]) @@ -24,6 +27,7 @@ def null(H, eps=1e-12): null_space = np.compress(null_mask, vh, axis=0) return np.transpose(null_space) + def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]): # optional state transition matrix, H modifier # and err_function if an error-state kalman filter (ESKF) @@ -129,11 +133,13 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh) extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind - code += "\n" + extra_header - code += "\n" + open(os.path.join(EXTERNAL_PATH, "ekf_c.c")).read() - code += "\n" + extra_post + code += '\nextern "C"{\n' + extra_header + "\n}\n" + code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read() + code += '\nextern "C"{\n' + extra_post + "\n}\n" header += "\n" + extra_header - compile_code(name, code, header, EXTERNAL_PATH) + + write_code(name, code, header) + class EKF_sym(): def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err, @@ -174,7 +180,7 @@ class EKF_sym(): self.rewind_obscache = [] self.init_state(x_initial, P_initial, None) - ffi, lib = wrap_compiled(name, EXTERNAL_PATH) + ffi, lib = load_code(name) kinds, self.feature_track_kinds = [], [] for func in dir(lib): if func[:2] == 'h_': @@ -517,9 +523,6 @@ class EKF_sym(): else: return True - - - def rts_smooth(self, estimates, norm_quats=False): ''' Returns rts smoothed results of diff --git a/selfdrive/locationd/kalman/helpers/feature_handler.py b/selfdrive/locationd/kalman/helpers/feature_handler.py new file mode 100755 index 0000000000..3596f021f1 --- /dev/null +++ b/selfdrive/locationd/kalman/helpers/feature_handler.py @@ -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 \n" + c_code += "#include \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() diff --git a/selfdrive/locationd/kalman/helpers/lst_sq_computer.py b/selfdrive/locationd/kalman/helpers/lst_sq_computer.py new file mode 100755 index 0000000000..91d51227c2 --- /dev/null +++ b/selfdrive/locationd/kalman/helpers/lst_sq_computer.py @@ -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() diff --git a/common/sympy_helpers.py b/selfdrive/locationd/kalman/helpers/sympy_helpers.py similarity index 97% rename from common/sympy_helpers.py rename to selfdrive/locationd/kalman/helpers/sympy_helpers.py index 78688a84cf..c5bfc8a5aa 100644 --- a/common/sympy_helpers.py +++ b/selfdrive/locationd/kalman/helpers/sympy_helpers.py @@ -78,4 +78,6 @@ def sympy_into_c(sympy_functions): c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#') c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#') + c_code = 'extern "C" {\n#include \n' + c_code + "\n}\n" + return c_header, c_code diff --git a/selfdrive/locationd/kalman/live_kf.py b/selfdrive/locationd/kalman/live_kf.py deleted file mode 100755 index 7cd1cbc96d..0000000000 --- a/selfdrive/locationd/kalman/live_kf.py +++ /dev/null @@ -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() diff --git a/selfdrive/locationd/kalman/live_model.py b/selfdrive/locationd/kalman/live_model.py deleted file mode 100644 index 94a01fc251..0000000000 --- a/selfdrive/locationd/kalman/live_model.py +++ /dev/null @@ -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) diff --git a/selfdrive/locationd/kalman/loc_kf.py b/selfdrive/locationd/kalman/loc_kf.py deleted file mode 100755 index ce7b4e759c..0000000000 --- a/selfdrive/locationd/kalman/loc_kf.py +++ /dev/null @@ -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) diff --git a/selfdrive/locationd/kalman/loc_model.py b/selfdrive/locationd/kalman/loc_model.py deleted file mode 100644 index 6255c2aaa8..0000000000 --- a/selfdrive/locationd/kalman/loc_model.py +++ /dev/null @@ -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) diff --git a/selfdrive/locationd/kalman/models/__init__.py b/selfdrive/locationd/kalman/models/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/locationd/kalman/models/gnss_kf.py b/selfdrive/locationd/kalman/models/gnss_kf.py new file mode 100755 index 0000000000..7caee1cb92 --- /dev/null +++ b/selfdrive/locationd/kalman/models/gnss_kf.py @@ -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() diff --git a/selfdrive/locationd/kalman/models/live_kf.py b/selfdrive/locationd/kalman/models/live_kf.py new file mode 100755 index 0000000000..c4f86987e3 --- /dev/null +++ b/selfdrive/locationd/kalman/models/live_kf.py @@ -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() diff --git a/selfdrive/locationd/kalman/models/loc_kf.py b/selfdrive/locationd/kalman/models/loc_kf.py new file mode 100755 index 0000000000..730d91ea42 --- /dev/null +++ b/selfdrive/locationd/kalman/models/loc_kf.py @@ -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) diff --git a/selfdrive/locationd/kalman/compute_pos.c b/selfdrive/locationd/kalman/templates/compute_pos.c similarity index 91% rename from selfdrive/locationd/kalman/compute_pos.c rename to selfdrive/locationd/kalman/templates/compute_pos.c index f77783da74..9499fa7b6b 100644 --- a/selfdrive/locationd/kalman/compute_pos.c +++ b/selfdrive/locationd/kalman/templates/compute_pos.c @@ -1,13 +1,15 @@ -#include -#include -#include +#include +#include +#include + typedef Eigen::Matrix R3M; typedef Eigen::Matrix R1M; typedef Eigen::Matrix O1M; typedef Eigen::Matrix M3D; +extern "C" { void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) { - + double res[KDIM*2] = {0}; double jac[KDIM*6] = {0}; @@ -28,7 +30,7 @@ void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) { void compute_pos(double *to_c, double *poses, double *img_positions, double *param, double *pos) { param[0] = img_positions[KDIM*2-2]; - param[1] = img_positions[KDIM*2-1]; + param[1] = img_positions[KDIM*2-1]; param[2] = 0.1; gauss_newton(param, poses, img_positions); @@ -49,3 +51,4 @@ void compute_pos(double *to_c, double *poses, double *img_positions, double *par ecef_output = rot*ecef_output + ecef_offset; memcpy(pos, ecef_output.data(), 3 * sizeof(double)); } +} diff --git a/selfdrive/locationd/kalman/ekf_c.c b/selfdrive/locationd/kalman/templates/ekf_c.c similarity index 100% rename from selfdrive/locationd/kalman/ekf_c.c rename to selfdrive/locationd/kalman/templates/ekf_c.c diff --git a/selfdrive/locationd/kalman/feature_handler.c b/selfdrive/locationd/kalman/templates/feature_handler.c similarity index 96% rename from selfdrive/locationd/kalman/feature_handler.c rename to selfdrive/locationd/kalman/templates/feature_handler.c index eeef6f8a25..330972339c 100644 --- a/selfdrive/locationd/kalman/feature_handler.c +++ b/selfdrive/locationd/kalman/templates/feature_handler.c @@ -1,3 +1,4 @@ +extern "C"{ bool sane(double track [K + 1][5]) { double diffs_x [K-1]; double diffs_y [K-1]; @@ -14,7 +15,7 @@ bool sane(double track [K + 1][5]) { (diffs_y[i] > 2*diffs_y[i-1] or diffs_y[i] < .5*diffs_y[i-1]))){ return false; - } + } } return true; } @@ -40,9 +41,9 @@ void merge_features(double *tracks, double *features, long long *empty_idxs) { track_arr[match][0][3] = 1; if (sane(track_arr[match])){ // label valid - track_arr[match][0][4] = 1; + track_arr[match][0][4] = 1; } - } + } } else { // gen new track with this feature track_arr[empty_idxs[empty_idx]][0][0] = 1; @@ -50,7 +51,8 @@ void merge_features(double *tracks, double *features, long long *empty_idxs) { track_arr[empty_idxs[empty_idx]][0][2] = 1; memcpy(track_arr[empty_idxs[empty_idx]][1], feature_arr[i], 5 * sizeof(double)); empty_idx = empty_idx + 1; - } + } } memcpy(tracks, track_arr, (K+1) * 6000 * 5 * sizeof(double)); } +} diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc index 4b93b68014..8b25a2a7cf 100644 --- a/selfdrive/locationd/locationd_yawrate.cc +++ b/selfdrive/locationd/locationd_yawrate.cc @@ -3,7 +3,7 @@ #include #include -#include +#include #include "cereal/gen/cpp/log.capnp.h" diff --git a/selfdrive/locationd/locationd_yawrate.h b/selfdrive/locationd/locationd_yawrate.h index c59734aa68..3b53ef561d 100644 --- a/selfdrive/locationd/locationd_yawrate.h +++ b/selfdrive/locationd/locationd_yawrate.h @@ -1,6 +1,6 @@ #pragma once -#include +#include #include "cereal/gen/cpp/log.capnp.h" #define DEGREES_TO_RADIANS 0.017453292519943295