Jacobian matrix of kalman state with quaternion

Question

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})=\ ?$$

Answer 1

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

Answer 2

Hey guys so i tried to derive this with my limited math skills, maybe someone can confirm/correct this.

The derivation of a quaternion product should be

$$ (q_1 \oplus q_2)' = q_1' \oplus q_2 + q_1 \oplus q_2' $$

Therefore the Jocobian can be derived as

$$ F_{ij}=\frac{\partial f_i}{\partial x_j} (\hat{x}_{k})= \left[ \begin{matrix} \frac{\partial(q_{k} \oplus q \{\omega_k \Delta t \})}{\partial q_k} && \frac{\partial(q_{k} \oplus q \{\omega_k \Delta t \})}{\partial \omega}\\ \frac{\partial\omega_k}{\partial q_k} && \frac{\partial\omega_k}{\partial \omega} \end{matrix} \right] = \left[ \begin{matrix} \frac{\partial q_{k}}{\partial q_k} \oplus q \{\omega_k \Delta t \} + q_k \oplus \frac{\partial q \{\omega_k \Delta t \}}{\partial q_k} && \frac{\partial q_{k}}{\partial \omega_k} \oplus q \{\omega_k \Delta t \} + q_k \oplus \frac{\partial q \{\omega_k \Delta t \}}{\partial \omega_k} \\ \frac{\partial\omega_k}{\partial q_k} && \frac{\partial\omega_k}{\partial \omega_k} \end{matrix} \right] = \left[ \begin{matrix} 1 \oplus q \{\omega_k \Delta t \} + q_k \oplus 0 && 0 \oplus q \{\omega_k \Delta t \} + q_k \oplus \frac{\partial q \{\omega_k \Delta t \}}{\partial \omega_k} \\ 0 && 1 \end{matrix} \right] = \left[ \begin{matrix} 1 \oplus q \{\omega_k \Delta t \} && q_k \oplus \frac{\partial q \{\omega_k \Delta t \}}{\partial \omega_k} \\ 0 && 1 \end{matrix} \right] $$

The derivative of the rotation rate is then given by

$$ \frac{\partial q \{\omega_k \Delta t \}}{\partial \omega_k} = \left[ \begin{matrix} \frac{\partial}{\partial \omega_k} cos(||\omega_{k}|| \frac{\Delta t}{2}) \\ \frac{\partial}{\partial \omega_k} \frac{\omega_{k}}{||\omega_{k}||}sin(||\omega_{k}||\frac{\Delta t}{2}) \end{matrix} \right] = \left[ \begin{matrix} \frac{-\Delta t \ \omega_k}{2 ||\omega_k||} sin(||\omega_{k}|| \frac{\Delta t}{2}) \\ \frac{\Delta t \ \omega_k^2 cos(||\omega_{k}|| \frac{\Delta t}{2})}{2(w_{k1}^2+w_{k2}^2+w_{k3}^2)} + \frac{sin(||\omega_{k}|| \frac{\Delta t}{2})}{||\omega_k||} - \frac{\omega_k^2 sin(||\omega_{k}|| \frac{\Delta t}{2})}{(w_{k1}^2+w_{k2}^2+w_{k3}^2)^{3/2}} \end{matrix} \right] $$