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)