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)