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)))
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,
.01**2,
math.radians(0.002)**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 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 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 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)