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 1, we use the following notation:
- for angular velocity
- for acceleration
- for the gyro bias
- 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,
Here the rate noise is assumed to be gaussian white noise with characteristics
The gyro bias is non-static (otherwise it would not need to be estimated) and modeled as a random walk process:
We’ll also assume that the associated covariance matrices are diagonal:
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
Here, is the gyro bias error.
Our state vector consists of the error in the orientation and the error in the gyro bias.
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:
The error quaternion is the rotation required to align the estimated orientation with the true orientation.
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 such that
For non-linear problems, the matrix is obtained by linearizing around the local derivative, i.e., we first obtain an expression for . The matrix is called the system dynamics matrix. See chapter 7 in 2 for details.
The state transition matrix is then equal to . Expanding the exponential and and ignoring higher order terms,
The task thus reduces to finding the matrix . Since our state vector has two components, we can split as:
Let’s first consider . 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 1with one important difference. In 1, 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 and is given as:
As in part 1 of this series, let’s define the cross product skew symmetric matrix as:
The expression for quaternion multiplication can now be written as:
here, is the vector part () of the quaternion.
The product can also be written as:
Using the cross product matrix for , this expression be written as:
Taking the difference of these two expressions,
From this result, it is clear that quaternion multiplication is not commutative. Simplifying,
Now following chapter 1 in3, let describe the relative orientation of frame A wrt frame B. Let the instantaneous angular velocity of A along direction be represented by . Then the incremental coordinate rotation in time is given by quaternion:
At time , the orientation is represented by the quaternion , where
From this, the time derivative of the quaternion can be obtained as
Here, is the quaternion representation. Written in terms of its scalar and vector components,
In order to obtain the system dynamics for the error quaternion, we need to express the derivative of the error quaternion as a function of .
Now from the definition of the error quaternion,
Taking the derivative,
Expressing the time derivative in terms of angular velocity using the expression derived earlier,
Now, from the definition of the error quaternion,
We are almost there! We have expressed in terms of . However we still need to get rid of .
Writing in terms of the estimated ,
Collecting terms, expanding the quaternions into their scalar and vector components and using 1,
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
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:
Replacing with , we finally obtain the expression we seek:
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,
We finally have our system dynamics equation!
As mentioned above, using a first order approximation, the state transition matrix can be obtained from the system dynamics matrix using
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.