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 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}
\\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 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 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 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 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,
