openpilot_comma/selfdrive/locationd/kalman/live_model.py

import numpy as np
import sympy as sp
import os
import sysconfig

from laika.constants import EARTH_GM
from common.sympy_helpers import euler_rotate, quat_rotate, quat_matrix_r
from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
from selfdrive.locationd.kalman.ekf_sym import gen_code


class States():
  ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
  ECEF_ORIENTATION = slice(3, 7)  # quat for pose of phone in ecef
  ECEF_VELOCITY = slice(7, 10)  # ecef velocity in m/s
  ANGULAR_VELOCITY = slice(10, 13)  # roll, pitch and yaw rates in device frame in radians/s
  GYRO_BIAS = slice(13, 16)  # roll, pitch and yaw biases
  ODO_SCALE = slice(16, 17)  # odometer scale
  ACCELERATION = slice(17, 20)  # Acceleration in device frame in m/s**2
  IMU_OFFSET = slice(20, 23)  # imu offset angles in radians

  ECEF_POS_ERR = slice(0, 3)
  ECEF_ORIENTATION_ERR = slice(3, 6)
  ECEF_VELOCITY_ERR = slice(6, 9)
  ANGULAR_VELOCITY_ERR = slice(9, 12)
  GYRO_BIAS_ERR = slice(12, 15)
  ODO_SCALE_ERR = slice(15, 16)
  ACCELERATION_ERR = slice(16, 19)
  IMU_OFFSET_ERR = slice(19, 22)


def gen_model(name, dim_state, dim_state_err, maha_test_kinds):
  # check if rebuild is needed
  try:
    dir_path = os.path.dirname(__file__)
    deps = [dir_path + '/' + 'ekf_c.c',
            dir_path + '/' + 'ekf_sym.py',
            dir_path + '/' + name + '_model.py',
            dir_path + '/' + name + '_kf.py']

    outs = [dir_path + '/' + name + '.o',
            dir_path + '/' + name + sysconfig.get_config_var('EXT_SUFFIX'),
            dir_path + '/' + name + '.cpp']
    out_times = list(map(os.path.getmtime, outs))
    dep_times = list(map(os.path.getmtime, deps))
    rebuild = os.getenv("REBUILD", False)
    if min(out_times) > max(dep_times) and not rebuild:
      return
    list(map(os.remove, outs))
  except OSError as e:
    print('HAHAHA')
    print(e)
    pass

  # make functions and jacobians with sympy
  # state variables
  state_sym = sp.MatrixSymbol('state', dim_state, 1)
  state = sp.Matrix(state_sym)
  x,y,z = state[States.ECEF_POS,:]
  q = state[States.ECEF_ORIENTATION,:]
  v = state[States.ECEF_VELOCITY,:]
  vx, vy, vz = v
  omega = state[States.ANGULAR_VELOCITY,:]
  vroll, vpitch, vyaw = omega
  roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS,:]
  odo_scale = state[16,:]
  acceleration = state[States.ACCELERATION,:]
  imu_angles= state[States.IMU_OFFSET,:]

  dt = sp.Symbol('dt')

  # calibration and attitude rotation matrices
  quat_rot = quat_rotate(*q)

  # Got the quat predict equations from here
  # A New Quaternion-Based Kalman Filter for
  # Real-Time Attitude Estimation Using the Two-Step
  # Geometrically-Intuitive Correction Algorithm
  A = 0.5*sp.Matrix([[0, -vroll, -vpitch, -vyaw],
                 [vroll, 0, vyaw, -vpitch],
                 [vpitch, -vyaw, 0, vroll],
                 [vyaw, vpitch, -vroll, 0]])
  q_dot = A * q

  # Time derivative of the state as a function of state
  state_dot = sp.Matrix(np.zeros((dim_state, 1)))
  state_dot[States.ECEF_POS,:] = v
  state_dot[States.ECEF_ORIENTATION,:] = q_dot
  state_dot[States.ECEF_VELOCITY,0] = quat_rot * acceleration

  # Basic descretization, 1st order intergrator
  # Can be pretty bad if dt is big
  f_sym = state + dt*state_dot

  state_err_sym = sp.MatrixSymbol('state_err',dim_state_err,1)
  state_err = sp.Matrix(state_err_sym)
  quat_err = state_err[States.ECEF_ORIENTATION_ERR,:]
  v_err = state_err[States.ECEF_VELOCITY_ERR,:]
  omega_err = state_err[States.ANGULAR_VELOCITY_ERR,:]
  acceleration_err = state_err[States.ACCELERATION_ERR,:]

  # Time derivative of the state error as a function of state error and state
  quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
  q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
  state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
  state_err_dot[States.ECEF_POS_ERR,:] = v_err
  state_err_dot[States.ECEF_ORIENTATION_ERR,:] = q_err_dot
  state_err_dot[States.ECEF_VELOCITY_ERR,:] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
  f_err_sym = state_err + dt*state_err_dot

  # Observation matrix modifier
  H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
  H_mod_sym[0:3, 0:3] = np.eye(3)
  H_mod_sym[3:7,3:6] = 0.5*quat_matrix_r(state[3:7])[:,1:]
  H_mod_sym[7:, 6:] = np.eye(dim_state-7)

  # these error functions are defined so that say there
  # is a nominal x and true x:
  # true x = err_function(nominal x, delta x)
  # delta x = inv_err_function(nominal x, true x)
  nom_x = sp.MatrixSymbol('nom_x',dim_state,1)
  true_x = sp.MatrixSymbol('true_x',dim_state,1)
  delta_x = sp.MatrixSymbol('delta_x',dim_state_err,1)

  err_function_sym = sp.Matrix(np.zeros((dim_state,1)))
  delta_quat = sp.Matrix(np.ones((4)))
  delta_quat[1:,:] = sp.Matrix(0.5*delta_x[3:6,:])
  err_function_sym[0:3,:] = sp.Matrix(nom_x[0:3,:] + delta_x[0:3,:])
  err_function_sym[3:7,0] = quat_matrix_r(nom_x[3:7,0])*delta_quat
  err_function_sym[7:,:] = sp.Matrix(nom_x[7:,:] + delta_x[6:,:])

  inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err,1)))
  inv_err_function_sym[0:3,0] = sp.Matrix(-nom_x[0:3,0] + true_x[0:3,0])
  delta_quat = quat_matrix_r(nom_x[3:7,0]).T*true_x[3:7,0]
  inv_err_function_sym[3:6,0] = sp.Matrix(2*delta_quat[1:])
  inv_err_function_sym[6:,0] = sp.Matrix(-nom_x[7:,0] + true_x[7:,0])

  eskf_params = [[err_function_sym, nom_x, delta_x],
                 [inv_err_function_sym, nom_x, true_x],
                 H_mod_sym, f_err_sym, state_err_sym]



  #
  # Observation functions
  #


  imu_rot = euler_rotate(*imu_angles)
  h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
                                 vpitch + pitch_bias,
                                 vyaw + yaw_bias])

  pos = sp.Matrix([x, y, z])
  gravity = quat_rot.T * ((EARTH_GM/((x**2 + y**2 + z**2)**(3.0/2.0)))*pos)
  h_acc_sym = imu_rot*(gravity + acceleration)
  h_phone_rot_sym = sp.Matrix([vroll,
                               vpitch,
                               vyaw])
  speed = vx**2 + vy**2 + vz**2
  h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])

  h_pos_sym = sp.Matrix([x, y, z])
  h_imu_frame_sym = sp.Matrix(imu_angles)

  h_relative_motion = sp.Matrix(quat_rot.T * v)


  obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
             [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
             [h_phone_rot_sym, ObservationKind.NO_ROT, None],
             [h_acc_sym, ObservationKind.PHONE_ACCEL, None],
             [h_pos_sym, ObservationKind.ECEF_POS, None],
             [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
             [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
             [h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]

  gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)