locationd: remove models unused in openpilot (#30481)
* Remove filters used exclusively by xx * Update SConstruct * Remove from release * Accomodate rednose build changes * Update rednose ref * rednose/helpers in rpath * Add rednose_filters to files_common * Change rednose_root * Copy rednose site_scons to docker images * Remove rednose from rpath * Bump rednose * Bump rednose * Bump rednosepull/30520/head
Kacper Rączy
1 year ago
committed by
GitHub
parent
7f14bdfb22
commit
f65e6bc30e
10 changed files with 44 additions and 937 deletions
@ -1 +1 @@ |
|||||||
Subproject commit 8658bed29686b2ddae191fd18302986c85542431 |
Subproject commit 44e8a891a2810f274a1fa980775155d9463e87b9 |
||||||
@ -1,10 +1,37 @@ |
|||||||
Import('env', 'common', 'cereal', 'messaging', 'libkf', 'transformations') |
Import('env', 'arch', 'common', 'cereal', 'messaging', 'rednose', 'transformations') |
||||||
|
|
||||||
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread'] |
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread', 'dl'] |
||||||
|
|
||||||
|
# build ekf models |
||||||
|
rednose_gen_dir = 'models/generated' |
||||||
|
rednose_gen_deps = [ |
||||||
|
"models/constants.py", |
||||||
|
] |
||||||
|
live_ekf = env.RednoseCompileFilter( |
||||||
|
target='live', |
||||||
|
filter_gen_script='models/live_kf.py', |
||||||
|
output_dir=rednose_gen_dir, |
||||||
|
extra_gen_artifacts=['live_kf_constants.h'], |
||||||
|
gen_script_deps=rednose_gen_deps, |
||||||
|
) |
||||||
|
car_ekf = env.RednoseCompileFilter( |
||||||
|
target='car', |
||||||
|
filter_gen_script='models/car_kf.py', |
||||||
|
output_dir=rednose_gen_dir, |
||||||
|
extra_gen_artifacts=[], |
||||||
|
gen_script_deps=rednose_gen_deps, |
||||||
|
) |
||||||
|
|
||||||
|
# locationd build |
||||||
|
locationd_sources = ["locationd.cc", "models/live_kf.cc"] |
||||||
|
|
||||||
ekf_sym_cc = env.SharedObject("#rednose/helpers/ekf_sym.cc") |
|
||||||
locationd_sources = ["locationd.cc", "models/live_kf.cc", ekf_sym_cc] |
|
||||||
lenv = env.Clone() |
lenv = env.Clone() |
||||||
lenv["_LIBFLAGS"] += f' {libkf[0].get_labspath()}' |
# ekf filter libraries need to be linked, even if no symbols are used |
||||||
locationd = lenv.Program("locationd", locationd_sources, LIBS=loc_libs + transformations) |
if arch != "Darwin": |
||||||
lenv.Depends(locationd, libkf) |
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"] |
||||||
|
|
||||||
|
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath) |
||||||
|
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath) |
||||||
|
locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations) |
||||||
|
lenv.Depends(locationd, rednose) |
||||||
|
lenv.Depends(locationd, live_ekf) |
||||||
|
|||||||
@ -1,35 +0,0 @@ |
|||||||
import numpy as np |
|
||||||
|
|
||||||
|
|
||||||
# source: GNSSMeasurement (https://github.com/commaai/laika/blob/master/laika/raw_gnss.py) |
|
||||||
class RawGNSSMeasurementIndices: |
|
||||||
PRN = 0 |
|
||||||
RECV_TIME_WEEK = 1 |
|
||||||
RECV_TIME_SEC = 2 |
|
||||||
GLONASS_FREQ = 3 |
|
||||||
|
|
||||||
PR = 4 |
|
||||||
PR_STD = 5 |
|
||||||
PRR = 6 |
|
||||||
PRR_STD = 7 |
|
||||||
|
|
||||||
SAT_POS = slice(8, 11) |
|
||||||
SAT_VEL = slice(11, 14) |
|
||||||
|
|
||||||
|
|
||||||
def parse_prr(m): |
|
||||||
sat_pos_vel_i = np.concatenate((m[RawGNSSMeasurementIndices.SAT_POS], |
|
||||||
m[RawGNSSMeasurementIndices.SAT_VEL])) |
|
||||||
R_i = np.atleast_2d(m[RawGNSSMeasurementIndices.PRR_STD]**2) |
|
||||||
z_i = m[RawGNSSMeasurementIndices.PRR] |
|
||||||
return z_i, R_i, sat_pos_vel_i |
|
||||||
|
|
||||||
|
|
||||||
def parse_pr(m): |
|
||||||
pseudorange = m[RawGNSSMeasurementIndices.PR] |
|
||||||
pseudorange_stdev = m[RawGNSSMeasurementIndices.PR_STD] |
|
||||||
sat_pos_freq_i = np.concatenate((m[RawGNSSMeasurementIndices.SAT_POS], |
|
||||||
np.array([m[RawGNSSMeasurementIndices.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 |
|
||||||
@ -1,190 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import sys |
|
||||||
from typing import List |
|
||||||
|
|
||||||
import numpy as np |
|
||||||
|
|
||||||
from openpilot.selfdrive.locationd.models.constants import ObservationKind |
|
||||||
from openpilot.selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr |
|
||||||
|
|
||||||
if __name__ == '__main__': # Generating sympy |
|
||||||
import sympy as sp |
|
||||||
from rednose.helpers.ekf_sym import gen_code |
|
||||||
else: |
|
||||||
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx |
|
||||||
from rednose.helpers.ekf_sym import EKF_sym |
|
||||||
|
|
||||||
|
|
||||||
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([1e16, 1e16, 1e16, |
|
||||||
10**2, 10**2, 10**2, |
|
||||||
1e14, (100)**2, (0.2)**2, |
|
||||||
(10)**2, (1)**2]) |
|
||||||
|
|
||||||
maha_test_kinds: List[int] = [] # ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS] |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def generate_code(generated_dir): |
|
||||||
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(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds) |
|
||||||
|
|
||||||
def __init__(self, generated_dir, cython=False, erratic_clock=False): |
|
||||||
# process noise |
|
||||||
clock_error_drift = 100.0 if erratic_clock else 0.1 |
|
||||||
self.Q = np.diag([0.03**2, 0.03**2, 0.03**2, |
|
||||||
3**2, 3**2, 3**2, |
|
||||||
(clock_error_drift)**2, (0)**2, (0.005)**2, |
|
||||||
.1**2, (.01)**2]) |
|
||||||
|
|
||||||
self.dim_state = self.x_initial.shape[0] |
|
||||||
|
|
||||||
# init filter |
|
||||||
filter_cls = EKF_sym_pyx if cython else EKF_sym |
|
||||||
self.filter = filter_cls(generated_dir, self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, |
|
||||||
self.dim_state, maha_test_kinds=self.maha_test_kinds) |
|
||||||
self.init_state(GNSSKalman.x_initial, covs=GNSSKalman.P_initial) |
|
||||||
|
|
||||||
@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__": |
|
||||||
generated_dir = sys.argv[2] |
|
||||||
GNSSKalman.generate_code(generated_dir) |
|
||||||
@ -1,105 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
import sys |
|
||||||
import numpy as np |
|
||||||
import sympy as sp |
|
||||||
|
|
||||||
from openpilot.selfdrive.locationd.models.constants import ObservationKind |
|
||||||
from rednose.helpers.ekf_sym import gen_code, EKF_sym |
|
||||||
|
|
||||||
|
|
||||||
class LaneKalman(): |
|
||||||
name = 'lane' |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def generate_code(generated_dir): |
|
||||||
# make functions and jacobians with sympy |
|
||||||
# state variables |
|
||||||
dim = 6 |
|
||||||
state = sp.MatrixSymbol('state', dim, 1) |
|
||||||
|
|
||||||
dd = sp.Symbol('dd') # WARNING: NOT TIME |
|
||||||
|
|
||||||
# Time derivative of the state as a function of state |
|
||||||
state_dot = sp.Matrix(np.zeros((dim, 1))) |
|
||||||
state_dot[:3,0] = sp.Matrix(state[3:6,0]) |
|
||||||
|
|
||||||
# Basic descretization, 1st order intergrator |
|
||||||
# Can be pretty bad if dt is big |
|
||||||
f_sym = sp.Matrix(state) + dd*state_dot |
|
||||||
|
|
||||||
# |
|
||||||
# Observation functions |
|
||||||
# |
|
||||||
h_lane_sym = sp.Matrix(state[:3,0]) |
|
||||||
obs_eqs = [[h_lane_sym, ObservationKind.LANE_PT, None]] |
|
||||||
gen_code(generated_dir, LaneKalman.name, f_sym, dd, state, obs_eqs, dim, dim) |
|
||||||
|
|
||||||
def __init__(self, generated_dir, pt_std=5): |
|
||||||
# state |
|
||||||
# left and right lane centers in ecef |
|
||||||
# WARNING: this is not a temporal model |
|
||||||
# the 'time' in this kalman filter is |
|
||||||
# the distance traveled by the vehicle, |
|
||||||
# which should approximately be the |
|
||||||
# distance along the lane path |
|
||||||
# a more logical parametrization |
|
||||||
# states 0-2 are ecef coordinates distance d |
|
||||||
# states 3-5 is the 3d "velocity" of the |
|
||||||
# lane in ecef (m/m). |
|
||||||
x_initial = np.array([0,0,0, |
|
||||||
0,0,0]) |
|
||||||
|
|
||||||
# state covariance |
|
||||||
P_initial = np.diag([1e16, 1e16, 1e16, |
|
||||||
1**2, 1**2, 1**2]) |
|
||||||
|
|
||||||
# process noise |
|
||||||
Q = np.diag([0.1**2, 0.1**2, 0.1**2, |
|
||||||
0.1**2, 0.1**2, 0.1*2]) |
|
||||||
|
|
||||||
self.dim_state = len(x_initial) |
|
||||||
|
|
||||||
# init filter |
|
||||||
self.filter = EKF_sym(generated_dir, self.name, Q, x_initial, P_initial, x_initial.shape[0], P_initial.shape[0]) |
|
||||||
self.obs_noise = {ObservationKind.LANE_PT: np.diag([pt_std**2]*3)} |
|
||||||
|
|
||||||
@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): |
|
||||||
data = np.atleast_2d(data) |
|
||||||
return self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) |
|
||||||
|
|
||||||
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 |
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__": |
|
||||||
generated_dir = sys.argv[2] |
|
||||||
LaneKalman.generate_code(generated_dir) |
|
||||||
@ -1,565 +0,0 @@ |
|||||||
#!/usr/bin/env python3 |
|
||||||
|
|
||||||
import sys |
|
||||||
|
|
||||||
import numpy as np |
|
||||||
import sympy as sp |
|
||||||
|
|
||||||
from rednose.helpers.ekf_sym import EKF_sym, gen_code |
|
||||||
from rednose.helpers.lst_sq_computer import LstSqComputer |
|
||||||
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate |
|
||||||
|
|
||||||
from openpilot.selfdrive.locationd.models.constants import ObservationKind |
|
||||||
from openpilot.selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr |
|
||||||
|
|
||||||
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) |
|
||||||
|
|
||||||
class States(): |
|
||||||
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters |
|
||||||
ECEF_ORIENTATION = slice(3, 7) # quat for orientation 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_UNUSED = slice(18, 19) # odometer scale |
|
||||||
ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2 |
|
||||||
FOCAL_SCALE_UNUSED = slice(22, 23) # focal length scale |
|
||||||
IMU_FROM_DEVICE_EULER = 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, |
|
||||||
ACCELEROMETER_SCALE_UNUSED = slice(29, 30) # scale of mems accelerometer |
|
||||||
ACCELEROMETER_BIAS = slice(30, 33) # bias of mems accelerometer |
|
||||||
# TODO the offset is likely a translation of the sensor, not a rotation of the camera |
|
||||||
WIDE_FROM_DEVICE_EULER = slice(33, 36) # wide camera offset angles in radians (tici only) |
|
||||||
# We currently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables |
|
||||||
# (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_FROM_DEVICE_EULER). |
|
||||||
# From experiments we see that ACCELEROMETER_BIAS is more correct than ACCELEROMETER_SCALE |
|
||||||
|
|
||||||
# Error-state has different slices because it is an ESKF |
|
||||||
ECEF_POS_ERR = slice(0, 3) |
|
||||||
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error |
|
||||||
ECEF_VELOCITY_ERR = slice(6, 9) |
|
||||||
ANGULAR_VELOCITY_ERR = slice(9, 12) |
|
||||||
CLOCK_BIAS_ERR = slice(12, 13) |
|
||||||
CLOCK_DRIFT_ERR = slice(13, 14) |
|
||||||
GYRO_BIAS_ERR = slice(14, 17) |
|
||||||
ODO_SCALE_ERR_UNUSED = slice(17, 18) |
|
||||||
ACCELERATION_ERR = slice(18, 21) |
|
||||||
FOCAL_SCALE_ERR_UNUSED = slice(21, 22) |
|
||||||
IMU_FROM_DEVICE_EULER_ERR = slice(22, 25) |
|
||||||
GLONASS_BIAS_ERR = slice(25, 26) |
|
||||||
GLONASS_FREQ_SLOPE_ERR = slice(26, 27) |
|
||||||
CLOCK_ACCELERATION_ERR = slice(27, 28) |
|
||||||
ACCELEROMETER_SCALE_ERR_UNUSED = slice(28, 29) |
|
||||||
ACCELEROMETER_BIAS_ERR = slice(29, 32) |
|
||||||
WIDE_FROM_DEVICE_EULER_ERR = slice(32, 35) |
|
||||||
|
|
||||||
|
|
||||||
class LocKalman(): |
|
||||||
name = "loc" |
|
||||||
x_initial = np.array([0, 0, 0, |
|
||||||
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, |
|
||||||
1, |
|
||||||
0, 0, 0, |
|
||||||
0, 0, 0], dtype=np.float64) |
|
||||||
|
|
||||||
# state covariance |
|
||||||
P_initial = np.diag([1e16, 1e16, 1e16, |
|
||||||
10**2, 10**2, 10**2, |
|
||||||
10**2, 10**2, 10**2, |
|
||||||
1**2, 1**2, 1**2, |
|
||||||
1e14, (100)**2, |
|
||||||
0.05**2, 0.05**2, 0.05**2, |
|
||||||
0.02**2, |
|
||||||
2**2, 2**2, 2**2, |
|
||||||
0.01**2, |
|
||||||
0.01**2, 0.01**2, 0.01**2, |
|
||||||
10**2, 1**2, |
|
||||||
0.2**2, |
|
||||||
0.05**2, |
|
||||||
0.05**2, 0.05**2, 0.05**2, |
|
||||||
0.01**2, 0.01**2, 0.01**2]) |
|
||||||
|
|
||||||
|
|
||||||
# measurements that need to pass mahalanobis distance outlier rejector |
|
||||||
maha_test_kinds = [ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE] |
|
||||||
dim_augment = 7 |
|
||||||
dim_augment_err = 6 |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def generate_code(generated_dir, 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[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 |
|
||||||
cb = state[States.CLOCK_BIAS, :] |
|
||||||
cd = state[States.CLOCK_DRIFT, :] |
|
||||||
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] |
|
||||||
acceleration = state[States.ACCELERATION, :] |
|
||||||
imu_from_device_euler = state[States.IMU_FROM_DEVICE_EULER, :] |
|
||||||
imu_from_device_euler[0, 0] = 0 # not observable enough |
|
||||||
imu_from_device_euler[2, 0] = 0 # not observable enough |
|
||||||
glonass_bias = state[States.GLONASS_BIAS, :] |
|
||||||
glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :] |
|
||||||
ca = state[States.CLOCK_ACCELERATION, :] |
|
||||||
accel_bias = state[States.ACCELEROMETER_BIAS, :] |
|
||||||
wide_from_device_euler = state[States.WIDE_FROM_DEVICE_EULER, :] |
|
||||||
wide_from_device_euler[0, 0] = 0 # not observable enough |
|
||||||
|
|
||||||
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 |
|
||||||
state_dot[States.CLOCK_BIAS, :] = cd |
|
||||||
state_dot[States.CLOCK_DRIFT, :] = 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[States.ECEF_ORIENTATION_ERR, :] |
|
||||||
v_err = state_err[States.ECEF_VELOCITY_ERR, :] |
|
||||||
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] |
|
||||||
cd_err = state_err[States.CLOCK_DRIFT_ERR, :] |
|
||||||
acceleration_err = state_err[States.ACCELERATION_ERR, :] |
|
||||||
ca_err = state_err[States.CLOCK_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) |
|
||||||
state_err_dot[States.CLOCK_BIAS_ERR, :] = cd_err |
|
||||||
state_err_dot[States.CLOCK_DRIFT_ERR, :] = 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, strict=True): |
|
||||||
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, strict=True): |
|
||||||
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, strict=True): |
|
||||||
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, strict=True): |
|
||||||
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, strict=True): |
|
||||||
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, strict=True): |
|
||||||
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) |
|
||||||
|
|
||||||
# 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:] |
|
||||||
|
|
||||||
h_pseudorange_sym = sp.Matrix([ |
|
||||||
sp.sqrt( |
|
||||||
(x - sat_x)**2 + |
|
||||||
(y - sat_y)**2 + |
|
||||||
(z - sat_z)**2 |
|
||||||
) + cb[0] |
|
||||||
]) |
|
||||||
|
|
||||||
h_pseudorange_glonass_sym = sp.Matrix([ |
|
||||||
sp.sqrt( |
|
||||||
(x - sat_x)**2 + |
|
||||||
(y - sat_y)**2 + |
|
||||||
(z - sat_z)**2 |
|
||||||
) + cb[0] + glonass_bias[0] + glonass_freq_slope[0] * 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[0]]) |
|
||||||
|
|
||||||
imu_from_device = euler_rotate(*imu_from_device_euler) |
|
||||||
h_gyro_sym = imu_from_device * sp.Matrix([vroll + roll_bias, |
|
||||||
vpitch + pitch_bias, |
|
||||||
vyaw + yaw_bias]) |
|
||||||
|
|
||||||
pos = sp.Matrix([x, y, z]) |
|
||||||
# add 1 for stability, prevent division by 0 |
|
||||||
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2 + 1)**(3.0 / 2.0))) * pos) |
|
||||||
h_acc_sym = imu_from_device * (gravity + acceleration + accel_bias) |
|
||||||
h_acc_stationary_sym = acceleration |
|
||||||
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) |
|
||||||
h_relative_motion = sp.Matrix(quat_rot.T * v) |
|
||||||
|
|
||||||
obs_eqs = [[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_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], |
|
||||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], |
|
||||||
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]] |
|
||||||
|
|
||||||
wide_from_device = euler_rotate(*wide_from_device_euler) |
|
||||||
# MSCKF configuration |
|
||||||
if N > 0: |
|
||||||
# experimentally found this is correct value for imx298 with 910 focal length |
|
||||||
# this is a variable so it can change with focus, but we disregard that for now |
|
||||||
# TODO: this isn't correct for tici |
|
||||||
focal_scale = 1.01 |
|
||||||
# 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))) |
|
||||||
h_track_wide_cam_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 |
|
||||||
track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_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_track_wide_cam_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]), |
|
||||||
focal_scale * (track_pos_rot_wide_cam_sym[2] / track_pos_rot_wide_cam_sym[0])]) |
|
||||||
|
|
||||||
h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N) * 3, 1))) |
|
||||||
h_msckf_test_sym[-3:, :] = track_pos_sym |
|
||||||
|
|
||||||
for n in range(N): |
|
||||||
idx = dim_main + n * dim_augment |
|
||||||
# err_idx = dim_main_err + n * dim_augment_err # FIXME: Why is this not used? |
|
||||||
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 |
|
||||||
track_pos_rot_wide_cam_sym = wide_from_device * track_pos_rot_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_track_wide_cam_sym[n * 2: n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]), |
|
||||||
focal_scale * (track_pos_rot_wide_cam_sym[2] / track_pos_rot_wide_cam_sym[0])]) |
|
||||||
h_msckf_test_sym[n * 3:n * 3 + 3, :] = track_pos_sym |
|
||||||
|
|
||||||
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_wide_cam_sym, ObservationKind.ORB_FEATURES_WIDE, 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, ObservationKind.ORB_FEATURES_WIDE]] |
|
||||||
else: |
|
||||||
msckf_params = None |
|
||||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds) |
|
||||||
|
|
||||||
def __init__(self, generated_dir, N=4, erratic_clock=False): |
|
||||||
name = f"{self.name}_{N}" |
|
||||||
|
|
||||||
|
|
||||||
# process noise |
|
||||||
q_clock_error = 100.0 if erratic_clock else 0.1 |
|
||||||
q_clock_error_rate = 10 if erratic_clock else 0.0 |
|
||||||
self.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, |
|
||||||
(q_clock_error)**2, (q_clock_error_rate)**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, |
|
||||||
(0.02 / 100)**2, |
|
||||||
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, |
|
||||||
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**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.0025**2, 0.0025**2, 0.0025**2]), |
|
||||||
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]), |
|
||||||
ObservationKind.NO_ACCEL: np.diag([0.0025**2, 0.0025**2, 0.0025**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) # lgtm[py/mismatched-multiple-assignment] |
|
||||||
self.computer = LstSqComputer(generated_dir, N) |
|
||||||
|
|
||||||
self.quaternion_idxs = [3, ] + [(self.dim_main + i * self.dim_augment + 3)for i in range(self.N)] |
|
||||||
|
|
||||||
# init filter |
|
||||||
self.filter = EKF_sym(generated_dir, 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, self.quaternion_idxs) |
|
||||||
|
|
||||||
@property |
|
||||||
def x(self): |
|
||||||
return self.filter.state() |
|
||||||
|
|
||||||
@property |
|
||||||
def t(self): |
|
||||||
return self.filter.get_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_FEATURES or kind == ObservationKind.ORB_FEATURES_WIDE: |
|
||||||
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) |
|
||||||
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.state()[3:7]) |
|
||||||
# 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!") |
|
||||||
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_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] |
|
||||||
if kind==ObservationKind.ORB_FEATURES: |
|
||||||
pt_std = 0.005 |
|
||||||
else: |
|
||||||
pt_std = 0.02 |
|
||||||
if times.any(): |
|
||||||
assert np.allclose(times[0, :-1], self.filter.get_augment_times(), atol=1e-7, rtol=0.0) |
|
||||||
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([pt_std**2] * (k)) |
|
||||||
|
|
||||||
good_idxs = np.all(np.isfinite(ecef_pos), axis=1) |
|
||||||
|
|
||||||
# This code relies on wide and narrow orb features being captured at the same time, |
|
||||||
# and wide features to be processed first. |
|
||||||
ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], |
|
||||||
augment=kind==ObservationKind.ORB_FEATURES) |
|
||||||
if ret is None: |
|
||||||
return |
|
||||||
|
|
||||||
# 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. |
|
||||||
y_full = np.zeros((z.shape[0], z.shape[1] - 3)) |
|
||||||
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 m in 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 m in 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__": |
|
||||||
N = int(sys.argv[1].split("_")[-1]) |
|
||||||
generated_dir = sys.argv[2] |
|
||||||
LocKalman.generate_code(generated_dir, N=N) |
|
||||||
Loading…
Reference in new issue