sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Live kf whitespace cleanup, fix covariance phone accel

Browse Source
pull/1086/head
Willem Melching 5 years ago
parent 6823971a11
commit 1de5d84a06
1 changed files with 63 additions and 71 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 134
      selfdrive/locationd/kalman/models/live_kf.py

134
selfdrive/locationd/kalman/models/live_kf.py
Unescape View File

@ -8,7 +8,7 @@ from selfdrive.locationd.kalman.helpers.sympy_helpers import (euler_rotate,
quat_matrix_r, quat_matrix_r,
quat_rotate) quat_rotate)
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
#from laika.constants import EARTH_GM
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
@ -44,7 +44,6 @@ class LiveKalman():
0, 0, 0, 0, 0, 0,
0, 0, 0]) 0, 0, 0])
# state covariance # state covariance
initial_P_diag = np.array([10000**2, 10000**2, 10000**2, initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
10**2, 10**2, 10**2, 10**2, 10**2, 10**2,
@ -60,10 +59,10 @@ class LiveKalman():
0.0**2, 0.0**2, 0.0**2, 0.0**2, 0.0**2, 0.0**2,
0.0**2, 0.0**2, 0.0**2, 0.0**2, 0.0**2, 0.0**2,
0.1**2, 0.1**2, 0.1**2, 0.1**2, 0.1**2, 0.1**2,
(0.005/100)**2, (0.005/100)**2, (0.005/100)**2, (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02/100)**2, (0.02 / 100)**2,
3**2, 3**2, 3**2, 3**2, 3**2, 3**2,
(0.05/60)**2, (0.05/60)**2, (0.05/60)**2]) (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
@staticmethod @staticmethod
def generate_code(): def generate_code():
@ -73,16 +72,16 @@ class LiveKalman():
state_sym = sp.MatrixSymbol('state', dim_state, 1) state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym) state = sp.Matrix(state_sym)
x,y,z = state[States.ECEF_POS,:] x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION,:] q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY,:] v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY,:] omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS,:] roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
odo_scale = state[16,:] odo_scale = state[16, :]
acceleration = state[States.ACCELERATION,:] acceleration = state[States.ACCELERATION, :]
imu_angles= state[States.IMU_OFFSET,:] imu_angles = state[States.IMU_OFFSET, :]
dt = sp.Symbol('dt') dt = sp.Symbol('dt')
@ -93,104 +92,97 @@ class LiveKalman():
# A New Quaternion-Based Kalman Filter for # A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step # Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm # Geometrically-Intuitive Correction Algorithm
A = 0.5*sp.Matrix([[0, -vroll, -vpitch, -vyaw], A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
[vroll, 0, vyaw, -vpitch], [vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll], [vpitch, -vyaw, 0, vroll],
[vyaw, vpitch, -vroll, 0]]) [vyaw, vpitch, -vroll, 0]])
q_dot = A * q q_dot = A * q
# Time derivative of the state as a function of state # Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1))) state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS,:] = v state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION,:] = q_dot state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY,0] = quat_rot * acceleration state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator # Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big # Can be pretty bad if dt is big
f_sym = state + dt*state_dot f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err',dim_state_err,1) state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym) state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR,:] quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR,:] v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR,:] omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR,:] acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state # 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]) 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) 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 = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR,:] = v_err state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR,:] = q_err_dot 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) state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
f_err_sym = state_err + dt*state_err_dot f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier # Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) 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[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[3:7, 3:6] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[7:, 6:] = np.eye(dim_state-7) H_mod_sym[7:, 6:] = np.eye(dim_state - 7)
# these error functions are defined so that say there # these error functions are defined so that say there
# is a nominal x and true x: # is a nominal x and true x:
# true x = err_function(nominal x, delta x) # true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x) # delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x',dim_state,1) nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x',dim_state,1) true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x',dim_state_err,1) delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state,1))) err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones((4))) delta_quat = sp.Matrix(np.ones((4)))
delta_quat[1:,:] = sp.Matrix(0.5*delta_x[3:6,:]) 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[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[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:,:]) 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 = 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]) 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] 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[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]) 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], eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x], [inv_err_function_sym, nom_x, true_x],
H_mod_sym, f_err_sym, state_err_sym] H_mod_sym, f_err_sym, state_err_sym]
# #
# Observation functions # Observation functions
# #
imu_rot = euler_rotate(*imu_angles) imu_rot = euler_rotate(*imu_angles)
h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias, h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias,
vpitch + pitch_bias, vpitch + pitch_bias,
vyaw + yaw_bias]) vyaw + yaw_bias])
pos = sp.Matrix([x, y, z]) pos = sp.Matrix([x, y, z])
gravity = quat_rot.T * ((EARTH_GM/((x**2 + y**2 + z**2)**(3.0/2.0)))*pos) 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_acc_sym = imu_rot * (gravity + acceleration)
h_phone_rot_sym = sp.Matrix([vroll, h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
vpitch,
vyaw]) speed = sp.sqrt(vx**2 + vy**2 + vz**2)
speed = vx**2 + vy**2 + vz**2 h_speed_sym = sp.Matrix([speed * odo_scale])
h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])
h_pos_sym = sp.Matrix([x, y, z]) h_pos_sym = sp.Matrix([x, y, z])
h_imu_frame_sym = sp.Matrix(imu_angles) h_imu_frame_sym = sp.Matrix(imu_angles)
h_relative_motion = sp.Matrix(quat_rot.T * v) h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None], obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
[h_gyro_sym, ObservationKind.PHONE_GYRO, None], [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None], [h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None], [h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None], [h_pos_sym, ObservationKind.ECEF_POS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_imu_frame_sym, ObservationKind.IMU_FRAME, 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) gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
@ -200,7 +192,7 @@ class LiveKalman():
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]), ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5*2]), ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]), ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]), ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]), ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),

Write Preview
Loading…
Cancel
Save