def right_phi_inv(cls, state, hat_state):
"""Inverse retraction.
.. math::
\\varphi^{-1}_{\\boldsymbol{\\hat{\\chi}}}
\\left(\\boldsymbol{\\chi}\\right) = \\left( \\begin{matrix}
\\log\\left( \\boldsymbol{\\hat{\\chi}}^{-1}
\\boldsymbol{\\chi} \\right) \\\\
\\mathbf{b}_g - \\mathbf{\\hat{b}}_g \\\\
\\mathbf{b}_a - \\mathbf{\\hat{b}}_a
\\end{matrix} \\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SE_2(3)
\\times \\mathbb{R}^6` with right multiplication.
Its corresponding retraction is :meth:`~ukfm.IMUGNSS.right_phi`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var hat_state: noise-free state :math:`\\boldsymbol{\hat{\\chi}}`.
"""
dR = hat_state.Rot.dot(state.Rot.T)
phi = SO3.log(dR)
J = SO3.inv_left_jacobian(phi)
dv = hat_state.v - dR * state.v
dp = hat_state.p - dR * state.p
xi = np.hstack([
phi,
J.dot(dv),
J.dot(dp), hat_state.b_gyro - state.b_gyro,
hat_state.b_acc - state.b_acc
])
return xi
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 get_states(cls, states, N):
Rots = np.zeros((N, 3, 3))
rpys = np.zeros((N, 3))
for n in range(N):
Rots[n] = states[n].Rot
rpys[n] = SO3.to_rpy(states[n].Rot)
return Rots, rpys
def f(cls, state, omega, w, dt):
""" Propagation function.
.. math::
\\mathbf{C}_{n+1} &= \\mathbf{C}_{n} \\exp\\left(\\left(\\mathbf{u}
+ \\mathbf{w}^{(0:3)} \\right) dt\\right), \\\\
\\mathbf{v}_{n+1} &= \\mathbf{v}_{n} + \\mathbf{a} dt, \\\\
\\mathbf{p}_{n+1} &= \\mathbf{p}_{n} + \\mathbf{v}_{n} dt +
\mathbf{a} dt^2/2,
where
.. math::
\\mathbf{a} = \\mathbf{C}_{n} \\left( \\mathbf{a}_b +
\\mathbf{w}^{(3:6)} \\right) + \\mathbf{g}
:var state: state :math:`\\boldsymbol{\\chi}`.
:var omega: input :math:`\\boldsymbol{\\omega}`.
:var w: noise :math:`\\mathbf{w}`.
:var dt: integration step :math:`dt` (s).
"""
acc = state.Rot.dot(omega.acc + w[3:6]) + cls.g
new_state = cls.STATE(Rot=state.Rot.dot(
SO3.exp((omega.gyro + w[:3]) * dt)),
v=state.v + acc * dt,
p=state.p + state.v * dt + 1 / 2 * acc * dt**2)
return new_state
def phi(cls, state, xi):
"""Retraction.
.. math::
\\varphi\\left(\\boldsymbol{\\chi}, \\boldsymbol{\\xi}\\right) =
\\left( \\begin{matrix}
\\exp\\left(\\boldsymbol{\\xi}^{(0:3)}\\right) \\mathbf{C} \\\\
\\mathbf{u} + \\boldsymbol{\\xi}^{(3:6)}
\\end{matrix} \\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(3)
\\times \\mathbb R^3`.
Its corresponding inverse operation is :meth:`~ukfm.PENDULUM.phi_inv`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var xi: state uncertainty :math:`\\boldsymbol{\\xi}`.
"""
new_state = cls.STATE(
Rot=state.Rot.dot(SO3.exp(xi[:3])),
u=state.u + xi[3:6],
)
return new_state
def errors(cls, Rots, hat_Rots):
N = Rots.shape[0]
errors = np.zeros((N, 3))
# get true states and estimates, and orientation error
for n in range(N):
errors[n] = SO3.log(Rots[n].dot(hat_Rots[n].T))
return errors
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 phi_inv(cls, state, hat_state):
"""Inverse retraction.
.. math::
\\varphi^{-1}_{\\boldsymbol{\\hat{\\chi}}}
\\left(\\boldsymbol{\\chi}\\right) = \\left( \\begin{matrix}
\\log\\left(\\mathbf{C} \\mathbf{\\hat{C}}^T \\right)\\\\
\\mathbf{v} - \\mathbf{\\hat{v}} \\\\
\\mathbf{p} - \\mathbf{\\hat{p}} \\\\
\\mathbf{b}_g - \\mathbf{\\hat{b}}_g \\\\
\\mathbf{b}_a - \\mathbf{\\hat{b}}_a
\\end{matrix} \\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(3)
\\times \\mathbb R^{15}`.
Its corresponding retraction is :meth:`~ukfm.IMUGNSS.phi`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var hat_state: noise-free state :math:`\\boldsymbol{\hat{\\chi}}`.
"""
xi = np.hstack([
SO3.log(hat_state.Rot.dot(state.Rot.T)), hat_state.v - state.v,
hat_state.p - state.p, hat_state.b_gyro - state.b_gyro,
hat_state.b_acc - state.b_acc
])
return xi
def errors(self, Rots, vs, ps, hat_Rots, hat_vs, hat_ps):
errors = np.zeros((self.N, 9))
for n in range(self.N):
errors[n, :3] = SO3.log(Rots[n].T.dot(hat_Rots[n]))
errors[:, 3:6] = vs - hat_vs
errors[:, 6:9] = ps - hat_ps
return errors
def phi_inv(cls, state, hat_state):
"""Inverse retraction.
.. math::
\\varphi^{-1}_{\\boldsymbol{\\hat{\\chi}}}\\left(\\boldsymbol{\\chi}
\\right) = \\left( \\begin{matrix}
\\log\\left(\\mathbf{C} \\mathbf{\\hat{C}}^T \\right)\\\\
\\mathbf{v} - \\mathbf{\\hat{v}} \\\\
\\mathbf{p} - \\mathbf{\\hat{p}}
\\end{matrix} \\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(3)
\\times \\mathbb R^6`.
Its corresponding retraction is :meth:`~ukfm.INERTIAL_NAVIGATION.phi`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var hat_state: noise-free state :math:`\\boldsymbol{\hat{\\chi}}`.
"""
xi = np.hstack([
SO3.log(state.Rot.dot(hat_state.Rot.T)), state.v - hat_state.v,
state.p - hat_state.p
])
return xi
def phi(cls, state, xi):
"""Retraction.
.. math::
\\varphi\\left(\\boldsymbol{\\chi}, \\boldsymbol{\\xi}\\right) =
\\left( \\begin{matrix}
\\mathbf{C} \\exp\\left(\\boldsymbol{\\xi}^{(0:3)}\\right) \\\\
\\mathbf{v} + \\boldsymbol{\\xi}^{(3:6)} \\\\
\\mathbf{p} + \\boldsymbol{\\xi}^{(6:9)}
\\end{matrix} \\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(3)
\\times \\mathbb R^6`.
Its corresponding inverse operation is
:meth:`~ukfm.INERTIAL_NAVIGATION.phi_inv`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var xi: state uncertainty :math:`\\boldsymbol{\\xi}`.
"""
new_state = cls.STATE(Rot=SO3.exp(xi[:3]).dot(state.Rot),
v=state.v + xi[3:6],
p=state.p + xi[6:9])
return new_state
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 right_phi(cls, state, xi):
"""Retraction.
.. math::
\\varphi\\left(\\boldsymbol{\\chi}, \\boldsymbol{\\xi}\\right)
= \\left( \\begin{matrix}
\\mathbf{C}_\\mathbf{T} \\mathbf{C} \\\\
\\mathbf{C}_\\mathbf{T}\\mathbf{v} + \\mathbf{r_1} \\\\
\\mathbf{C}_\\mathbf{T}\\mathbf{p} + \\mathbf{r_2} \\\\
\\mathbf{b}_g + \\boldsymbol{\\xi}^{(9:12)} \\\\
\\mathbf{b}_a + \\boldsymbol{\\xi}^{(12:15)}
\\end{matrix} \\right)
where
.. math::
\\mathbf{T} = \\exp\\left(\\boldsymbol{\\xi}^{(0:9)}\\right)
= \\begin{bmatrix}
\\mathbf{C}_\\mathbf{T} & \\mathbf{r_1} &\\mathbf{r}_2 \\\\
\\mathbf{0}^T & & \\mathbf{I}
\\end{bmatrix}
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SE_2(3)
\\times \\mathbb{R}^6` with right multiplication.
Its corresponding inverse operation is
:meth:`~ukfm.IMUGNSS.right_phi_inv`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var xi: state uncertainty :math:`\\boldsymbol{\\xi}`.
"""
dR = SO3.exp(xi[:3])
J = SO3.left_jacobian(xi[:3])
new_state = cls.STATE(Rot=dR.dot(state.Rot),
v=dR.dot(state.v) + J.dot(xi[3:6]),
p=dR.dot(state.p) + J.dot(xi[6:9]),
b_gyro=state.b_gyro + xi[9:12],
b_acc=state.b_acc + xi[12:15])
return new_state
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 f(cls, state, omega, w, dt):
""" Propagation function.
.. math::
\\mathbf{C}_{n+1} = \\mathbf{C}_{n}
\\exp\\left(\\left(\\mathbf{u} + \\mathbf{w} \\right)
dt \\right)
:var state: state :math:`\\boldsymbol{\\chi}`.
:var omega: input :math:`\\boldsymbol{\\omega}`.
:var w: noise :math:`\\mathbf{w}`.
:var dt: integration step :math:`dt` (s).
"""
new_state = cls.STATE(
Rot=state.Rot.dot(SO3.exp((omega.gyro + w)*dt)),
)
return new_state
def simu_f(self, model_std):
# set noise to zero to compute true trajectory
w = np.zeros(6)
# init variables at zero and do for loop
omegas = []
Rot0 = SO3.from_rpy(57.3/180*np.pi, 40/180*np.pi, 0)
states = [self.STATE(Rot0, np.array([-10/180*np.pi,
30/180*np.pi, 0]))]
for n in range(1, self.N):
# true input
omegas.append(self.INPUT())
# propagate state
w[:3] = model_std[0]*np.random.randn(3)
w[3:] = model_std[1]*np.random.randn(3)
states.append(self.f(states[n-1], omegas[n-1], w, self.dt))
return states, omegas
def right_phi_inv(cls, state, hat_state):
"""Inverse retraction.
.. math::
\\varphi^{-1}_{\\boldsymbol{\\hat{\\chi}}}\\left(\\boldsymbol{\\chi}
\\right) = \\log\\left(
\\boldsymbol{\\hat{\\chi}}\\boldsymbol{\chi}^{-1} \\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(3)`
with right multiplication.
Its corresponding retraction is :meth:`~ukfm.ATTITUDE.right_phi`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var hat_state: noise-free state :math:`\\boldsymbol{\hat{\\chi}}`.
"""
xi = SO3.log(hat_state.Rot.dot(state.Rot.T))
return xi
def phi(cls, state, xi):
"""Retraction.
.. math::
\\varphi\\left(\\boldsymbol{\\chi}, \\boldsymbol{\\xi}\\right) =
\\mathbf{C} \\exp\\left(\\boldsymbol{\\xi}\\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(3)`
with left multiplication.
Its corresponding inverse operation is :meth:`~ukfm.ATTITUDE.phi_inv`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var xi: state uncertainty :math:`\\boldsymbol{\\xi}`.
"""
new_state = cls.STATE(
Rot=state.Rot.dot(SO3.exp(xi))
)
return new_state
def f(cls, state, omega, w, dt):
""" Propagation function.
.. math::
\\mathbf{C}_{n+1} &= \\mathbf{C}_{n} \\exp\\left(\\left(\\mathbf{u}
- \mathbf{b}_g + \\mathbf{w}^{(0:3)} \\right) dt\\right) \\\\
\\mathbf{v}_{n+1} &= \\mathbf{v}_{n} + \\mathbf{a} dt, \\\\
\\mathbf{p}_{n+1} &= \\mathbf{p}_{n} + \\mathbf{v}_{n} dt
+ \mathbf{a} dt^2/2 \\\\
\\mathbf{b}_{g,n+1} &= \\mathbf{b}_{g,n}
+ \\mathbf{w}^{(6:9)}dt \\\\
\\mathbf{b}_{a,n+1} &= \\mathbf{b}_{a,n} +
\\mathbf{w}^{(9:12)} dt
where
.. math::
\\mathbf{a} = \\mathbf{C}_{n}
\\left( \\mathbf{a}_b -\mathbf{b}_a
+ \\mathbf{w}^{(3:6)} \\right) + \\mathbf{g}
Ramdom-walk noises on biases are not added as the Jacobian w.r.t. these
noise are trivial. This spares some computations of the UKF.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var omega: input :math:`\\boldsymbol{\\omega}`.
:var w: noise :math:`\\mathbf{w}`.
:var dt: integration step :math:`dt` (s).
"""
gyro = omega.gyro - state.b_gyro + w[:3]
acc = state.Rot.dot(omega.acc - state.b_acc + w[3:6]) + cls.g
new_state = cls.STATE(
Rot=state.Rot.dot(SO3.exp(gyro * dt)),
v=state.v + acc * dt,
p=state.p + state.v * dt + 1 / 2 * acc * dt**2,
# noise is not added on biases
b_gyro=state.b_gyro,
b_acc=state.b_acc)
return new_state
def phi_inv(cls, state, hat_state):
"""Inverse retraction.
.. math::
\\varphi^{-1}_{\\boldsymbol{\\hat{\\chi}}}\\left(\\boldsymbol{\\chi}
\\right) = \\left( \\begin{matrix}
\\log\\left(\\mathbf{\\hat{C}}^T \\mathbf{C} \\right)\\\\
\\mathbf{u} - \\mathbf{\\hat{u}}
\\end{matrix} \\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(3)
\\times \\mathbb R^3`.
Its corresponding retraction is :meth:`~ukfm.PENDULUM.phi`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var hat_state: noise-free state :math:`\\boldsymbol{\hat{\\chi}}`.
"""
xi = np.hstack([SO3.log(hat_state.Rot.T.dot(state.Rot)),
state.u - hat_state.u])
return xi
def f(cls, state, omega, w, dt):
""" Propagation function.
.. math::
\\mathbf{C}_{n+1} &= \\mathbf{C}_{n} \\exp\\left(\\left(\\mathbf{u}
+ \\mathbf{w}^{(0:3)} \\right) dt\\right), \\\\
\\mathbf{u}_{n+1} &= \\mathbf{u}_{n} + \\dot{\\mathbf{u}} dt,
where
.. math::
\\dot{\\mathbf{u}} = \\begin{bmatrix}
-\\omega_y \\omega_x\\ \\\\ \\omega_x \\omega_z
\\\\ 0 \end{bmatrix} +
\\frac{g}{l} \\left(\\mathbf{e}^b \\right)^\\wedge
\\mathbf{C}^T \\mathbf{e}^b + \\mathbf{w}^{(3:6)}
:var state: state :math:`\\boldsymbol{\\chi}`.
:var omega: input :math:`\\boldsymbol{\\omega}`.
:var w: noise :math:`\\mathbf{w}`.
:var dt: integration step :math:`dt` (s).
"""
e3_i = state.Rot.T.dot(cls.e3)
u = state.u
d_u = np.array([-u[1]*u[2], u[0]*u[2], 0]) + \
cls.g/cls.L*np.cross(cls.e3, e3_i)
new_state = cls.STATE(
Rot=state.Rot.dot(SO3.exp((u+w[:3])*dt)),
u=state.u + (d_u+w[3:6])*dt
)
return new_state
ekf_nees = np.zeros_like(ukf_nees)
################################################################################
# Monte-Carlo Runs
# ==============================================================================
# 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,
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
