|
|
|
@ -40,7 +40,6 @@ class GNSSKalman(): |
|
|
|
|
|
|
|
|
|
maha_test_kinds = [] # ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS] |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@staticmethod |
|
|
|
|
def generate_code(): |
|
|
|
|
dim_state = GNSSKalman.x_initial.shape[0] |
|
|
|
@ -68,7 +67,6 @@ class GNSSKalman(): |
|
|
|
|
# Can be pretty bad if dt is big |
|
|
|
|
f_sym = state + dt * state_dot |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# |
|
|
|
|
# Observation functions |
|
|
|
|
# |
|
|
|
@ -85,17 +83,21 @@ class GNSSKalman(): |
|
|
|
|
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( |
|
|
|
|
h_pseudorange_sym = sp.Matrix([ |
|
|
|
|
sp.sqrt( |
|
|
|
|
(x - sat_x)**2 + |
|
|
|
|
(y - sat_y)**2 + |
|
|
|
|
(z - sat_z)**2) + |
|
|
|
|
cb]) |
|
|
|
|
(z - sat_z)**2 |
|
|
|
|
) + cb |
|
|
|
|
]) |
|
|
|
|
|
|
|
|
|
h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt( |
|
|
|
|
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]) |
|
|
|
|
(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) |
|
|
|
|