def test_F(self):
w_hat = np.array([1.0, 2.0, 3.0])
q_hat = np.array([0.0, 0.0, 0.0, 1.0])
a_hat = np.array([1.0, 2.0, 3.0])
w_G = np.array([0.1, 0.1, 0.1])
F = self.imu_state.F(w_hat, q_hat, a_hat, w_G)
# -- First row --
self.assertTrue(np_equal(F[0:3, 0:3], -skew(w_hat)))
self.assertTrue(np_equal(F[0:3, 3:6], -np.eye(3)))
# -- Third Row --
self.assertTrue(np_equal(F[6:9, 0:3], dot(-C(q_hat).T, skew(a_hat))))
self.assertTrue(np_equal(F[6:9, 6:9], -2.0 * skew(w_G)))
self.assertTrue(np_equal(F[6:9, 9:12], -C(q_hat).T))
self.assertTrue(np_equal(F[6:9, 12:15], -skewsq(w_G)))
# -- Fifth Row --
# self.assertTrue(np_equal(F[12:15, 6:9], np.eye(3)))
# Plot matrix
# debug = True
debug = False
if debug:
ax = plt.subplot(111)
ax.matshow(F)
plt.show()
def test_J(self):
# Setup
cam_q_CI = np.array([0.0, 0.0, 0.0, 1.0])
cam_p_IC = np.array([1.0, 1.0, 1.0])
q_hat_IG = np.array([0.0, 0.0, 0.0, 1.0])
N = 1
J = self.imu_state.J(cam_q_CI, cam_p_IC, q_hat_IG, N)
# Assert
C_CI = C(cam_q_CI)
C_IG = C(q_hat_IG)
# -- First row --
self.assertTrue(np_equal(J[0:3, 0:3], C_CI))
# -- Second row --
self.assertTrue(np_equal(J[3:6, 0:3], skew(dot(C_IG.T, cam_p_IC))))
# -- Third row --
self.assertTrue(np_equal(J[3:6, 12:15], I(3)))
# Plot matrix
# debug = True
debug = False
if debug:
ax = plt.subplot(111)
ax.matshow(J)
plt.show()
def quatrcomp(q):
"""Quaternion right compound
Source:
Page 4.
Trawny, Nikolas, and Stergios I. Roumeliotis. "Indirect Kalman filter
for 3D attitude estimation." University of Minnesota, Dept. of Comp.
Sci. & Eng., Tech. Rep 2 (2005): 2005.
Parameters
----------
q : np.array - 4x1
Quaternion (x, y, z, w)
Returns
-------
"""
q1, q2, q3, q4 = q.ravel()
vector = np.array([[q1], [q2], [q3]])
scalar = q4
return np.block([[scalar * np.eye(3) + skew(vector), vector],
[-vector.T, scalar]])
def J(self, cam_q_CI, cam_p_IC, q_hat_IG, N):
"""Jacobian J matrix
Parameters
----------
cam_q_CI : np.array
Rotation from IMU to camera frame
in quaternion (x, y, z, w)
cam_p_IC : np.array
Position of camera in IMU frame
q_hat_IG : np.array
Rotation from global to IMU frame
N : float
Number of camera states
Returns
-------
J: np.array
Jacobian matrix J
"""
C_CI = C(cam_q_CI)
C_IG = C(q_hat_IG)
J = zeros((6, 15 + 6 * N))
# -- First row --
J[0:3, 0:3] = C_CI
# -- Second row --
J[3:6, 0:3] = skew(dot(C_IG.T, cam_p_IC))
J[3:6, 12:15] = I(3)
return J
def test_skew(self):
v = np.array([[1.0], [2.0], [3.0]])
X = skew(v)
X_expected = np.array([[0.0, -3.0, 2.0],
[3.0, 0.0, -1.0],
[-2.0, 1.0, 0.0]])
self.assertTrue(np.array_equal(X, X_expected))
def F(self, w_hat, q_hat, a_hat, w_G):
"""Transition Jacobian F matrix
Predicition or transition matrix in an EKF
Parameters
----------
w_hat : np.array
Estimated angular velocity
q_hat : np.array
Estimated quaternion (x, y, z, w)
a_hat : np.array
Estimated acceleration
w_G : np.array
Earth's angular velocity (i.e. Earth's rotation)
Returns
-------
F : np.array - 15x15
Transition jacobian matrix F
"""
# F matrix
F = zeros((15, 15))
# -- First row --
F[0:3, 0:3] = -skew(w_hat)
F[0:3, 3:6] = -I(3)
# -- Third Row --
F[6:9, 0:3] = -dot(C(q_hat).T, skew(a_hat))
F[6:9, 6:9] = -2.0 * skew(w_G)
F[6:9, 9:12] = -C(q_hat).T
F[6:9, 12:15] = -skewsq(w_G)
# -- Fifth Row --
F[12:15, 6:9] = np.ones(3)
return F
def update(self, a_m, w_m, dt):
"""IMU state update
Parameters
----------
a_m : np.array
Accelerometer measurement
w_m : np.array
Gyroscope measurement
dt : float
Time difference (s)
Returns
-------
a: np.array
Corrected accelerometer measurement
w: np.array
Corrected gyroscope measurement
"""
# Calculate new accel and gyro estimates
# a_hat = (a_m - self.b_a)
# w_hat = (w_m - self.b_g - dot(C(self.q_IG), self.w_G))
a_hat = a_m
w_hat = w_m
# Build the jacobians F and G
F = self.F(w_hat, self.q_IG, a_hat, self.w_G)
G = self.G(self.q_IG)
# Propagate IMU states
# -- Orientation
self.q_IG += dot(0.5 * Omega(w_hat), self.q_IG) * dt
# self.q_IG = quatnormalize(self.q_IG)
# -- Velocity
self.v_G += (dot(C(self.q_IG).T, a_hat) -
2 * dot(skew(self.w_G), self.v_G) -
dot(skewsq(self.w_G), self.p_G)) * dt
# self.v_G = self.v_G + (dot(C(self.q_IG).T, a_hat) - 2 * dot(skew(self.w_G), self.v_G) - dot(skewsq(self.w_G), self.p_G) + self.g_G) * dt # NOQA
# -- Position
self.p_G += self.v_G * dt
# Update covariance
Phi = I(self.size) + F * dt
self.P = dot(Phi, dot(self.P, Phi.T)) + dot(G, dot(self.Q, G.T)) * dt
self.P = enforce_psd(self.P)
return self.P, Phi
def Omega(w):
"""Omega function
Parameters
----------
w : np.array
Angular velocity
Returns
-------
Differential form of an angular velocity (np.array)
"""
w = w.reshape((3, 1))
return np.block([[-skew(w), w], [-w.T, 0.0]])
def H(self, track, track_cam_states, p_G_f):
"""Form the Jacobian measurement matrix
- $H^{(j)}_x$ of j-th feature track with respect to state $X$
- $H^{(j)}_f$ of j-th feature track with respect to feature position
Parameters
----------
track : FeatureTrack
Feature track of length M
track_cam_states : list of CameraState
N Camera states
p_G_f : np.array
Feature position in global frame
Returns
-------
H_x_j: np.matrix
Measurement jacobian matrix w.r.t state (size: 2*M x (15+6)N)
"""
x_imu_size = self.imu_state.size # Size of imu state
x_cam_size = self.cam_states[0].size # Size of cam state
N = self.N() # Number of camera states
M = track.tracked_length() # Length of feature track
# Measurement jacobian w.r.t feature
H_f_j = zeros((2 * M, 3))
# Measurement jacobian w.r.t state
H_x_j = zeros((2 * M, x_imu_size + x_cam_size * N))
# Pose index
pose_idx = track.frame_start - self.cam_states[0].frame_id
assert track_cam_states[0].frame_id == track.frame_start
assert track_cam_states[-1].frame_id == track.frame_end
# Form measurement jacobians
for i in range(M):
# Feature position in camera frame
C_CG = C(track_cam_states[i].q_CG)
p_G_C = track_cam_states[i].p_G
p_C_f = dot(C_CG, (p_G_f - p_G_C))
X, Y, Z = p_C_f.ravel()
# dh / dg
dhdg = (1.0 / Z) * np.array([[1.0, 0.0, -X / Z],
[0.0, 1.0, -Y / Z]])
# Row start and end index
rs = 2 * i
re = 2 * i + 2
# Column start and end index
cs_dhdq = x_imu_size + (x_cam_size * pose_idx)
ce_dhdq = x_imu_size + (x_cam_size * pose_idx) + 3
cs_dhdp = x_imu_size + (x_cam_size * pose_idx) + 3
ce_dhdp = x_imu_size + (x_cam_size * pose_idx) + 6
# H_f_j measurement jacobian w.r.t feature
H_f_j[rs:re, :] = dot(dhdg, C_CG)
# H_x_j measurement jacobian w.r.t state
H_x_j[rs:re, cs_dhdq:ce_dhdq] = dot(dhdg, skew(p_C_f))
H_x_j[rs:re, cs_dhdp:ce_dhdp] = dot(-dhdg, C_CG)
# Update pose_idx
pose_idx += 1
return H_f_j, H_x_j