Jacobian matrix of kalman state with quaternion

jacobiankalman filterpartial derivativequaternions

How can i derive the Jacobian matrix for a Kalman filter state $x$, where $q$ stands for the orientation as quaternion and $\omega$ represents the angular velocity as vector

$$x_k=
\left[
\begin{matrix}
q \\
\omega
\end{matrix}
\right]
$$

$$
f(\hat{x}_{k-1})=
\left[
\begin{matrix}
q_{k-1} \oplus q \{\omega_{k-1} \Delta t \} \\
\omega_{k-1}
\end{matrix}
\right]
$$

$$
q \{\omega_{k-1} \Delta t \} = \left[
\begin{matrix}
cos(||\omega_{k-1}|| \frac{\Delta t}{2}) \\
\frac{\omega_{k-1}}{||\omega_{k-1}||}sin(||\omega_{k-1}||\frac{\Delta t}{2})
\end{matrix}
\right]
$$

$$ F_{ij}=\frac{\partial f_i}{\partial x_j} (\hat{x}_{k-1})=\ ?$$

Best Answer

Unit quaternions are great for parameterizing rotation in 3-D space, but trying to estimate them directly in a conventional Kalman filter setting can be tricky. This is because unit quaternions are constrained to live on the unit sphere in 4-D space ($S^3 \subset \mathbb{R}^4$). Hence, their probability density function (pdf) is restricted to the surface of the unit sphere. If one uses a Gaussian distribution to parameterize the pdf (as is done in a Kalman filter), the expectation conditioned on the measurements will lie inside the unit sphere and hence by definition will not be a unit quaternion. In addition the covariance matrix will shrink in the directions orthogonal to the surface of the unit sphere, which leads to a singular covariance matrix after several updates. This conceptual problem is explained in more detail in the references linked below. In order to circumvent this estimation problem, a common engineering practice is to represent the true orientation ($\pmb{q}$) as a small deviation from a reference orientation ($\bar{\pmb{q}}$) as:

$$ \pmb{q} = \bar{\pmb{q}} \oplus \pmb{\delta} (\pmb{e}) $$

The deviation $\pmb{\delta} \in S^3$ can be approximately parameterized by an error vector $\pmb{e} \in \mathbb{R}^3$ as:

$$ \pmb{\delta} \approx \begin{bmatrix} 1 & \frac{\pmb{e}}{2}\end{bmatrix}^T $$

For small orientation deviations, this approximation is good upto the second order. The idea then is to compute an estimate of the error vector $\hat{\pmb{e}}$ within the Kalman filter while simultaneously and separately propagating the reference quaternion through the numerical integration of:

$$\dot{\bar{\pmb{q}}} = \frac{1}{2} \cdot \bar{\pmb{q}} \oplus \begin{bmatrix} 0 \\ \bar{\pmb{\omega}} \end{bmatrix} $$

For this diff equation if we can assume that the reference angular velocity ($\bar{\pmb{\omega}}$) remains constant during the sample time, the discrete equivalent is:

$$ \bar{\pmb{q}}_k = \bar{\pmb{q}}_{k-1} \oplus \left[ \begin{matrix} cos(||\pmb{\omega}_{k-1}|| \frac{\Delta t}{2}) \\ \frac{\pmb{\omega}_{k-1}}{||\pmb{\omega}_{k-1}||} \cdot sin(||\pmb{\omega}_{k-1}||\frac{\Delta t}{2}) \end{matrix} \right] $$

The propagation dynamics for the error state can be shown to be linear (approximately) and is given by:

$$\dot{\pmb{e}} = \pmb{F}\pmb{e} + \pmb{G}\pmb{\eta}$$

where,

$\pmb{\eta} = \pmb{\omega} - \bar{\pmb{\omega}} $ - Error angular velocity assumed to be a white noise process with spectral density matrix $Q$

$\pmb{F} = - \left[ \bar{\pmb{\omega}} \times \right]$

$\pmb{G} = \pmb{I}$

Derivations of the propagation dynamics and the matrices $\pmb{F}$ and $\pmb{G}$ can be found in the references given below.

The covariance propagation equation is:

$$\dot{\pmb{P}}_e = \pmb{F}\pmb{P}_e + \pmb{P}_e\pmb{F}^T + \pmb{G}\pmb{Q}\pmb{G}^T$$

It is also worth noting that when $\pmb{e} = \pmb{0}$, then $\pmb{\delta} (\pmb{e})$ is the identity quaternion. Thus, after each measurement update, the error vector $\pmb{e}$ can be reset to zero by updating the reference quaternion as:

$$\bar{\pmb{q}}^+_k = \bar{\pmb{q}}^-_k \oplus \pmb{\delta} (\hat{\pmb{e}}_k)$$

Hope this helps!

References:

  1. Kalman Filtering for Attitude Estimation with Quaternions and Concepts from Manifold Theory
  2. Attitude Error Representations for Kalman Filtering
Related Question