Sensor Fusion: Part 2 (combining Gyro-Accel data)

In the previous post, we laid some of the mathematical foundation behind the kalman filter. In this post, we’ll look at our first concrete example – performing sensor fusion between a gyro and an accelerometer.

Real world MEMS Gyros and accels have typically two major error sources – bias drift and random noise. There are also scaling errors and cross axis errors (X/Y/Z axis may not be exactly perpendicular), however those errors tend to be small and won’t be considered in our analysis.

Following ¹, we use the following notation:

$\bf{\omega}$ for angular velocity
$\bf{a}$ for acceleration
$\bf{\omega_b}$ for the gyro bias
$\bf{a_b}$ for the accel bias
subscript m to denote measured quantities and hat to denote estimated quantities
True values appear without annotation.

The measured value of the gyro is the true value corrupted with the gyro bias and random noise. Therefore,
$\bf{\omega_m} = \bf{\omega} + \bf{\omega_b} + \bf{n_r}$

Here the rate noise $\bf{n_r}$ is assumed to be gaussian white noise with characteristics
$E[\boldsymbol{n_r}] = 0_{3\times1}, E[\boldsymbol{n_r} (t+\tau) \boldsymbol{n_r^T}(t)] = \boldsymbol{N_r}\delta(\tau)$

The gyro bias is non-static (otherwise it would not need to be estimated) and modeled as a random walk process:
$E[\boldsymbol{n_b}] = 0_{3\times1}, E[\boldsymbol{n_b} (t+\tau) \boldsymbol{n_b^T}(t)] = \boldsymbol{N_b}\delta(\tau)$

We’ll also assume that the associated covariance matrices are diagonal:
$\boldsymbol{N_r} = diag(\sigma^{2}_{r1}, \sigma^{2}_{r2}, \sigma^{2}_{r3})$

and
$\boldsymbol{N_b} = diag(\sigma^2_{b1}, \sigma^2_{b2}, \sigma^2_{b3})$

In the next post, we’ll consider how to set these covariances from measurement logs.

The estimated angular rate is the difference between the measured rate and our estimate of the gyro bias
$\bf{\hat{\omega}} = \bf{\omega_m} - \bf{\hat{\omega_b}}$
$\bf{\hat{\omega}} = \bf{\omega} + \bf{\omega_b} + \bf{n_r} - \bf{\hat{\omega_b}}$
$\bf{\hat{\omega}} = \bf{\omega} - \bf{\delta{\omega_b}} + \bf{n_r}$

Here, $\bf{\delta{\omega_b}} = \bf{\hat{\omega_b}} - \bf{\omega_b}$ is the gyro bias error.

Our state vector consists of the error in the orientation and the error in the gyro bias.
$\bf{x} = [\boldsymbol{\delta{\theta}}, \delta{\bf{\omega_b}}]$

The error in orientation is defined as the vector of small rotations that aligns the estimated rotation with the true rotation. The corresponding error quaternion is approximated by:

$\delta{q} \approx \begin{bmatrix}1 \\ \frac{\boldsymbol{\delta{\theta}}}{2}\end{bmatrix}$

The error quaternion is the rotation required to align the estimated orientation with the true orientation.
$q = \hat{q}*\delta{q}$
Here $*$ denotes quaternion multiplication.

The key task in the EKF formulation is to come up with the state propagation matrix which describes how the state changes from one step to the next. In other words, we are interested in a matrix $F$ such that
$\boldsymbol{x_{t+dt}} = F\boldsymbol{x_t}$

For non-linear problems, the matrix $F$ is obtained by linearizing around the local derivative, i.e., we first obtain an expression for $\boldsymbol{\dot{x}}=J\bf{x}$ . The matrix $J$ is called the system dynamics matrix. See chapter 7 in ² for details.

The state transition matrix $F$ is then equal to $\exp({J}dt})$ . Expanding the exponential and and ignoring higher order terms,
$F = I + Jdt$ .

The task thus reduces to finding the matrix $J$ . Since our state vector has two components, we can split $J}$ as:

$J= \begin{bmatrix} J_{\delta{\boldsymbol{\theta}}} & J_{\boldsymbol{\omega_b}} \end{bmatrix}$

Let’s first consider $J_{\delta{\boldsymbol{\theta}}}$ . This is the trickiest part of the entire EKF formulation, so hang tight! I’ll first mention some quaternion algebra equations that we’ll be using later in the derivation. This algebra is taken from ¹with one important difference. In ¹, the quaternions are defined as q = [vector scalar], whereas we’ll be using the more common definition: q = [scalar vector].

The product of two quaternions $q$ and $p$ is given as:

$q*p = \begin{bmatrix}q_1 & -q_2 & -q_3 & -q_4 \\ q_2 & q_1 & -q_4 & q_3 \\ q_3 & q_4 & q_1 & -q_2 \\ q_4 & -q_3 & q_2 & q_1 \end{bmatrix}\begin{bmatrix}p_1\\p_2\\p_3\\p_4\end{bmatrix}$

As in part 1 of this series, let’s define the cross product skew symmetric matrix as:

$[q]_{\times} = \begin{bmatrix} 0 & -q_4 & q_3\\ q_4 & 0 & q_2 \\ -q_3 & q_2 & 0 \end{bmatrix}$

The expression for quaternion multiplication can now be written as:

$q*p = \begin{bmatrix}q_1 & -\bf{q} \\ \bf{q}^T & q_1\boldsymbol{I}_{3\times3} + [q]_{\times} \end{bmatrix}\begin{bmatrix}p_1\\p_2\\p_3\\p_4\end{bmatrix}$

here, $\bf{q}$ is the vector part ( $[q_2, q_3, q_4]$ ) of the quaternion.

The product can also be written as:

$q*p = \begin{bmatrix}p_1 & -p_2 & -p_3 & -p_4 \\ p_2 & p_1 & p_4 & -p_3 \\ p_3 & -p_4 & p_1 & p_2 \\ p_4 & p_3 & -p_2 & p_1 \end{bmatrix}\begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix}$

Using the cross product matrix for $p$ , this expression be written as:

$q*p = \begin{bmatrix}p_1 & -\bf{p} \\ \bf{p}^T & p_1\boldsymbol{I}_{3\times3} + [p]_{\times} \end{bmatrix}\begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix}$

Taking the difference of these two expressions,

$q*p - p*q = (\begin{bmatrix}q_1 & -\bf{q} \\ \bf{q}^T & q_1\boldsymbol{I}_{3\times3} + [q]_{\times} \end{bmatrix} - \begin{bmatrix}q_1 & -\bf{q} \\ \bf{q}^T & q_1\boldsymbol{I}_{3\times3} - [q]_{\times} \end{bmatrix})p$

From this result, it is clear that quaternion multiplication is not commutative. Simplifying,

(1) $\begin{equation*} q*p - p*q = \begin{bmatrix} 0 & 0 \\ 0 & 2[q]_{\times} \end{bmatrix} p \end{equation*}$

Now following chapter 1 in³, let $q(t)$ describe the relative orientation of frame A wrt frame B. Let the instantaneous angular velocity of A along direction $\bf{s}$ be represented by $\bf{\omega}$ . Then the incremental coordinate rotation in time $\delta{t}$ is given by quaternion:

$\delta{q(\delta{t})} \approx \begin{bmatrix} 1 \\ \frac{\bf{\omega}\delta{t}}{2} \end{bmatrix}$

At time $t+\delta{t}$ , the orientation is represented by the quaternion $q(t+\delta{t})$ , where
$q(t+\delta{t}) = q(t) * \delta{q(\delta{t})}$

From this, the time derivative of the quaternion can be obtained as
$\dot{q} = \frac{1}{2}q*\omega$

Here, $\omega$ is the quaternion representation. Written in terms of its scalar and vector components,
$\omega = \begin{bmatrix} 0 \\ \boldsymbol{\omega}\end{bmatrix}$

In order to obtain the system dynamics for the error quaternion, we need to express the derivative of the error quaternion $\dot{\delta{q}}$ as a function of $\delta{q}$ .

Now from the definition of the error quaternion,
$q = \hat{q}*\delta{q}$

Taking the derivative,
$\dot{q} = \dot{\hat{q}}*\delta{q} + \hat{q}*\dot{\delta{q}}$

Expressing the time derivative in terms of angular velocity using the expression derived earlier,
$\frac{1}{2}q*\omega = \frac{1}{2}\hat{q}*\hat{\omega}*\delta{q} + \hat{q}*\dot{\delta{q}}$

Moving the term containing $\delta{q}$ to the LHS

(2) $\begin{equation*} \hat{q}*\dot{\delta{q}} = \frac{1}{2}q*\omega - \frac{1}{2}\hat{q}*\hat{\omega}*\delta{q} \end{equation*}$

Now, from the definition of the error quaternion,
$q = \hat{q}*\delta{q}$

pre-multiplying by $\hat{q}^{-1}$

(3) $\begin{equation*} \hat{q}^{-1}*q = \delta{q} \end{equation*}$

Pre-multiplying 2 by $\hat{q}^{-1}$ and using the equation 3,
$\dot{\delta{q}} = \frac{1}{2}\delta{q}*\omega - \frac{1}{2}\hat{\omega}*\delta{q}$

We are almost there! We have expressed $\dot{\delta{q}}$ in terms of $\delta{q}$ . However we still need to get rid of $\omega$ .

Writing $\omega$ in terms of the estimated $\hat{\omega}$ ,
$\dot{\delta{q}} = \frac{1}{2}\delta{q}*(\hat{\omega} + \delta{\omega_b} - \boldsymbol{n_r}) - \frac{1}{2}\hat{\omega}*\delta{q}$

Collecting terms, expanding the quaternions into their scalar and vector components and using 1,

$\dot{\delta{q}} = \frac{1}{2}\begin{bmatrix} 0 & 0 \\ 0 & -2[\hat{\omega}]_\times \end{bmatrix}\begin{bmatrix}1 \\ \boldsymbol{\delta{q}}\end{bmatrix} - \frac{1}{2}\begin{bmatrix} 0 & -(-\delta{\omega_b}} + \boldsymbol{n_r}) \\ (-\delta{\omega_b}} + \boldsymbol{n_r}) & -[(-\delta{\omega_b}} + \boldsymbol{n_r})]_\times \end{bmatrix}\begin{bmatrix} 1 \\ \boldsymbol{\delta{q}}\end{bmatrix}$

Note that we have replaced quaternion multiplication by matrix multiplication. In the second term of the equation above, we can ignore the second order parts that multiply the gyro bias error and noise with the error quaternion. Applying this simplification, we obtain

$\dot{\delta{q}} = \begin{bmatrix} 0 \\ -[\hat{\omega}]_\times\delta{q} \end{bmatrix} - \frac{1}{2}\begin{bmatrix} 0 \\ -\delta{\omega_b} + \boldsymbol{n_r}) \end{bmatrix}$

Now recall that our state vector actually consists of the error in rotation, not the error quaternion. The relationship between the two is given by:
$\delta{q} \approx \begin{bmatrix}1 \\ \frac{\boldsymbol{\delta{\theta}}}{2}\end{bmatrix}$

Replacing $\delta{q}$ with $\delta{\boldsymbol{\theta}}$ , we finally obtain the expression we seek:

(4) $\begin{equation*}\delta{\dot{\boldsymbol{\theta}}} = -\hat{\boldsymbol{\omega}}\times\delta{\boldsymbol{\theta}} + \delta{\boldsymbol{\omega_b}} - \boldsymbol{n_r}\end{equation*}$

The other part of our state vector, gyro bias error is very easy to handle. Since the bias changes very slowly, we can simply assume that the error is constant over the iteration time interval. Thus,
$\dot{\delta{\boldsymbol{\omega_b}}} = 0$

We finally have our system dynamics equation!

(5) $\begin{equation*} \begin{bmatrix} \delta{\dot{\boldsymbol{\theta}}} \\ \dot{\delta{\boldsymbol{\omega_b}}}\end{bmatrix} = \begin{bmatrix}-[\hat{\omega}]_\times & I_{3\times3} \\ 0_{3\times3} & 0_{3\times3}\end{bmatrix} \begin{bmatrix}\delta{\boldsymbol{\theta}} \\ \delta{\boldsymbol{\omega_b}} \end{bmatrix} \end{equation*}$

As mentioned above, using a first order approximation, the state transition matrix can be obtained from the system dynamics matrix using
$F = I + Jdt$ .

Therefore,
$F = \begin{bmatrix}-[\hat{\phi}]_\times & I_{3\times3}\delta{t} \\ 0_{3\times3} & 0_{3\times3}\end{bmatrix}$

Where
$[\hat{\phi}]_\times = [\hat{\omega}]_\times \delta{t}$

We’re mostly done with the mathematical heavy lifting. In the next post, we’ll look at the Matlab implementation for all this math which will hopefully clarify the concepts presented in this post.

Trawny N, Roumeliotis S. Indirect Kalman Filter for 3D Attitude Estimation. MARS; 2005:24. http://www-users.cs.umn.edu/~trawny/Publications/Quaternions_3D.pdf.

Labbe R. Kalman and Bayesian Filters in Python. Kalman and Bayesian Filters in Python. https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python. Published April 22, 2017.

Stevens BL, Lewis FL, Johnson EN. Aircraft Control and Simulation: Dynamics, Controls Design, and Autonomous Systems. John Wiley & Sons, Inc; 2015. doi:10.1002/9781119174882

Telesens

Sensor Fusion: Part 2 (combining Gyro-Accel data)

Be the first to comment

Leave a Reply Cancel reply