Gnss kf whitespace

old-commit-hash: 7015a4704e
commatwo_master
Willem Melching 5 years ago
parent 055c58c1da
commit a42c464f7e
  1. 18
      selfdrive/locationd/kalman/models/gnss_kf.py

@ -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)

Loading…
Cancel
Save