      fix.alt = float(fix_pos_geo[2])
  
      fix.speed = float(np.linalg.norm(predicted_state[7:10]))
  
      orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[3:7]))
      fix.roll = float(orientation_ned_euler[0]*180/np.pi)
      fix.pitch = float(orientation_ned_euler[1]*180/np.pi)
      fix.heading = float(orientation_ned_euler[2]*180/np.pi)
  
      fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])]
 
</s> remove     fix.speed = float(np.linalg.norm(predicted_state[7:10]))
 </s> add     fix.speed = float(np.linalg.norm(predicted_state[States.ECEF_VELOCITY])) </s> remove     local_vel = rotations_from_quats(predicted_state[3:7]).T.dot(predicted_state[7:10])
 </s> add     local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY]) </s> remove     fix_ecef = predicted_state[0:3]
 </s> add     fix_ecef = predicted_state[States.ECEF_POS] </s> remove     self.raw_gnss_counter = 0
 </s> add  </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)
    orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION]))
      fix.alt = float(fix_pos_geo[2])
  
      fix.speed = float(np.linalg.norm(predicted_state[7:10]))
  
    orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION]))
      fix.roll = float(orientation_ned_euler[0]*180/np.pi)
      fix.pitch = float(orientation_ned_euler[1]*180/np.pi)
      fix.heading = float(orientation_ned_euler[2]*180/np.pi)
  
      fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])]
keep keep keep keep replace keep keep keep keep keep
 get ready for live localizer 
selfdrive/locationd/locationd.py