                           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,
 
</s> remove 
class LiveKalman():
  def __init__(self, N=0, max_tracks=3000):
    x_initial = 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
    P_initial = np.diag([10000**2, 10000**2, 10000**2,
 </s> add 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, </s> remove       initial_state[:3] = initial_ecef
      initial_state[3:7] = initial_pose_ecef_quat
      initial_state[7:10] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))
      initial_state[18] = 1
      initial_state[22] = 1
      covs_diag = np.array([10**2,10**2,10**2,
                     quat_uncertainty, quat_uncertainty, quat_uncertainty,
                     2**2, 2**2, 2**2,
                     1, 1, 1,
                     20000000**2, 100**2,
                     0.01**2, 0.01**2, 0.01**2,
                     0.02**2,
                     2**2, 2**2, 2**2,
                     .01**2,
                     0.01**2, 0.01**2, 0.01**2,
                     10**2, 1**2,
                     0.2**2])
      self.kf.init_state(initial_state, covs=np.diag(covs_diag), filter_time=current_time)
 </s> add       initial_state = inital_x
      initial_covs_diag = initial_P_diag

      initial_state[States.ECEF_POS] = initial_ecef
      initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat
      initial_state[States.ECEF_VELOCITY] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))

      initial_covs_diag[States.ECEF_POS_ERR] = 10**2
      initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty
      initial_covs_diag[States.ECEF_VELOCITY_ERR] = 1**2
      self.kf.init_state(initial_state, covs=np.diag(initial_covs_diag), filter_time=current_time) </s> remove       initial_state = np.zeros(29)
 </s> add  </s> remove     # initing with bad bearing allowed, maybe bad?
    if not self.filter_ready and len(list(self.dog.orbits.keys())) >6:  # and log.gpsLocationExternal.speed > 5:
 </s> add     # TODO initing with bad bearing not allowed, maybe not bad?
    if not self.filter_ready and log.gpsLocationExternal.speed > 5: </s> remove     if dog:
      self.dog = dog
    else:
      self.dog = AstroDog(auto_update=True)
 </s> add 
class LiveKalman():
  def __init__(self, N=0, max_tracks=3000):


                           0.02**2,
                           1**2, 1**2, 1**2,
                           (0.01)**2, (0.01)**2, (0.01)**2])
  
class LiveKalman():
  def __init__(self, N=0, max_tracks=3000):


      # 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,
keep keep keep add keep keep keep keep
 get ready for live localizer 
selfdrive/locationd/kalman/live_kf.py