From 4bfe88a31ad40db9ec4ba2b91c6eaa974546a0df Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Tue, 11 Feb 2020 15:37:40 -0800 Subject: [PATCH] loc_local is dead old-commit-hash: 8d4dda391cfdd163ea1ddb83c0795d31ec133643 --- selfdrive/locationd/kalman/loc_local_kf.py | 128 ------------------ selfdrive/locationd/kalman/loc_local_model.py | 80 ----------- 2 files changed, 208 deletions(-) delete mode 100755 selfdrive/locationd/kalman/loc_local_kf.py delete mode 100644 selfdrive/locationd/kalman/loc_local_model.py diff --git a/selfdrive/locationd/kalman/loc_local_kf.py b/selfdrive/locationd/kalman/loc_local_kf.py deleted file mode 100755 index f39902ad2e..0000000000 --- a/selfdrive/locationd/kalman/loc_local_kf.py +++ /dev/null @@ -1,128 +0,0 @@ -#!/usr/bin/env python3 -import numpy as np -from selfdrive.locationd.kalman import loc_local_model - -from selfdrive.locationd.kalman.kalman_helpers import ObservationKind -from selfdrive.locationd.kalman.ekf_sym import EKF_sym - - - -class States(): - VELOCITY = slice(0,3) # device frame velocity in m/s - ANGULAR_VELOCITY = slice(3, 6) # roll, pitch and yaw rates in device frame in radians/s - GYRO_BIAS = slice(6, 9) # roll, pitch and yaw biases - ODO_SCALE = slice(9, 10) # odometer scale - ACCELERATION = slice(10, 13) # Acceleration in device frame in m/s**2 - - -class LocLocalKalman(): - def __init__(self): - x_initial = np.array([0, 0, 0, - 0, 0, 0, - 0, 0, 0, - 1, - 0, 0, 0]) - - # state covariance - P_initial = np.diag([10**2, 10**2, 10**2, - 1**2, 1**2, 1**2, - 0.05**2, 0.05**2, 0.05**2, - 0.02**2, - 1**2, 1**2, 1**2]) - - # process noise - Q = np.diag([0.0**2, 0.0**2, 0.0**2, - .01**2, .01**2, .01**2, - (0.005/100)**2, (0.005/100)**2, (0.005/100)**2, - (0.02/100)**2, - 3**2, 3**2, 3**2]) - - self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), - ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2])} - - # MSCKF stuff - self.dim_state = len(x_initial) - self.dim_main = self.dim_state - - name = 'loc_local' - loc_local_model.gen_model(name, self.dim_state) - - # init filter - self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_main, self.dim_main) - - @property - def x(self): - return self.filter.state() - - @property - def t(self): - return self.filter.filter_time - - @property - def P(self): - return self.filter.covs() - - def predict(self, t): - if self.t: - # Does NOT modify filter state - return self.filter._predict(self.x, self.P, t - self.t)[0] - else: - raise RuntimeError("Request predict on filter with uninitialized time") - - def rts_smooth(self, estimates): - return self.filter.rts_smooth(estimates, norm_quats=True) - - - def init_state(self, state, covs_diag=None, covs=None, filter_time=None): - if covs_diag is not None: - P = np.diag(covs_diag) - elif covs is not None: - P = covs - else: - P = self.filter.covs() - self.filter.init_state(state, P, filter_time) - - def predict_and_observe(self, t, kind, data): - if len(data) > 0: - data = np.atleast_2d(data) - if kind == ObservationKind.CAMERA_ODO_TRANSLATION: - r = self.predict_and_update_odo_trans(data, t, kind) - elif kind == ObservationKind.CAMERA_ODO_ROTATION: - r = self.predict_and_update_odo_rot(data, t, kind) - elif kind == ObservationKind.ODOMETRIC_SPEED: - r = self.predict_and_update_odo_speed(data, t, kind) - else: - r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) - return r - - def get_R(self, kind, n): - obs_noise = self.obs_noise[kind] - dim = obs_noise.shape[0] - R = np.zeros((n, dim, dim)) - for i in range(n): - R[i,:,:] = obs_noise - return R - - def predict_and_update_odo_speed(self, speed, t, kind): - z = np.array(speed) - R = np.zeros((len(speed), 1, 1)) - for i, _ in enumerate(z): - R[i,:,:] = np.diag([0.2**2]) - return self.filter.predict_and_update_batch(t, kind, z, R) - - def predict_and_update_odo_trans(self, trans, t, kind): - z = trans[:,:3] - R = np.zeros((len(trans), 3, 3)) - for i, _ in enumerate(z): - R[i,:,:] = np.diag(trans[i,3:]**2) - return self.filter.predict_and_update_batch(t, kind, z, R) - - def predict_and_update_odo_rot(self, rot, t, kind): - z = rot[:,:3] - R = np.zeros((len(rot), 3, 3)) - for i, _ in enumerate(z): - R[i,:,:] = np.diag(rot[i,3:]**2) - return self.filter.predict_and_update_batch(t, kind, z, R) - -if __name__ == "__main__": - LocLocalKalman() diff --git a/selfdrive/locationd/kalman/loc_local_model.py b/selfdrive/locationd/kalman/loc_local_model.py deleted file mode 100644 index 0a9f72e5db..0000000000 --- a/selfdrive/locationd/kalman/loc_local_model.py +++ /dev/null @@ -1,80 +0,0 @@ -import numpy as np -import sympy as sp -import os - -from selfdrive.locationd.kalman.kalman_helpers import ObservationKind -from selfdrive.locationd.kalman.ekf_sym import gen_code - - -def gen_model(name, dim_state): - - # check if rebuild is needed - try: - dir_path = os.path.dirname(__file__) - deps = [dir_path + '/' + 'ekf_c.c', - dir_path + '/' + 'ekf_sym.py', - dir_path + '/' + 'loc_local_model.py', - dir_path + '/' + 'loc_local_kf.py'] - - outs = [dir_path + '/' + name + '.o', - dir_path + '/' + name + '.so', - dir_path + '/' + name + '.cpp'] - out_times = list(map(os.path.getmtime, outs)) - dep_times = list(map(os.path.getmtime, deps)) - rebuild = os.getenv("REBUILD", False) - if min(out_times) > max(dep_times) and not rebuild: - return - list(map(os.remove, outs)) - except OSError: - pass - - # make functions and jacobians with sympy - # state variables - state_sym = sp.MatrixSymbol('state', dim_state, 1) - state = sp.Matrix(state_sym) - v = state[0:3,:] - omega = state[3:6,:] - vroll, vpitch, vyaw = omega - vx, vy, vz = v - roll_bias, pitch_bias, yaw_bias = state[6:9,:] - odo_scale = state[9,:] - accel = state[10:13,:] - - dt = sp.Symbol('dt') - - # Time derivative of the state as a function of state - state_dot = sp.Matrix(np.zeros((dim_state, 1))) - state_dot[:3,:] = accel - - # Basic descretization, 1st order intergrator - # Can be pretty bad if dt is big - f_sym = sp.Matrix(state + dt*state_dot) - - # - # Observation functions - # - - # extra args - #imu_rot = euler_rotate(*imu_angles) - #h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias, - # vpitch + pitch_bias, - # vyaw + yaw_bias]) - h_gyro_sym = sp.Matrix([vroll + roll_bias, - vpitch + pitch_bias, - vyaw + yaw_bias]) - - speed = vx**2 + vy**2 + vz**2 - h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale]) - - h_relative_motion = sp.Matrix(v) - h_phone_rot_sym = sp.Matrix([vroll, - vpitch, - vyaw]) - - - obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None], - [h_gyro_sym, ObservationKind.PHONE_GYRO, None], - [h_phone_rot_sym, ObservationKind.NO_ROT, None], - [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], - [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None]] - gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)