[Math] extended kalman filter equation for orientation quaternion

kalman filterorientation

I have a body pose data sampled with a given frequency. Using model with constant velocity motion between frames i filter position with EKF. State equation is given by:
$$
\begin{pmatrix}
x_{k+1} \\
y_{k+1} \\
z_{k+1} \\
\dot{x}_{k+1} \\
\dot{y}_{k+1} \\
\dot{z}_{k+1} \\
\end{pmatrix} =
\begin{pmatrix}
1 & 0 & 0 & dt & 0 & 0 \\
0 & 1 & 0 & 0 & dt & 0 \\
0 & 0 & 1 & 0 & 0 & dt \\
0 & 0 & 0 & 1 & 0 & 0 \\
0 & 0 & 0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 & 0 & 1 \\
\end{pmatrix} *
\begin{pmatrix}
x_k \\
y_k \\
z_k \\
\dot{x}_k \\
\dot{y}_k \\
\dot{z}_k \\
\end{pmatrix}
$$
How can i add orientation into the model? The same idea seems to be correct, assume the body is rotated with a constant angular speed. But what equation should i use? And what representation of orientation is suitable? Orientation in the form of rotation matrix (which i can convert to orientation quaternion) is given at the same points in time as it is with position.

Best Answer

The biggest issue with dealing with rotation in an EKF is the fact that (so far as I know) there is no sensible way to define the rotation in a vector space. Euler angles, Quaternions and rotation matrices are not vectors, but instead form groups.

This is a problem for the EKF because it assumes that all state variables are vectors. Think, for example, how you would form a covariance matrix about a quaternion. A covariance matrix is defined as:

$$ E[(x-\hat{x})(x-\hat{x})^\top]$$

But, in the case of a quaternion, what does $(q-\hat{q})$ even mean? It's no longer a unit quaternion. Same thing goes for rotation matrices. Euler angles are a little more subtle, but similar logic applies (if you subtract two "vectors" of euler angles, what does the result really mean?)

Given a linearization, euler angles actually work okay. Here's an example of a quadrotor EKF using euler angles as the orientation states Quadrotor Dynamics and Control -Beard. Obviously, euler angles have issues with gimbal lock that this source doesn't address, and euler angles are extremely computationally inefficient due to all that trigonometry, but it's meant as an introductory kalman filter implementation. If you're just getting started, it might make sense to start here.

However, to get around these problems, some smart people in spacecraft attitude estimation worked on this and came up with the "Multiplicative" EKF. In the MEKF, you can think of the covariance being defined with respect to an "error vector" about some quaternion reference state. You propagate the quaternion using standard quaternion dynamics, but then update the quaternion and define the covariance using a 3-vector parameterized as a gibbs vector or rodriguez parameters. Multiplicative vs Additive Filtering for spacecraft attitude determination - Markley. The gibbs vector actually is a vector, but it has a singularity, so it's only good for small rotations. Therefore you need a reference quaternion, and you define small "errors" to that reference with a vector. The "multiplicative" term comes from the fact that you end up "multiplying" this error vector into the reference quaternion, rather than add it with simple vector addition like you would all the other states.

This practice of dealing with non-vector components of filter states has recently been a little bit better formalized with some new notation that makes it more natural and cleans up the syntax significantly Integrating Generic Sensor Fusion Algorithms with Sound State Representations through Encapsulation of Manifolds -Hertzberg et al.. This avoids the use of error state variables, and allows you to "pretend" like you are dealing with vector states rather than these group objects (such as quaternions). The paper I linked has an example UKF implementation using quaternions, which could serve as a decent basis for your implementation.

Related Question