# Jacobian matrix of kalman state with quaternion

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

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:

• Thank you for your feedback, i appreciate it. I will look into it and accept your answer as soon as i have understood it :) Commented Apr 28, 2020 at 17:45
• Your welcome. Let me know if I can help clarify anything :) Commented Apr 30, 2020 at 18:04
• Thank you :) I working with github.com/cntools/libsurvive/blob/master/src/survive_imu.c They simple use $$F_{ij} = \left[ \begin{matrix} 1 && \Delta t \\ 0 && 1 \end{matrix} \right]$$ as Jacobian and i just don't get why. I am currently working through this amazing document arxiv.org/pdf/1711.02508.pdf and try to implement your suggested approach, to see how it changes the filter result. Once i have a specific question i will come back to your offer :) Commented May 2, 2020 at 23:08
• Alright took me quite some time so wrap my head around this, especially the derivations and transformations. I will open up follow up questions for the things that aren't clear to me. Commented Jul 5, 2020 at 19:57

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]$$