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 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 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 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 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 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_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