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)
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)
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)
def __init__(self, steer_ratio=15, stiffness_factor=1, angle_offset=0):
self.dim_state = self.x_initial.shape[0]
x_init = self.x_initial
x_init[States.STEER_RATIO] = steer_ratio
x_init[States.STIFFNESS] = stiffness_factor
x_init[States.ANGLE_OFFSET] = angle_offset
# 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,
global_vars=self.global_vars)
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
])
# measurements that need to pass mahalanobis distance outlier rejector
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[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, :][0, 0]
#cd = state[States.CLOCK_DRIFT, :][0, 0]
cb, cd = state[13:15, :]
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
#odo_scale = state[States.ODO_SCALE, :][0,0]
odo_scale = state[18, :]
acceleration = state[States.ACCELERATION, :]
#focal_scale = state[States.FOCAL_SCALE, :][0,0]
focal_scale = state[22, :]
imu_angles = state[States.IMU_OFFSET, :]
glonass_bias, glonass_freq_slope = state[26:28, :]
ca = state[28, 0]
#glonass_bias = state[States.GLONASS_BIAS, :][0,0]
#glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :][0,0]
#ca = state[States.CLOCK_ACCELERATION, :][0,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[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
state_dot[13, 0] = cd
state_dot[14, 0] = ca
#state_dot[States.CLOCK_BIAS, 0][0,0] = cd
state_dot[States.CLOCK_DRIFT, 0][0, 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[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, :][0,:]
cd_err = state_err[13, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
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[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, :][0,:] = cd_err
#state_err_dot[States.CLOCK_DRIFT_ERR, :][0,:] = ca_err
state_err_dot[12, :][0, :] = cd_err
state_err_dot[13, :][0, :] = 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 = sp.sqrt(vx**2 + vy**2 + vz**2)
h_speed_sym = sp.Matrix([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 # 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
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))
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)
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[States.ODO_SCALE, :][0,:]
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[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
# 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[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 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 = sp.sqrt(vx**2 + vy**2 + vz**2)
h_speed_sym = sp.Matrix([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)
class CarKalman():
name = 'car'
x_initial = np.array([
1.0,
15.0,
0.0,
0.0,
10.0,
0.0,
0.0,
0.0,
])
# process noise
Q = np.diag([
(.05 / 100)**2,
.0001**2,
math.radians(0.0001)**2,
math.radians(0.1)**2,
.1**2,
.01**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
])
P_initial = Q.copy()
obs_noise = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(5.0**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
maha_test_kinds = [
] # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED]
global_vars = [
sp.Symbol('mass'),
sp.Symbol('rotational_inertia'),
sp.Symbol('center_to_front'),
sp.Symbol('center_to_rear'),
sp.Symbol('stiffness_front'),
sp.Symbol('stiffness_rear'),
]
@staticmethod
def generate_code():
dim_state = CarKalman.x_initial.shape[0]
name = CarKalman.name
maha_test_kinds = CarKalman.maha_test_kinds
# globals
m, j, aF, aR, cF_orig, cR_orig = CarKalman.global_vars
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
x = state[States.STIFFNESS, :][0, 0]
cF, cR = x * cF_orig, x * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast)
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[
sp.Matrix([angle_offset_fast]),
ObservationKind.ANGLE_OFFSET_FAST, None
],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([x]), ObservationKind.STIFFNESS, None],
]
gen_code(name,
f_sym,
dt,
state_sym,
obs_eqs,
dim_state,
dim_state,
maha_test_kinds=maha_test_kinds,
global_vars=CarKalman.global_vars)
def __init__(self, steer_ratio=15, stiffness_factor=1, angle_offset=0):
self.dim_state = self.x_initial.shape[0]
x_init = self.x_initial
x_init[States.STEER_RATIO] = steer_ratio
x_init[States.STIFFNESS] = stiffness_factor
x_init[States.ANGLE_OFFSET] = angle_offset
# 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,
global_vars=self.global_vars)
@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 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 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, R=None):
if len(data) > 0:
data = np.atleast_2d(data)
if R is None:
R = self.get_R(kind, len(data))
self.filter.predict_and_update_batch(t, kind, data, R)
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)
class CarKalman():
name = 'car'
x_initial = np.array([
1.0,
15.0,
0.0,
0.0,
10.0,
0.0,
0.0,
0.0,
])
# state covariance
P_initial = np.diag([
.1**2,
.1**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
10**2,
10**2,
1**2,
1**2,
])
# process noise
Q = np.diag([
(.05 / 10)**2,
.0001**2,
math.radians(0.01)**2,
math.radians(0.2)**2,
.1**2,
.1**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
])
obs_noise = {
ObservationKind.ROAD_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]),
ObservationKind.ROAD_FRAME_YAW_RATE:
np.atleast_2d(math.radians(0.1)**2),
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(50.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(50.0**2),
}
maha_test_kinds = [
] # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED]
@staticmethod
def generate_code():
dim_state = CarKalman.x_initial.shape[0]
name = CarKalman.name
maha_test_kinds = CarKalman.maha_test_kinds
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
# TODO: Read from car params at runtime
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.toyota.interface import CarInterface
from selfdrive.car.toyota.values import CAR
CP = CarInterface.get_params(CAR.COROLLA_TSS2)
VM = VehicleModel(CP)
m = VM.m
j = VM.j
aF = VM.aF
aR = VM.aR
x = state[States.STIFFNESS, :][0, 0]
cF, cR = x * VM.cF, x * VM.cR
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast)
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[
sp.Matrix([angle_offset_fast]),
ObservationKind.ANGLE_OFFSET_FAST, None
],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([x]), ObservationKind.STIFFNESS, None],
]
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 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 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)
self.filter.predict_and_update_batch(t, kind, data,
self.get_R(kind, len(data)))
