code: #!/usr/bin/env python3
  import json
  import numpy as np
  import sympy as sp
  import cereal.messaging as messaging
  from cereal import log
  from common.params import Params
  import common.transformations.coordinates as coord
  from common.transformations.orientation import ecef_euler_from_ned, \
                                                 euler_from_quat, \
                                                 ned_euler_from_ecef, \
                                                 quat_from_euler, euler_from_rot, \
                                                 rot_from_quat, rot_from_euler
  from rednose.helpers import KalmanError
  from selfdrive.locationd.models.live_kf import LiveKalman, States, ObservationKind
  from selfdrive.locationd.models.constants import GENERATED_DIR
  from selfdrive.swaglog import cloudlog
  
  #from datetime import datetime
  #from laika.gps_time import GPSTime
  
  from sympy.utilities.lambdify import lambdify
  from rednose.helpers.sympy_helpers import euler_rotate
  
  SensorSource = log.SensorEventData.SensorSource
  
  
  VISION_DECIMATION = 2
  SENSOR_DECIMATION = 10
  POSENET_STD_HIST = 40
  
  
  def to_float(arr):
    return [float(arr[0]), float(arr[1]), float(arr[2])]
  
  
  def get_H():
    # this returns a function to eval the jacobian
    # of the observation function of the local vel
    roll = sp.Symbol('roll')
    pitch = sp.Symbol('pitch')
    yaw = sp.Symbol('yaw')
    vx = sp.Symbol('vx')
    vy = sp.Symbol('vy')
    vz = sp.Symbol('vz')
  
    h = euler_rotate(roll, pitch, yaw).T*(sp.Matrix([vx, vy, vz]))
    H = h.jacobian(sp.Matrix([roll, pitch, yaw, vx, vy, vz]))
    H_f = lambdify([roll, pitch, yaw, vx, vy, vz], H)
    return H_f
  
  
  class Localizer():
    def __init__(self, disabled_logs=None, dog=None):
      if disabled_logs is None:
        disabled_logs = []
  
      self.kf = LiveKalman(GENERATED_DIR)
      self.reset_kalman()
      self.max_age = .1  # seconds
      self.disabled_logs = disabled_logs
      self.calib = np.zeros(3)
      self.device_from_calib = np.eye(3)
      self.calib_from_device = np.eye(3)
      self.calibrated = False
      self.H = get_H()
  
      self.posenet_invalid_count = 0
      self.posenet_speed = 0
      self.car_speed = 0
      self.posenet_stds = 10*np.ones((POSENET_STD_HIST))
  
      self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS])
  
      self.unix_timestamp_millis = 0
      self.last_gps_fix = 0
      self.device_fell = False
  
    @staticmethod
    def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov, calibrated):
      predicted_std = np.sqrt(np.diagonal(predicted_cov))
  
      fix_ecef = predicted_state[States.ECEF_POS]
      fix_ecef_std = predicted_std[States.ECEF_POS_ERR]
      vel_ecef = predicted_state[States.ECEF_VELOCITY]
      vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR]
      fix_pos_geo = coord.ecef2geodetic(fix_ecef)
      #fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
      orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION])
      orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
      device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
      calibrated_orientation_ecef = euler_from_rot(calib_from_device.dot(device_from_ecef))
  
      acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION])
      acc_calib_std = np.sqrt(np.diagonal(calib_from_device.dot(
        predicted_cov[States.ACCELERATION_ERR, States.ACCELERATION_ERR]).dot(
          calib_from_device.T)))
      ang_vel_calib = calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY])
      ang_vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot(
        predicted_cov[States.ANGULAR_VELOCITY_ERR, States.ANGULAR_VELOCITY_ERR]).dot(
          calib_from_device.T)))
  
      vel_device = device_from_ecef.dot(vel_ecef)
      device_from_ecef_eul = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]).T
      idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + \
             list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop))
      condensed_cov = predicted_cov[idxs][:, idxs]
      HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef])))
      vel_device_cov = HH.dot(condensed_cov).dot(HH.T)
      vel_device_std = np.sqrt(np.diagonal(vel_device_cov))
  
      vel_calib = calib_from_device.dot(vel_device)
      vel_calib_std = np.sqrt(np.diagonal(calib_from_device.dot(
        vel_device_cov).dot(calib_from_device.T)))
  
      orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
      #orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
      ned_vel = converter.ecef2ned(fix_ecef + vel_ecef) - converter.ecef2ned(fix_ecef)
      #ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
  
      fix = messaging.log.LiveLocationKalman.new_message()
  
      # write measurements to msg
      measurements = [
        # measurement field, value, std, valid
        (fix.positionGeodetic, fix_pos_geo, np.nan*np.zeros(3), True),
        (fix.positionECEF, fix_ecef, fix_ecef_std, True),
        (fix.velocityECEF, vel_ecef, vel_ecef_std, True),
        (fix.velocityNED, ned_vel, np.nan*np.zeros(3), True),
        (fix.velocityDevice, vel_device, vel_device_std, True),
        (fix.accelerationDevice, predicted_state[States.ACCELERATION], predicted_std[States.ACCELERATION_ERR], True),
        (fix.orientationECEF, orientation_ecef, orientation_ecef_std, True),
        (fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), calibrated),
        (fix.orientationNED, orientation_ned, np.nan*np.zeros(3), True),
        (fix.angularVelocityDevice, predicted_state[States.ANGULAR_VELOCITY], predicted_std[States.ANGULAR_VELOCITY_ERR], True),
        (fix.velocityCalibrated, vel_calib, vel_calib_std, calibrated),
        (fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, calibrated),
        (fix.accelerationCalibrated, acc_calib, acc_calib_std, calibrated),
      ]
  
      for field, value, std, valid in measurements:
        # TODO: can we write the lists faster?
        field.value = to_float(value)
        field.std = to_float(std)
        field.valid = valid
  
      return fix
  
    def liveLocationMsg(self):
      fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P, self.calibrated)
      # experimentally found these values, no false positives in 20k minutes of driving
      old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:])
      std_spike = new_mean/old_mean > 4 and new_mean > 7
  
      fix.posenetOK = not (std_spike and self.car_speed > 5)
      fix.deviceStable = not self.device_fell
      self.device_fell = False
  
      #fix.gpsWeek = self.time.week
      #fix.gpsTimeOfWeek = self.time.tow
      fix.unixTimestampMillis = self.unix_timestamp_millis
  
      if np.linalg.norm(fix.positionECEF.std) < 50 and self.calibrated:
        fix.status = 'valid'
      elif np.linalg.norm(fix.positionECEF.std) < 50:
        fix.status = 'uncalibrated'
      else:
        fix.status = 'uninitialized'
      return fix
  
    def update_kalman(self, time, kind, meas, R=None):
      try:
        self.kf.predict_and_observe(time, kind, meas, R)
      except KalmanError:
        cloudlog.error("Error in predict and observe, kalman reset")
        self.reset_kalman()
  
    def handle_gps(self, current_time, log):
      # ignore the message if the fix is invalid
      if log.flags % 2 == 0:
        return
  
      self.last_gps_fix = current_time
  
      self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
      ecef_pos = self.converter.ned2ecef([0, 0, 0])
      ecef_vel = self.converter.ned2ecef(np.array(log.vNED)) - ecef_pos
      ecef_pos_R = np.diag([(3*log.verticalAccuracy)**2]*3)
      ecef_vel_R = np.diag([(log.speedAccuracy)**2]*3)
  
      #self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3))
      self.unix_timestamp_millis = log.timestamp
      gps_est_error = np.sqrt((self.kf.x[0] - ecef_pos[0])**2 +
                              (self.kf.x[1] - ecef_pos[1])**2 +
                              (self.kf.x[2] - ecef_pos[2])**2)
  
      orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION])
      orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef)
      orientation_ned_gps = np.array([0, 0, np.radians(log.bearingDeg)])
      orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi
      initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
      if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1:
        cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset")
        self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
        self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat)
      elif gps_est_error > 50:
        cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset")
        self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
  
      self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R)
      self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R)
  
    def handle_car_state(self, current_time, log):
      self.speed_counter += 1
  
      if self.speed_counter % SENSOR_DECIMATION == 0:
        self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, [log.vEgo])
        self.car_speed = abs(log.vEgo)
        if log.vEgo == 0:
          self.update_kalman(current_time, ObservationKind.NO_ROT, [0, 0, 0])
  
    def handle_cam_odo(self, current_time, log):
      self.cam_counter += 1
  
      if self.cam_counter % VISION_DECIMATION == 0:
        rot_device = self.device_from_calib.dot(log.rot)
        rot_device_std = self.device_from_calib.dot(log.rotStd)
        self.update_kalman(current_time,
                           ObservationKind.CAMERA_ODO_ROTATION,
                           np.concatenate([rot_device, 10*rot_device_std]))
        trans_device = self.device_from_calib.dot(log.trans)
        trans_device_std = self.device_from_calib.dot(log.transStd)
        self.posenet_speed = np.linalg.norm(trans_device)
        self.posenet_stds[:-1] = self.posenet_stds[1:]
        self.posenet_stds[-1] = trans_device_std[0]
        self.update_kalman(current_time,
                           ObservationKind.CAMERA_ODO_TRANSLATION,
                           np.concatenate([trans_device, 10*trans_device_std]))
  
    def handle_sensors(self, current_time, log):
      # TODO does not yet account for double sensor readings in the log
      for sensor_reading in log:
        sensor_time = 1e-9 * sensor_reading.timestamp
        # TODO: handle messages from two IMUs at the same time
        if sensor_reading.source == SensorSource.lsm6ds3:
          continue
  
        # Gyro Uncalibrated
        if sensor_reading.sensor == 5 and sensor_reading.type == 16:
          self.gyro_counter += 1
          if self.gyro_counter % SENSOR_DECIMATION == 0:
            v = sensor_reading.gyroUncalibrated.v
            self.update_kalman(sensor_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]])
  
        # Accelerometer
        if sensor_reading.sensor == 1 and sensor_reading.type == 1:
          # check if device fell, estimate 10 for g
          # 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
          self.device_fell = self.device_fell or (np.linalg.norm(np.array(sensor_reading.acceleration.v) - np.array([10, 0, 0])) > 40)
  
          self.acc_counter += 1
          if self.acc_counter % SENSOR_DECIMATION == 0:
            v = sensor_reading.acceleration.v
            self.update_kalman(sensor_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]])
  
    def handle_live_calib(self, current_time, log):
      if len(log.rpyCalib):
        self.calib = log.rpyCalib
        self.device_from_calib = rot_from_euler(self.calib)
        self.calib_from_device = self.device_from_calib.T
        self.calibrated = log.calStatus == 1
  
    def reset_kalman(self, current_time=None, init_orient=None, init_pos=None):
      self.filter_time = current_time
      init_x = LiveKalman.initial_x.copy()
      # too nonlinear to init on completely wrong
      if init_orient is not None:
        init_x[3:7] = init_orient
      if init_pos is not None:
        init_x[:3] = init_pos
      self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time)
  
      self.observation_buffer = []
  
      self.gyro_counter = 0
      self.acc_counter = 0
      self.speed_counter = 0
      self.cam_counter = 0
  
  
  def locationd_thread(sm, pm, disabled_logs=None):
    if disabled_logs is None:
      disabled_logs = []
  
    if sm is None:
      socks = ['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration', 'carState']
      sm = messaging.SubMaster(socks, ignore_alive=['gpsLocationExternal'])
    if pm is None:
      pm = messaging.PubMaster(['liveLocationKalman'])
  
    params = Params()
    localizer = Localizer(disabled_logs=disabled_logs)
  
    while True:
      sm.update()
  
      for sock, updated in sm.updated.items():
        if updated and sm.valid[sock]:
          t = sm.logMonoTime[sock] * 1e-9
          if sock == "sensorEvents":
            localizer.handle_sensors(t, sm[sock])
          elif sock == "gpsLocationExternal":
            localizer.handle_gps(t, sm[sock])
          elif sock == "carState":
            localizer.handle_car_state(t, sm[sock])
          elif sock == "cameraOdometry":
            localizer.handle_cam_odo(t, sm[sock])
          elif sock == "liveCalibration":
            localizer.handle_live_calib(t, sm[sock])
  
      if sm.updated['cameraOdometry']:
        t = sm.logMonoTime['cameraOdometry']
        msg = messaging.new_message('liveLocationKalman')
        msg.logMonoTime = t
  
        msg.liveLocationKalman = localizer.liveLocationMsg()
        msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
        msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents']
  
        gps_age = (t / 1e9) - localizer.last_gps_fix
        msg.liveLocationKalman.gpsOK = gps_age < 1.0
        pm.send('liveLocationKalman', msg)
  
        if sm.frame % 1200 == 0 and msg.liveLocationKalman.gpsOK:  # once a minute
          location = {
            'latitude': msg.liveLocationKalman.positionGeodetic.value[0],
            'longitude': msg.liveLocationKalman.positionGeodetic.value[1],
            'altitude': msg.liveLocationKalman.positionGeodetic.value[2],
          }
          params.put("LastGPSPosition", json.dumps(location))
  
  
  def main(sm=None, pm=None):
    locationd_thread(sm, pm)
  
  
  if __name__ == "__main__":
    import os
    os.environ["OMP_NUM_THREADS"] = "1"
    main()
  
apis: numpy.eye numpy.ones numpy.array numpy.zeros numpy.diag numpy.concatenate numpy.mod numpy.sqrt numpy.diagonal numpy.linalg.norm numpy.radians numpy.mean