def nees(self, err, Ps, Rots, vs, ps, name):
J = np.eye(9)
neess = np.zeros((self.N, 2))
def err2nees(err, P):
# separate orientation and position
nees_Rot = err[:3].dot(np.linalg.inv(P[:3, :3]).dot(err[:3])) / 3
nees_p = err[6:9].dot(np.linalg.inv(P[6:9, 6:9]).dot(err[6:9])) / 3
return np.array([nees_Rot, nees_p])
for n in range(1, self.N):
# covariance need to be turned
if name == 'STD':
P = Ps[n]
elif name == 'LEFT':
J[3:6, 3:6] = Rots[n]
J[6:9, 6:9] = Rots[n]
P = J.dot(Ps[n]).dot(J.T)
else:
J[3:6, :3] = SO3.wedge(vs[n])
J[6:9, :3] = SO3.wedge(ps[n])
P = J.dot(Ps[n]).dot(J.T)
neess[n] = err2nees(err[n], P)
return neess
def iekf_FG_ana(cls, state, omega, dt):
F = np.eye(9)
F[3:6, :3] = SO3.wedge(cls.g) * dt
F[6:9, :3] = F[3:6, :3] * dt / 2
F[6:9, 3:6] = dt * np.eye(3)
G = np.zeros((9, 6))
G[:3, :3] = state.Rot * dt
G[3:6, 3:6] = state.Rot * dt
G[3:6, :3] = SO3.wedge(state.v).dot(state.Rot) * dt
G[6:9, :3] = SO3.wedge(state.p).dot(state.Rot) * dt
return F, G
def ekf_H_ana(cls, state):
H = np.zeros((3 * cls.N_ldk, 9))
for i in range(cls.N_ldk):
H[3 * i:3 * (i + 1), :3] = state.Rot.T.dot(
SO3.wedge(cls.ldks[i] - state.p))
H[3 * i:3 * (i + 1), 6:9] = -state.Rot.T
return H
def ekf_FG_ana(cls, state, omega, dt):
F = np.eye(9)
F[3:6, :3] = -SO3.wedge(state.Rot.dot(omega.acc) * dt)
F[6:9, :3] = F[3:6, :3] * dt / 2
F[6:9, 3:6] = dt * np.eye(3)
G = np.zeros((9, 6))
G[:3, :3] = state.Rot * dt
G[3:6, 3:6] = state.Rot * dt
return F, G
def plot_results(self, hat_states, hat_Ps, states):
Rots, us = self.get_states(states, self.N)
hat_Rots, hat_us = self.get_states(hat_states, self.N)
t = np.linspace(0, self.T, self.N)
ps = np.zeros((self.N, 3))
ukf3sigma = np.zeros((self.N, 3))
hat_ps = np.zeros_like(ps)
A = np.eye(6)
e3wedge = SO3.wedge(self.L*self.e3)
for n in range(self.N):
ps[n] = self.L*Rots[n].dot(self.e3)
hat_ps[n] = self.L*hat_Rots[n].dot(self.e3)
A[:3, :3] = hat_Rots[n].dot(e3wedge)
P = A.dot(hat_Ps[n]).dot(A.T)
ukf3sigma[n] = np.diag(P[:3, :3])
errors = np.linalg.norm(ps - hat_ps, axis=1)
ukf3sigma = 3*np.sqrt(np.sum(ukf3sigma, 1))
fig, ax = plt.subplots(figsize=(10, 6))
ax.set(xlabel='$t$ (s)', ylabel='position (m)', title='position (m)')
plt.plot(t, ps, linewidth=2)
plt.plot(t, hat_ps)
ax.legend([r'$x$', r'$y$', r'$z$', r'$x$ UKF', r'$y$ UKF', r'$z$ UKF'])
ax.set_xlim(0, t[-1])
fig, ax = plt.subplots(figsize=(10, 6))
ax.set(xlabel='$x$ (m)', ylabel='$y$ (m)',
title='position in $xs$ plan')
plt.plot(ps[:, 0], ps[:, 1], linewidth=2, c='black')
plt.plot(hat_ps[:, 0], hat_ps[:, 1], c='blue')
ax.legend([r'true position', r'UKF'])
ax.axis('equal')
fig, ax = plt.subplots(figsize=(10, 6))
ax.set(xlabel='$y$ (m)', ylabel='$z$ (m)',
title='position in $yz$ plan')
plt.plot(ps[:, 1], ps[:, 2], linewidth=2, c='black')
plt.plot(hat_ps[:, 1], hat_ps[:, 2], c='blue')
ax.legend([r'true position', r'UKF'])
ax.axis('equal')
fig, ax = plt.subplots(figsize=(10, 6))
ax.set(xlabel='$t$ (s)', ylabel='error (m)',
title=' position error (m)')
plt.plot(t, errors, c='blue')
plt.plot(t, ukf3sigma, c='blue', linestyle='dashed')
ax.legend([r'UKF', r'$3\sigma$ UKF'])
ax.set_xlim(0, t[-1])
ax.set_ylim(0, 0.5)
def ekf_H_ana(cls, state):
H = np.vstack([state.Rot.T.dot(SO3.wedge(cls.g)),
state.Rot.T.dot(SO3.wedge(cls.b))])
return H
# We run the Monte-Carlo through a for loop.
for n_mc in range(N_mc):
print("Monte-Carlo iteration(s): " + str(n_mc+1) + "/" + str(N_mc))
# simulate true states and noisy inputs
states, omegas = model.simu_f(imu_std)
# simulate measurements
ys, one_hot_ys = model.simu_h(states, obs_freq, obs_std)
# initialize filters
state0 = model.STATE(
Rot=states[0].Rot.dot(SO3.exp(Rot0_std*np.random.randn(3))),
v=states[0].v,
p=states[0].p + p0_std*np.random.randn(3))
# IEKF and right UKF covariance need to be turned
J = np.eye(9)
J[6:9, :3] = SO3.wedge(state0.p)
right_P0 = J.dot(P0).dot(J.T)
ukf = UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,
phi=model.phi, phi_inv=model.phi_inv, alpha=alpha)
left_ukf = UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,
phi=model.left_phi, phi_inv=model.left_phi_inv, alpha=alpha)
right_ukf = UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,
phi=model.right_phi, phi_inv=model.right_phi_inv,
alpha=alpha)
iekf = EKF(model=model, state0=state0, P0=right_P0, Q=Q, R=R,
FG_ana=model.iekf_FG_ana, H_ana=model.iekf_H_ana,
phi=model.right_phi)
ekf = EKF(model=model, state0=state0, P0=right_P0, Q=Q, R=R,
FG_ana=model.ekf_FG_ana, H_ana=model.ekf_H_ana,
phi=model.phi)
