def phi(cls, state, xi):
"""Retraction.
.. math::
\\varphi\\left(\\boldsymbol{\\chi}, \\boldsymbol{\\xi}\\right) =
\\left( \\begin{matrix}
\\mathbf{C} \\exp\\left(\\boldsymbol{\\xi}^{(0)}\\right) \\\\
\\mathbf{p} + \\boldsymbol{\\xi}^{(1:3)} \\\\
\\mathbf{p}_1^l + \\boldsymbol{\\xi}^{(3:5)} \\\\
\\vdots \\\\
\\mathbf{p}_L^l + \\boldsymbol{\\xi}^{(3+2L:5+2L)}
\\end{matrix} \\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(2)
\\times \\mathbb R^{2(L+1)}`.
Its corresponding inverse operation (for robot state only) is
:meth:`~ukfm.SLAM2D.red_phi_inv`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var xi: state uncertainty :math:`\\boldsymbol{\\xi}`.
"""
k = int(xi[3:].shape[0] / 2)
p_ls = state.p_l + np.reshape(xi[3:], (k, 2))
new_state = cls.STATE(Rot=state.Rot.dot(SO2.exp(xi[0])),
p=state.p + xi[1:3],
p_l=p_ls)
return new_state
def aug_phi(cls, state, xi):
"""Retraction used for augmenting state.
The retraction :meth:`~ukfm.SLAM2D.phi` applied on the robot state only.
"""
new_state = cls.STATE(Rot=state.Rot.dot(SO2.exp(xi[0])),
p=state.p + xi[1:])
return new_state
def red_phi(cls, state, xi):
"""Retraction (reduced).
The retraction :meth:`~ukfm.SLAM2D.phi` applied on the robot state only.
"""
new_state = cls.STATE(Rot=state.Rot.dot(SO2.exp(xi[0])),
p=state.p + xi[1:3],
p_l=state.p_l)
return new_state
def up_phi(cls, state, xi):
"""Retraction used for updating state and infering Jacobian.
The retraction :meth:`~ukfm.SLAM2D.phi` applied on the robot state and
one landmark only.
"""
new_state = cls.STATE(Rot=state.Rot.dot(SO2.exp(xi[0])),
p=state.p + xi[1:3],
p_l=state.p_l + xi[3:5])
return new_state
def red_phi_inv(cls, state, hat_state):
"""Inverse retraction (reduced).
.. math::
\\varphi^{-1}_{\\boldsymbol{\\hat{\\chi}}}\\left(\\boldsymbol{\\chi}
\\right) = \\left( \\begin{matrix} \\log\\left(\\mathbf{C}
\\mathbf{\\hat{C}}^T\\right) \\\\
\\mathbf{p} - \\mathbf{\\hat{p}} \\end{matrix} \\right)
The state is viewed as a element :math:`\\boldsymbol{\chi} \\in SO(2)
\\times \\mathbb R^{2(L+1)}`.
Its corresponding retraction is :meth:`~ukfm.SLAM2D.red_phi`.
:var state: state :math:`\\boldsymbol{\\chi}`.
:var hat_state: noise-free state :math:`\\boldsymbol{\hat{\\chi}}`.
"""
xi = np.hstack(
[SO2.log(hat_state.Rot.dot(state.Rot.T)), hat_state.p - state.p])
return xi
def f(cls, state, omega, w, dt):
""" Propagation function.
.. math::
\\mathbf{C}_{n+1} &= \\mathbf{C}_{n} \\exp\\left(\\left(\\omega +
\\mathbf{w}^{(1)} \\right) dt\\right) \\\\
\\mathbf{p}_{n+1} &= \\mathbf{p}_{n} + \\left( \\mathbf{v}_{n} +
\\mathbf{w}^{(0)} \\right) dt \\\\
\\mathbf{p}_{1,n+1}^l &= \\mathbf{p}_{1,n}^l \\\\
\\vdots \\\\
\\mathbf{p}_{L,n+1}^l &= \\mathbf{p}_{L,n}^l
: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(SO2.exp((omega.gyro + w[1]) * dt)),
p=state.p + state.Rot.dot(np.hstack([omega.v + w[0], 0])) * dt,
p_l=state.p_l)
return new_state
def errors(self, Rots, hat_Rots, ps, hat_ps):
errors = np.zeros((self.N, 3))
for n in range(self.N):
errors[n, 0] = SO2.log(Rots[n].T.dot(hat_Rots[n]))
errors[:, 1:] = ps - hat_ps
return errors
#
# .. note::
#
# We sample for each Monte-Carlo run an initial heading error from the true
# distribution (:math:`\mathbf{P}_0`). This requires many Monte-Carlo
# samples.
for n_mc in range(N_mc):
print("Monte-Carlo iteration(s): " + str(n_mc + 1) + "/" + str(N_mc))
# simulation true trajectory
states, omegas = model.simu_f(odo_std, radius)
# simulate measurement
ys, one_hot_ys = model.simu_h(states, gps_freq, gps_std)
# initialize filter with inaccurate state
state0 = model.STATE(Rot=states[0].Rot.dot(
SO2.exp(theta0_std * np.random.randn(1))),
p=states[0].p)
# define the filters
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,