      # 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:
        self.filter_ready = True
        initial_ecef = fix_ecef
        initial_state = np.zeros(29)
        gps_bearing = log.gpsLocationExternal.bearing*(np.pi/180)
        initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing])
        initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
        gps_speed = log.gpsLocationExternal.speed
        quat_uncertainty = 0.2**2
 
</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       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   def handle_ublox_gnss(self, log, current_time):
    if hasattr(log.ubloxGnss, 'measurementReport'):
      self.raw_gnss_counter += 1
      if True or self.raw_gnss_counter % 3==0:
        processed_raw = gnss.process_measurements(gnss.read_raw_ublox(log.ubloxGnss.measurementReport), dog=self.dog)
        corrected_raw = gnss.correct_measurements(processed_raw, self.kf.x[:3], dog=self.dog)
        corrected_raw = np.array([c.as_array() for c in corrected_raw]).reshape((-1,14))
        self.update_kalman(current_time, ObservationKind.PSEUDORANGE_GPS, corrected_raw)
        self.update_kalman(current_time, ObservationKind.PSEUDORANGE_RATE_GPS, corrected_raw)
    #elif hasattr(log.ubloxGnss, 'ephemeris'):
    #  self.dog.add_ublox_ephems([log])
    #  if len(self.dog.orbits.keys()) < 6:
    #    print 'Added ublox ephem now has ', len(self.dog.orbits.keys())

  def handle_qcom_gnss(self, log, current_time):
    if hasattr(log.qcomGnss, 'drSvPoly') and self.week is not None:
      self.dog.add_qcom_ephems([log], self.week)
      if len(list(self.dog.orbits.keys())) < 6:
        print('Added qcom ephem now has ', len(list(self.dog.orbits.keys())))
    if hasattr(log.qcomGnss, 'drMeasurementReport') and log.qcomGnss.drMeasurementReport.source == "gps":
      self.week = log.qcomGnss.drMeasurementReport.gpsWeek

 </s> add  </s> remove     fix_ecef = predicted_state[0:3]
 </s> add     fix_ecef = predicted_state[States.ECEF_POS] </s> remove     if dog:
      self.dog = dog
    else:
      self.dog = AstroDog(auto_update=True)
 </s> add 
      # 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:
        self.filter_ready = True
        initial_ecef = fix_ecef

        gps_bearing = log.gpsLocationExternal.bearing*(np.pi/180)
        initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing])
        initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
        gps_speed = log.gpsLocationExternal.speed
        quat_uncertainty = 0.2**2
keep keep keep keep replace keep keep keep keep keep
 get ready for live localizer 
selfdrive/locationd/locationd.py