  from .kalman_helpers import ObservationKind
  from .ekf_sym import EKF_sym
  
  
  
  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,
                           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,
 
</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 import laika.raw_gnss as gnss
from laika.astro_dog import AstroDog

from selfdrive.locationd.kalman.loc_kf import LocKalman
 </s> add from selfdrive.locationd.kalman.live_kf import LiveKalman, States, inital_x, initial_P_diag </s> remove           if max(abs(self.kf.x[23:26])) > 0.07:
 </s> add           if max(abs(self.kf.x[States.IMU_OFFSET])) > 0.07:
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,
  from .kalman_helpers import ObservationKind
  from .ekf_sym import EKF_sym
  
  
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
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,
keep keep keep keep replace replace replace replace replace replace replace replace replace replace replace replace replace replace keep keep keep keep keep
 get ready for live localizer 
selfdrive/locationd/kalman/live_kf.py