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

Loc kf whitespace, fix phone accelerometer covariance

Browse Source 
old-commit-hash: 965a9ae042
commatwo_master
Willem Melching 6 years ago
parent e2b05dee77
commit 055c58c1da
1 changed files with 186 additions and 189 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 375
      selfdrive/locationd/kalman/models/loc_kf.py

375
selfdrive/locationd/kalman/models/loc_kf.py
Unescape View File

@ -1,7 +1,5 @@
#!/usr/bin/env python3
import os
import numpy as np
import sympy as sp
@ -11,7 +9,6 @@ from selfdrive.locationd.kalman.helpers.lst_sq_computer import LstSqComputer
from selfdrive.locationd.kalman.helpers.sympy_helpers import (euler_rotate,
quat_matrix_r,
quat_rotate)
#from laika.constants import EARTH_GM
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
@ -36,20 +33,20 @@ def parse_pr(m):
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
CLOCK_BIAS = slice(13, 14) # clock bias in light-meters,
CLOCK_DRIFT = slice(14, 15) # clock drift in light-meters/s,
GYRO_BIAS = slice(15, 18) # roll, pitch and yaw biases
ODO_SCALE = slice(18, 19) # odometer scale
ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2
FOCAL_SCALE = slice(22, 23) # focal length scale
IMU_OFFSET = slice(23,26) # imu offset angles in radians
GLONASS_BIAS = slice(26,27) # GLONASS bias in m expressed as bias + freq_num*freq_slope
GLONASS_FREQ_SLOPE = slice(27, 28) # GLONASS bias in m expressed as bias + freq_num*freq_slope
CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2,
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
CLOCK_BIAS = slice(13, 14) # clock bias in light-meters,
CLOCK_DRIFT = slice(14, 15) # clock drift in light-meters/s,
GYRO_BIAS = slice(15, 18) # roll, pitch and yaw biases
ODO_SCALE = slice(18, 19) # odometer scale
ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2
FOCAL_SCALE = slice(22, 23) # focal length scale
IMU_OFFSET = slice(23, 26) # imu offset angles in radians
GLONASS_BIAS = slice(26, 27) # GLONASS bias in m expressed as bias + freq_num*freq_slope
GLONASS_FREQ_SLOPE = slice(27, 28) # GLONASS bias in m expressed as bias + freq_num*freq_slope
CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2,
class LocKalman():
@ -69,33 +66,33 @@ class LocKalman():
# state covariance
P_initial = np.diag([10000**2, 10000**2, 10000**2,
10**2, 10**2, 10**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
(200000)**2, (100)**2,
0.05**2, 0.05**2, 0.05**2,
0.02**2,
1**2, 1**2, 1**2,
0.01**2,
(0.01)**2, (0.01)**2, (0.01)**2,
10**2, 1**2,
0.05**2])
10**2, 10**2, 10**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
(200000)**2, (100)**2,
0.05**2, 0.05**2, 0.05**2,
0.02**2,
1**2, 1**2, 1**2,
0.01**2,
(0.01)**2, (0.01)**2, (0.01)**2,
10**2, 1**2,
0.05**2])
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**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,
(.1)**2, (0.0)**2,
(0.005/100)**2, (0.005/100)**2, (0.005/100)**2,
(0.02/100)**2,
3**2, 3**2, 3**2,
0.001**2,
(0.05/60)**2, (0.05/60)**2, (0.05/60)**2,
(.1)**2, (.01)**2,
0.005**2])
maha_test_kinds = [ObservationKind.ORB_FEATURES] #, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
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,
(.1)**2, (0.0)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02 / 100)**2,
3**2, 3**2, 3**2,
0.001**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
(.1)**2, (.01)**2,
0.005**2])
maha_test_kinds = [ObservationKind.ORB_FEATURES] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
dim_augment = 7
dim_augment_err = 6
@ -116,20 +113,20 @@ class LocKalman():
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x,y,z = state[0:3,:]
q = state[3:7,:]
v = state[7:10,:]
x, y, z = state[0:3, :]
q = state[3:7, :]
v = state[7:10, :]
vx, vy, vz = v
omega = state[10:13,:]
omega = state[10:13, :]
vroll, vpitch, vyaw = omega
cb, cd = state[13:15,:]
roll_bias, pitch_bias, yaw_bias = state[15:18,:]
odo_scale = state[18,:]
acceleration = state[19:22,:]
focal_scale = state[22,:]
imu_angles= state[23:26,:]
glonass_bias, glonass_freq_slope = state[26:28,:]
ca = state[28,0]
cb, cd = state[13:15, :]
roll_bias, pitch_bias, yaw_bias = state[15:18, :]
odo_scale = state[18, :]
acceleration = state[19:22, :]
focal_scale = state[22, :]
imu_angles = state[23:26, :]
glonass_bias, glonass_freq_slope = state[26:28, :]
ca = state[28, 0]
dt = sp.Symbol('dt')
@ -140,88 +137,84 @@ class LocKalman():
# 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]])
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[:3,:] = v
state_dot[3:7,:] = q_dot
state_dot[7:10,0] = quat_rot * acceleration
state_dot[13,0] = cd
state_dot[14,0] = ca
state_dot[:3, :] = v
state_dot[3:7, :] = q_dot
state_dot[7:10, 0] = quat_rot * acceleration
state_dot[13, 0] = cd
state_dot[14, 0] = ca
# Basic descretization, 1st order intergrator
# 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)
quat_err = state_err[3:6,:]
v_err = state_err[6:9,:]
omega_err = state_err[9:12,:]
cd_err = state_err[13,:]
acceleration_err = state_err[18:21,:]
ca_err = state_err[27,:]
quat_err = state_err[3:6, :]
v_err = state_err[6:9, :]
omega_err = state_err[9:12, :]
cd_err = state_err[13, :]
acceleration_err = state_err[18:21, :]
ca_err = state_err[27, :]
# 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[:3,:] = v_err
state_err_dot[3:6,:] = q_err_dot
state_err_dot[6:9,:] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
state_err_dot[12,:] = cd_err
state_err_dot[13,:] = ca_err
f_err_sym = state_err + dt*state_err_dot
state_err_dot[:3, :] = v_err
state_err_dot[3:6, :] = q_err_dot
state_err_dot[6:9, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
state_err_dot[12, :] = cd_err
state_err_dot[13, :] = ca_err
f_err_sym = state_err + dt * state_err_dot
# convenient indexing
# q idxs are for quats and p idxs are for other
q_idxs = [[3, dim_augment]] + [[dim_main + n*dim_augment + 3, dim_main + (n+1)*dim_augment] for n in range(N)]
q_err_idxs = [[3, dim_augment_err]] + [[dim_main_err + n*dim_augment_err + 3, dim_main_err + (n+1)*dim_augment_err] for n in range(N)]
p_idxs = [[0, 3]] + [[dim_augment, dim_main]] + [[dim_main + n*dim_augment , dim_main + n*dim_augment + 3] for n in range(N)]
p_err_idxs = [[0, 3]] + [[dim_augment_err, dim_main_err]] + [[dim_main_err + n*dim_augment_err, dim_main_err + n*dim_augment_err + 3] for n in range(N)]
q_idxs = [[3, dim_augment]] + [[dim_main + n * dim_augment + 3, dim_main + (n + 1) * dim_augment] for n in range(N)]
q_err_idxs = [[3, dim_augment_err]] + [[dim_main_err + n * dim_augment_err + 3, dim_main_err + (n + 1) * dim_augment_err] for n in range(N)]
p_idxs = [[0, 3]] + [[dim_augment, dim_main]] + [[dim_main + n * dim_augment, dim_main + n * dim_augment + 3] for n in range(N)]
p_err_idxs = [[0, 3]] + [[dim_augment_err, dim_main_err]] + [[dim_main_err + n * dim_augment_err, dim_main_err + n * dim_augment_err + 3] for n in range(N)]
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
H_mod_sym[p_idx[0]:p_idx[1],p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1]-p_idx[0])
H_mod_sym[p_idx[0]:p_idx[1], p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0])
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
H_mod_sym[q_idx[0]:q_idx[1],q_err_idx[0]:q_err_idx[1]] = 0.5*quat_matrix_r(state[q_idx[0]:q_idx[1]])[:,1:]
H_mod_sym[q_idx[0]:q_idx[1], q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(state[q_idx[0]:q_idx[1]])[:, 1:]
# 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)
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)))
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
delta_quat = sp.Matrix(np.ones((4)))
delta_quat[1:,:] = sp.Matrix(0.5*delta_x[q_err_idx[0]: q_err_idx[1],:])
err_function_sym[q_idx[0]:q_idx[1],0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1],0])*delta_quat
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[q_err_idx[0]: q_err_idx[1], :])
err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
err_function_sym[p_idx[0]:p_idx[1],:] = sp.Matrix(nom_x[p_idx[0]:p_idx[1],:] + delta_x[p_err_idx[0]:p_err_idx[1],:])
err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix(nom_x[p_idx[0]:p_idx[1], :] + delta_x[p_err_idx[0]:p_err_idx[1], :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err,1)))
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs):
inv_err_function_sym[p_err_idx[0]:p_err_idx[1],0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1],0] + true_x[p_idx[0]:p_idx[1],0])
inv_err_function_sym[p_err_idx[0]:p_err_idx[1], 0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] + true_x[p_idx[0]:p_idx[1], 0])
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs):
delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1],0]).T*true_x[q_idx[0]:q_idx[1],0]
inv_err_function_sym[q_err_idx[0]:q_err_idx[1],0] = sp.Matrix(2*delta_quat[1:])
delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0]
inv_err_function_sym[q_err_idx[0]:q_err_idx[1], 0] = sp.Matrix(2 * delta_quat[1:])
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]
[inv_err_function_sym, nom_x, true_x],
H_mod_sym, f_err_sym, state_err_sym]
#
# Observation functions
#
@ -238,92 +231,95 @@ class LocKalman():
los_x, los_y, los_z = sat_los_sym
orb_x, orb_y, orb_z = orb_epos_sym
h_pseudorange_sym = sp.Matrix([sp.sqrt(
(x - sat_x)**2 +
(y - sat_y)**2 +
(z - sat_z)**2) +
cb])
h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt(
(x - sat_x)**2 +
(y - sat_y)**2 +
(z - sat_z)**2) +
cb + glonass_bias + glonass_freq_slope*glonass_freq])
h_pseudorange_sym = sp.Matrix([
sp.sqrt(
(x - sat_x)**2 +
(y - sat_y)**2 +
(z - sat_z)**2
) + cb
])
h_pseudorange_glonass_sym = sp.Matrix([
sp.sqrt(
(x - sat_x)**2 +
(y - sat_y)**2 +
(z - sat_z)**2
) + cb + glonass_bias + glonass_freq_slope * glonass_freq
])
los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z]))
los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2)
h_pseudorange_rate_sym = sp.Matrix([los_vector[0]*(sat_vx - vx) +
los_vector[1]*(sat_vy - vy) +
los_vector[2]*(sat_vz - vz) +
cd])
h_pseudorange_rate_sym = sp.Matrix([los_vector[0] * (sat_vx - vx) +
los_vector[1] * (sat_vy - vy) +
los_vector[2] * (sat_vz - vz) +
cd])
imu_rot = euler_rotate(*imu_angles)
h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
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])
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 = sp.sqrt(vx**2 + vy**2 + vz**2)
h_speed_sym = sp.Matrix([speed * odo_scale])
# orb stuff
orb_pos_sym = sp.Matrix([orb_x - x, orb_y - y, orb_z - z])
orb_pos_rot_sym = quat_rot.T * orb_pos_sym
s = orb_pos_rot_sym[0]
h_orb_point_sym = sp.Matrix([(1/s)*(orb_pos_rot_sym[1]),
(1/s)*(orb_pos_rot_sym[2])])
h_orb_point_sym = sp.Matrix([(1 / s) * (orb_pos_rot_sym[1]),
(1 / s) * (orb_pos_rot_sym[2])])
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_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym],
[h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym],
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym],
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym],
[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],
[h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym]]
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym],
[h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym],
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym],
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym],
[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],
[h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym]]
# MSCKF configuration
if N > 0:
focal_scale =1
focal_scale = 1
# Add observation functions for orb feature tracks
track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1)
track_x, track_y, track_z = track_epos_sym
h_track_sym = sp.Matrix(np.zeros(((1 + N)*2, 1)))
h_track_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1)))
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
track_pos_rot_sym = quat_rot.T * track_pos_sym
h_track_sym[-2:,:] = sp.Matrix([focal_scale*(track_pos_rot_sym[1]/track_pos_rot_sym[0]),
focal_scale*(track_pos_rot_sym[2]/track_pos_rot_sym[0])])
h_track_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]),
focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])])
h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N)*3, 1)))
h_msckf_test_sym[-3:,:] = sp.Matrix([track_x - x,track_y - y , track_z - z])
h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N) * 3, 1)))
h_msckf_test_sym[-3:, :] = sp.Matrix([track_x - x, track_y - y, track_z - z])
for n in range(N):
idx = dim_main + n*dim_augment
err_idx = dim_main_err + n*dim_augment_err
x, y, z = state[idx:idx+3]
q = state[idx+3:idx+7]
idx = dim_main + n * dim_augment
err_idx = dim_main_err + n * dim_augment_err # FIXME: Why is this not used?
x, y, z = state[idx:idx + 3]
q = state[idx + 3:idx + 7]
quat_rot = quat_rotate(*q)
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
track_pos_rot_sym = quat_rot.T * track_pos_sym
h_track_sym[n*2:n*2+2,:] = sp.Matrix([focal_scale*(track_pos_rot_sym[1]/track_pos_rot_sym[0]),
focal_scale*(track_pos_rot_sym[2]/track_pos_rot_sym[0])])
h_msckf_test_sym[n*3:n*3+3,:] = sp.Matrix([track_x - x, track_y - y, track_z - z])
h_track_sym[n * 2:n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]),
focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])])
h_msckf_test_sym[n * 3:n * 3 + 3, :] = sp.Matrix([track_x - x, track_y - y, track_z - z])
obs_eqs.append([h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym])
obs_eqs.append([h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym])
obs_eqs.append([h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym])
@ -337,9 +333,9 @@ class LocKalman():
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_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.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**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.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.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
@ -347,8 +343,8 @@ class LocKalman():
self.N = N
self.dim_main = LocKalman.x_initial.shape[0]
self.dim_main_err = LocKalman.P_initial.shape[0]
self.dim_state = self.dim_main + self.dim_augment*self.N
self.dim_state_err = self.dim_main_err + self.dim_augment_err*self.N
self.dim_state = self.dim_main + self.dim_augment * self.N
self.dim_state_err = self.dim_main_err + self.dim_augment_err * self.N
if self.N > 0:
x_initial, P_initial, Q = self.pad_augmented(self.x_initial, self.P_initial, self.Q)
@ -379,15 +375,15 @@ class LocKalman():
def pad_augmented(self, x, P, Q=None):
if x.shape[0] == self.dim_main and self.N > 0:
x = np.pad(x, (0, self.N*self.dim_augment), mode='constant')
x[self.dim_main+3::7] = 1
x = np.pad(x, (0, self.N * self.dim_augment), mode='constant')
x[self.dim_main + 3::7] = 1
if P.shape[0] == self.dim_main_err and self.N > 0:
P = np.pad(P, [(0, self.N*self.dim_augment_err), (0, self.N*self.dim_augment_err)], mode='constant')
P[self.dim_main_err:, self.dim_main_err:] = 10e20*np.eye(self.dim_augment_err *self.N)
P = np.pad(P, [(0, self.N * self.dim_augment_err), (0, self.N * self.dim_augment_err)], mode='constant')
P[self.dim_main_err:, self.dim_main_err:] = 10e20 * np.eye(self.dim_augment_err * self.N)
if Q is None:
return x, P
else:
Q = np.pad(Q, [(0, self.N*self.dim_augment_err), (0, self.N*self.dim_augment_err)], mode='constant')
Q = np.pad(Q, [(0, self.N * self.dim_augment_err), (0, self.N * self.dim_augment_err)], mode='constant')
return x, P, Q
def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
@ -424,15 +420,15 @@ class LocKalman():
else:
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
# Normalize quats
quat_norm = np.linalg.norm(self.filter.x[3:7,0])
quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
# Should not continue if the quats behave this weirdly
if not 0.1 < quat_norm < 10:
raise RuntimeError("Sir! The filter's gone all wobbly!")
self.filter.x[3:7,0] = self.filter.x[3:7,0]/quat_norm
self.filter.x[3:7, 0] = self.filter.x[3:7, 0] / quat_norm
for i in range(self.N):
d1 = self.dim_main
d3 = self.dim_augment
self.filter.x[d1+d3*i+3:d1+d3*i+7] /= np.linalg.norm(self.filter.x[d1+i*d3 + 3:d1+i*d3 + 7,0])
self.filter.x[d1 + d3 * i + 3:d1 + d3 * i + 7] /= np.linalg.norm(self.filter.x[d1 + i * d3 + 3:d1 + i * d3 + 7, 0])
return r
def get_R(self, kind, n):
@ -440,7 +436,7 @@ class LocKalman():
dim = obs_noise.shape[0]
R = np.zeros((n, dim, dim))
for i in range(n):
R[i,:,:] = obs_noise
R[i, :, :] = obs_noise
return R
def predict_and_update_pseudorange(self, meas, t, kind):
@ -449,12 +445,11 @@ class LocKalman():
z = np.zeros((len(meas), 1))
for i, m in enumerate(meas):
z_i, R_i, sat_pos_freq_i = parse_pr(m)
sat_pos_freq[i,:] = sat_pos_freq_i
z[i,:] = z_i
R[i,:,:] = R_i
sat_pos_freq[i, :] = sat_pos_freq_i
z[i, :] = z_i
R[i, :, :] = R_i
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_freq)
def predict_and_update_pseudorange_rate(self, meas, t, kind):
R = np.zeros((len(meas), 1, 1))
z = np.zeros((len(meas), 1))
@ -462,61 +457,63 @@ class LocKalman():
for i, m in enumerate(meas):
z_i, R_i, sat_pos_vel_i = parse_prr(m)
sat_pos_vel[i] = sat_pos_vel_i
R[i,:,:] = R_i
R[i, :, :] = R_i
z[i, :] = z_i
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel)
def predict_and_update_orb(self, orb, t, kind):
true_pos = orb[:,2:]
z = orb[:,:2]
true_pos = orb[:, 2:]
z = orb[:, :2]
R = np.zeros((len(orb), 2, 2))
for i, _ in enumerate(z):
R[i,:,:] = np.diag([10**2, 10**2])
R[i, :, :] = np.diag([10**2, 10**2])
return self.filter.predict_and_update_batch(t, kind, z, R, true_pos)
def predict_and_update_odo_speed(self, speed, t, kind):
z = np.array(speed)
R = np.zeros((len(speed), 1, 1))
for i, _ in enumerate(z):
R[i,:,:] = np.diag([0.2**2])
R[i, :, :] = np.diag([0.2**2])
return self.filter.predict_and_update_batch(t, kind, z, R)
def predict_and_update_odo_trans(self, trans, t, kind):
z = trans[:,:3]
z = trans[:, :3]
R = np.zeros((len(trans), 3, 3))
for i, _ in enumerate(z):
R[i,:,:] = np.diag(trans[i,3:]**2)
R[i, :, :] = np.diag(trans[i, 3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)
def predict_and_update_odo_rot(self, rot, t, kind):
z = rot[:,:3]
z = rot[:, :3]
R = np.zeros((len(rot), 3, 3))
for i, _ in enumerate(z):
R[i,:,:] = np.diag(rot[i,3:]**2)
R[i, :, :] = np.diag(rot[i, 3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)
def predict_and_update_orb_features(self, tracks, t, kind):
k = 2*(self.N+1)
k = 2 * (self.N + 1)
R = np.zeros((len(tracks), k, k))
z = np.zeros((len(tracks), k))
ecef_pos = np.zeros((len(tracks), 3))
ecef_pos[:] = np.nan
poses = self.x[self.dim_main:].reshape((-1,7))
times = tracks.reshape((len(tracks),self.N+1, 4))[:,:,0]
poses = self.x[self.dim_main:].reshape((-1, 7))
times = tracks.reshape((len(tracks), self.N + 1, 4))[:, :, 0]
good_counter = 0
if times.any() and np.allclose(times[0,:-1], self.filter.augment_times, rtol=1e-6):
if times.any() and np.allclose(times[0, :-1], self.filter.augment_times, rtol=1e-6):
for i, track in enumerate(tracks):
img_positions = track.reshape((self.N+1, 4))[:,2:]
img_positions = track.reshape((self.N + 1, 4))[:, 2:]
# TODO not perfect as last pose not used
#img_positions = unroll_shutter(img_positions, poses, self.filter.state()[7:10], self.filter.state()[10:13], ecef_pos[i])
# img_positions = unroll_shutter(img_positions, poses, self.filter.state()[7:10], self.filter.state()[10:13], ecef_pos[i])
ecef_pos[i] = self.computer.compute_pos(poses, img_positions[:-1])
z[i] = img_positions.flatten()
R[i,:,:] = np.diag([0.005**2]*(k))
R[i, :, :] = np.diag([0.005**2] * (k))
if np.isfinite(ecef_pos[i][0]):
good_counter += 1
if good_counter > self.max_tracks:
break
good_idxs = np.all(np.isfinite(ecef_pos),axis=1)
good_idxs = np.all(np.isfinite(ecef_pos), axis=1)
# have to do some weird stuff here to keep
# to have the observations input from mesh3d
# consistent with the outputs of the filter
@ -524,8 +521,8 @@ class LocKalman():
ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], augment=True)
if ret is None:
return
y_full = np.zeros((z.shape[0], z.shape[1] - 3))
#print sum(good_idxs), len(tracks)
if sum(good_idxs) > 0:
y_full[good_idxs] = np.array(ret[6])
ret = ret[:6] + (y_full, z, ecef_pos)
@ -537,7 +534,7 @@ class LocKalman():
R = np.zeros((len(test_data), len(z[0]), len(z[0])))
ecef_pos = [self.x[:3]]
for i, _ in enumerate(z):
R[i,:,:] = np.diag([0.1**2]*len(z[0]))
R[i, :, :] = np.diag([0.1**2] * len(z[0]))
ret = self.filter.predict_and_update_batch(t, kind, z, R, ecef_pos)
self.filter.augment()
return ret

Write Preview
Loading…
Cancel
Save