Which orientation parametrisation to choose?
Source: MKok 2017
Estimation algorithms (filtering, smoothing) usually assume that the unknown states and parameters are represented in Euclidean space
- Due to wrapping and gimbal lock, Euclidian addition and subtraction don’t work
- Also generally don’t work for rotation matrices and unit quaternions
- Constraints (unit quaternion norm, rotation matrix orthogonality) are usually hard to implement in estimation algorithms
These concerns led to the development of the MEKF .
To deal with this: Linearisation of an orientation in SO(3) Alternative method to estimate orientation:
- assume that the orientation states lie on a manifold, e.g. using a spherical distribution, e.g. Bingham distribution
- this constrains the orientation estimates and their uncertainties to SO(3)
Common practice:
- Using unit quaternions in estimation algorithms and to normalise the resulting quaternions every time they lose their normalisations
- Or other methods to handle quaternion normalisation
Source: Markley 2014
All three-parameter representations of the rotation group have discontinuities or singularities
The estimator must implement some way of avoiding the singular points
R-P-Y Euler angles singular at $P = 90$ or $P=-90^\circ$
Gibbs / Rodrigues parameters are not suitable for filtering, as they are unable to represent $180^\circ$ rotations
The quaternion has become the standard for representing rotations in filtering, as
- it is the lowest-dimensional parametrisation that is free of singularities.
- easier to normalise quaternions rather than enforce orthogonality constraint on a rotation matrix
However, the normalisation constraint imposed on unit quaternions are not compatible with vanilla EKF.
$$\begin{aligned} \hat{\mathbf{q}}^+ = \hat{\mathbf{q}}^- + \mathbf{K} \left[ \mathbf{y} - \mathbf{h}(\hat{\mathbf{x}})^- \right] \end{aligned}$$
The addition causes the normalisation constraint to be violated! s. additive quaternion filtering , multiplicative quaternion-filtering (mekf)
Source: Whampsey MEKF
Problem for rotation propagation in KFs:
$$
\begin{aligned}
\mathbf{x}{k|k} &= \mathbf{x}{k|k-1} + \mathbf{K}_k \mathbf{y}_k\
\text{upd} &= \text{pred} + \mathbf{K} \cdot \text{residual}
\end{aligned}
$$
$\mathbf{x}_{k|k-1}$ is a unit quaternion; adding a term to it makes it non-unitary!
- deviation from the SO(3) group
- the vanilla KF update equation clashes with the invariant properties of rotations
- it assumes the states are in Euclidian space, i.e. vector operations are applicable
- but rotations are SO(3) and are therefore not really valid KF states. We need the SO(3) group operations! “Groups are only closed under their respective group operations.”
How to get around this?
- Renormalise the sum
May work to a certain extent, but this throws off the above eqn (a posteriori correction) from the original KF implementation, and