openpilot_comma/selfdrive/locationd/kalman/helpers/__init__.py

import numpy as np
import os
from bisect import bisect
from tqdm import tqdm
from cffi import FFI

TEMPLATE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'templates'))
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'generated'))


def write_code(name, code, header):
  if not os.path.exists(GENERATED_DIR):
    os.mkdir(GENERATED_DIR)

  open(os.path.join(GENERATED_DIR, f"{name}.cpp"), 'w').write(code)
  open(os.path.join(GENERATED_DIR, f"{name}.h"), 'w').write(header)


def load_code(name):
  shared_fn = os.path.join(GENERATED_DIR, f"lib{name}.so")
  header_fn = os.path.join(GENERATED_DIR, f"{name}.h")
  header = open(header_fn).read()

  ffi = FFI()
  ffi.cdef(header)
  return (ffi, ffi.dlopen(shared_fn))


class KalmanError(Exception):
  pass


class ObservationKind():
  UNKNOWN = 0
  NO_OBSERVATION = 1
  GPS_NED = 2
  ODOMETRIC_SPEED = 3
  PHONE_GYRO = 4
  GPS_VEL = 5
  PSEUDORANGE_GPS = 6
  PSEUDORANGE_RATE_GPS = 7
  SPEED = 8
  NO_ROT = 9
  PHONE_ACCEL = 10
  ORB_POINT = 11
  ECEF_POS = 12
  CAMERA_ODO_TRANSLATION = 13
  CAMERA_ODO_ROTATION = 14
  ORB_FEATURES = 15
  MSCKF_TEST = 16
  FEATURE_TRACK_TEST = 17
  LANE_PT = 18
  IMU_FRAME = 19
  PSEUDORANGE_GLONASS = 20
  PSEUDORANGE_RATE_GLONASS = 21
  PSEUDORANGE = 22
  PSEUDORANGE_RATE = 23

  ROAD_FRAME_XY_SPEED = 24  # (x, y) [m/s]
  ROAD_FRAME_YAW_RATE = 25  # [rad/s]
  STEER_ANGLE = 26  # [rad]
  ANGLE_OFFSET_FAST = 27  # [rad]
  STIFFNESS = 28  # [-]
  STEER_RATIO = 29  # [-]

  names = [
    'Unknown',
    'No observation',
    'GPS NED',
    'Odometric speed',
    'Phone gyro',
    'GPS velocity',
    'GPS pseudorange',
    'GPS pseudorange rate',
    'Speed',
    'No rotation',
    'Phone acceleration',
    'ORB point',
    'ECEF pos',
    'camera odometric translation',
    'camera odometric rotation',
    'ORB features',
    'MSCKF test',
    'Feature track test',
    'Lane ecef point',
    'imu frame eulers',
    'GLONASS pseudorange',
    'GLONASS pseudorange rate',

    'Road Frame x,y speed',
    'Road Frame yaw rate',
    'Steer Angle',
    'Fast Angle Offset',
    'Stiffness',
    'Steer Ratio',
  ]

  @classmethod
  def to_string(cls, kind):
    return cls.names[kind]



SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
           ObservationKind.PSEUDORANGE_RATE_GPS,
           ObservationKind.PSEUDORANGE_GLONASS,
           ObservationKind.PSEUDORANGE_RATE_GLONASS]


def run_car_ekf_offline(kf, observations_by_kind):
  from laika.raw_gnss import GNSSMeasurement
  observations = []
  # create list of observations with element format: [kind, time, data]
  for kind in observations_by_kind:
    for t, data in zip(observations_by_kind[kind][0], observations_by_kind[kind][1]):
      observations.append([t, kind, data])
  observations.sort(key=lambda obs: obs[0])

  times, estimates = run_observations_through_filter(kf, observations)

  forward_states = np.stack(e[1] for e in estimates)
  forward_covs = np.stack(e[3] for e in estimates)
  smoothed_states, smoothed_covs = kf.rts_smooth(estimates)

  observations_dict = {}
  # TODO assuming observations and estimates
  # are same length may not work with VO
  for e in estimates:
    t = e[4]
    kind = str(int(e[5]))
    res = e[6]
    z = e[7]
    ea = e[8]
    if len(z) == 0:
      continue
    if kind not in observations_dict:
      observations_dict[kind] = {}
      observations_dict[kind]['t'] = np.array(len(z)*[t])
      observations_dict[kind]['z'] = np.array(z)
      observations_dict[kind]['ea'] = np.array(ea)
      observations_dict[kind]['residual'] = np.array(res)
    else:
      observations_dict[kind]['t'] = np.append(observations_dict[kind]['t'], np.array(len(z)*[t]))
      observations_dict[kind]['z'] = np.vstack((observations_dict[kind]['z'], np.array(z)))
      observations_dict[kind]['ea'] = np.vstack((observations_dict[kind]['ea'], np.array(ea)))
      observations_dict[kind]['residual'] = np.vstack((observations_dict[kind]['residual'], np.array(res)))

  # add svIds to gnss data
  for kind in map(str, SAT_OBS):
    if int(kind) in observations_by_kind and kind in observations_dict:
      observations_dict[kind]['svIds'] = np.array([])
      observations_dict[kind]['CNO'] = np.array([])
      observations_dict[kind]['std'] = np.array([])
      for obs in observations_by_kind[int(kind)][1]:
        observations_dict[kind]['svIds'] = np.append(observations_dict[kind]['svIds'],
                                                     np.array([obs[:,GNSSMeasurement.PRN]]))
        observations_dict[kind]['std'] = np.append(observations_dict[kind]['std'],
                                                   np.array([obs[:,GNSSMeasurement.PR_STD]]))
  return smoothed_states, smoothed_covs, forward_states, forward_covs, times, observations_dict


def run_observations_through_filter(kf, observations, filter_time=None):
  estimates = []

  for obs in tqdm(observations):
    t = obs[0]
    kind = obs[1]
    data = obs[2]
    estimates.append(kf.predict_and_observe(t, kind, data))
  times = [x[4] for x in estimates]
  return times, estimates


def save_residuals_plot(obs, save_path, data_name):
  import matplotlib.pyplot as plt
  import mpld3  # pylint: disable=import-error
  fig = plt.figure(figsize=(10,20))
  fig.suptitle('Residuals of ' + data_name, fontsize=24)
  n = len(list(obs.keys()))
  start_times = [obs[kind]['t'][0] for kind in obs]
  start_time = min(start_times)
  xlims = [start_time + 3, start_time + 60]

  for i, kind in enumerate(obs):
    ax = fig.add_subplot(n, 1, i+1)
    ax.set_xlim(xlims)
    t = obs[kind]['t']
    res = obs[kind]['residual']
    start_idx = bisect(t, xlims[0])
    if len(res) == start_idx:
      continue
    ylim = max(np.linalg.norm(res[start_idx:], axis=1))
    ax.set_ylim([-ylim, ylim])
    if int(kind) in SAT_OBS:
      svIds = obs[kind]['svIds']
      for svId in set(svIds):
        svId_idx = (svIds == svId)
        t = obs[kind]['t'][svId_idx]
        res = obs[kind]['residual'][svId_idx]
        ax.plot(t, res, label='SV ' + str(int(svId)))
        ax.legend(loc='right')
    else:
      ax.plot(t, res)
    plt.title('Residual of kind ' + ObservationKind.to_string(int(kind)), fontsize=20)
  plt.tight_layout()
  os.makedirs(save_path)
  mpld3.save_html(fig, save_path + 'residuals_plot.html')