Sensor Fusion – Part 3: Implementation of Gyro-Accel Sensor Fusion

May 2, 2017 ankur6ue Sensor Fusion 6

Accelerometer updates are applied at 1/3 of the sampling frequency. The jump in the covariance estimate between successive updates is easily seen.

In the previous posts, we laid the mathematical foundation necessary for implementing the error state kalman filter. In this post, we’ll provide the Matlab implementation for performing sensor fusion between accelerometer and gyroscope data using the math developed earlier.

Lets recapitulate our notation and definition of various quantities as introduced in the previous post.

    • \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 need not 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})

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}}, \boldsymbol{\delta{\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}

We derived the state transition matrix F in the last post. This matrix is given by:

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}

In a typical Kalman filter implementation, the state is updated every time step. Our implementation uses a different configuration of the Kalman filter called the feedback configuration. In this configuration, when the error state variables are updated as a result of processing a measurement, the updates are applied directly to the system state (in this case, the orientation and gyro bias). Thus, at each time step, we set the error state to be 0. For more information about the feedback configuration, see chapter 6 of 1.

We do update the error state covariance in every time step. This is done in the regular way:

P_{k\mid k-1}=F_{k}P_{k-1\mid k-1}{F} _{k}^{T}+ {Q} _{k}

Here k denotes the time step index.

Now let’s look at the measurement update step. We’ll first use the accelerometer to provide measurement updates and then add image based measurements. The accelerometer readings can be used as a measurement source as they provide direct measurement of the orientation and don’t suffer from drift (although they can have a bias, which can also be estimated). As the name suggests, the accelerometer measures the total acceleration in the body coordinate system. For a body at rest or moving a constant velocity, the accelerometer will measure the reaction to the force of gravity. Let’s denote the reaction to acceleration due to gravity by the vector [0, 0, g] and the current orientation of the body frame wrt the global frame by C. Then, the accelerometer readings are given as:

\boldsymbol{a} = C\boldsymbol{g}

While formulating the measurement update equation, we need to express the measurement residual (denoted by z), the difference between the estimated and the true value of the measurements as a linear function of the state vector.

In order to formulate the residual, we need to transform \bf{a} back to the global reference frame. This can be done by post multiplying by \hat{C}^T. Note that don’t know the true DCM C, only its estimated value. Therefore multiplying by \hat{C} is the best we can do.

\boldsymbol{z} = C\hat{C}^T\boldsymbol{g} - \boldsymbol{g}

\boldsymbol{z} = C(\hat{C}^T -C^T)\boldsymbol{g}

Now recall from the previous post that C = \hat{C} + \hat{C}[\delta{\theta}]_\times. Also, since [\delta{\theta}]_\times is skew symmetric, [\delta{\theta}]_\times^T = -[\delta{\theta}]_\times. Applying these properties,

\boldsymbol{z} = C[\delta{\theta}]_\times \hat{C}^T\boldsymbol{g}

Now, applying the approximation that for small orientation errors, C \approx \hat{C},

\boldsymbol{z} \approx \hat{C}[\delta{\theta}]_\times \hat{C}^T\boldsymbol{g}

From equation 49 in 1, [Ca]_\times= C[a]_\times C^T. Therefore,

\boldsymbol{z} = [\hat{C}\boldsymbol{\delta{\theta}}]_\times\boldsymbol{g}

From equation 39 in 1, [a]_\times\boldsymbol{b} = -[b]_\times\boldsymbol{a}. Therefore,

(1)   \begin{equation*} \boldsymbol{z} = -[g]_\times \hat{C}\boldsymbol{\delta{\theta}}\end{equation*}

The accelerometer doesn’t provide direct observability on the bias, and therefore the term corresponding to the bias error part in the state vector is 0. The complete H matrix is given by:

(2)   \begin{equation*} H_{accel} = \begin{bmatrix} -[g]_\times \hat{C} & 0 \end{bmatrix} \end{equation*}

Setting the Covariance Matrices

A major benefit of the Kalman filter is that not only does it perform the most optimal state estimation (under its operating assumptions) but it also provides an estimate of the state covariance matrix, allowing us to put a confidence interval around the estimated state variables. To do this, the filter takes as input three covariance matrices:

  • P_0: The initial value of the state covariance matrix. This can be set to suitable values reflecting our initial uncertainty in measuring the initial state. It only effects the transient behaviour of the filter, not the final (converged) value of the covariance matrix.
  • R_{accel}: This is measurement uncertainty matrix, reflecting the uncertainty induced due to various errors in the measurements.
  • P_k: This is the process noise covariance, and reflects the amount of uncertainty being added in each step of the Kalman filter update. This matrix is usually the trickiest to estimate.

A common method used to quantify various noise sources in inertial sensor data is called AllanVariance. The basic idea is to compute the variance between clusters of overlapping data with different stride lengths.

The basic idea is that short term sources of noise such as random walk and quantification errors appear over smaller values of m and average out over larger samples. Long term error sources such as gyro bias random walk appears over larger samples. Thus, by examining the slope of the plot of variance against stride length, different source of error can be visualized and quantified. I however found several aspects of Allan Variance charts hard to understand. For example, it is not clear to me why the gyro random walk noise appears on the chart with a gradient of -0.5.

I therefore tried a different method to obtain good values for the elements of the covariance matrix. I collected data from the MPU6050 unit (integrated MEMS gyro+accel from Invensense) with the unit at rest and fit a Gaussian distribution to the data using the fitdist function in Matlab. The code is shown below:

The histogram plot of the gyro_x and accel_x data are shown below. The normal distribution is indeed a good fit for the MEMS sensor data.

The sigma values for the three axes of the MPU6050 gyro and accels are shown below

x y z
Gyro 5.4732e-04 6.1791e-04 6.2090e-04
Accel 2.8e-03 2.5e-03 3.8e-03

From this data, the measurement covariance matrix can be initialized directly as

R_{accel} = \begin{bmatrix}\sigma_{ax}^2 & 0 & 0\\ 0 & \sigma_{ay}^2 & 0\\ 0 & 0 & \sigma_{az}^2 \end{bmatrix}

Where the \sigma_{ax} denotes the std. dev of the standard deviation of the measurements along the x axis of the accelerometer as shown in the table above. For the process noise covariance, I set the part corresponding to the orientation as:

Q_d(\delta{\theta}) = \begin{bmatrix}\sigma_{gx}^2 & 0 & 0\\ 0 & \sigma_{gy}^2 & 0\\ 0 & 0 & \sigma_{gz}^2 \end{bmatrix}\delta{t}

Multiplying with \delta{t} models the uncertainty injected in each iteration of the Kalman filter and works well in practice, as we’ll see shortly. The part corresponding to the gyro bias can be set to some very low value as the gyro bias changes very slowly.

Sensor Simulator

In my experience working with real sensors and Kalman filtering, it takes a lot of trial and error to get your algorithms working. If you try to work with real data directly, bugs are very hard to debug since there are so many things that can go wrong. Therefore, it is important to come up with a simulation system that models the sensors using the chosen sensor parameters and generates simulated measurements that can be used as input to the sensor fusion algorithms.

To initialize the simulator, I create a “rotation trajectory” consisting of a series of orientation waypoints.

I then create a rotation path by interpolating between one waypoint to the next. This simple scheme will create sharp changes when transition from one waypoint to another occurs. A more sophisticated system will try to do some variant of spline fitting to create a smoother trajectory without abrupt changes. Here’s a good reference that describes the problem and discusses various solutions: smooth_trajectory .

This code also attempts to create a trajectory for the position and velocity, but that can be ignored for now since those quantities are not part of our state vector. They will be later when we add image based measurements.

To generate measurement data, angular velocity necessary to transition from one orientation to the other is calculated and random noise and bias errors are added.

The sensor model is initialized from the statistics collected from the MPU6050 data logs, as explained above

Let’s now look at the calculations during each iteration of the filter.

Let’s see what happens if we run this code without applying any accelerometer updates (we’ll discuss those in a second). We start with zero for our estimated value of the gyro biases. Due to the bias and random noise in the gyro measurments, we expect the estimated orientation to gradually drift from the true orientation and the covariance of the error state to grow with every iteration. As the two videos below show, this is indeed what happens.

Let’s now add the accelerometer updates. The relevant code is shown below. The code implements the math we developed earlier.

Adding the accelerometer update correctly estimates the gyro bias and corrects for the drift in pitch and roll. The final estimated gyro bias is shown below.

x y z
Estimated Gyro bias 0.0127 -0.0179 -0.0069

As mentioned in 2, a good way to visualize if the Kalman filter is performing well is to compare the range of variation of the residuals (difference between the true and estimated states) with the corresponding element of the covariance matrix. When the filter is performing optimally, the residuals should lie within the corresponding sigma bound. For example, approximately 99% of the residuals should lie between +-3\sigma. We show the variation of the error along the x-axis (roll error) and plot it along the +-3\sigma_x. \sigma_x is obtained by taking the square root of the first element of the covariance matrix.

Here, the accelerometer measurement updates are being applied every step of the iteration. If we apply the updates less frequently, we can see the jaggedness in the graph for \sigma_x. The jaggedness occurs because the covariance increases between successive measurement updates and is brought down when the update is applied. The covariance converges in both cases, however as one might expect, it converges to a higher value when the accelerometer updates are applied less frequently.

 

From equation 1, it can be seen that when the axis of rotation aligns with the current orientation, \hat{C}\delta{\boldsymbol{\theta}} = k\boldsymbol{g}, where k is a scalar. Hence z=0. Therefore rotation along the current orientation (local vertical) doesn’t change the accelerometer residual, and is therefore unobservable.

A few more observations. The choice of the initial value of the covariance matrix only affects the transient behaviour of the filter and doesn’t change the final (converged) value of the covariance. Therefore, this matrix can be set to a diagonal matrix with suitably low values. The process noise covariance matrix does affect the final covariance matrix and can be used as a design parameter to weigh the gyro and accel measurements suitably. We set this matrix to a diagonal matrix with the terms corresponding to the orientation errors equal to the square of the standard deviation of the gyro measurements multiplied by the time step. The intuition being that this reflects the uncertainty injected in every update step. The terms corresponding to the gyro bias error can be set to a very low value as the bias changes very slowly.

To evaluate the performance of this filter implementation on real data, I applied the kalman filtering to IMU data being streamed in over a serial port. The data was collected by connecting the MPU6050 to an Arduino over a I2C connection and the raw data was sent to the PC over a serial port connection. Matlab provides APIs for receiving data over a serial port by setting up a function callback which made it easy to switch the data source to be live data instead of simulated data (contact me for the code). The sensor fusion results for live data are similar to that obtained for simulated data, except for one difference. As noted above, accelerometer measurements don’t provide observability for orientation changes along the axis of rotation and thus I disabled the update for the gyro bias error along the local z-axis. Enabling this update would cause the gyro bias along the local z-axis to become unstable and thereby lead to errors in orientation.

The plot of orientation and gyro bias errors and the corresponding 3\sigma are shown below. In general, we don’t know the true orientation for live data without expensive calibration steps. In my case, I kept the IMU on a flat surface and measured the orientation along one of the axes by calculating the angle from accelerometer data.

Accelerometer updates are applied at 1/3 of the sampling frequency. The jump in the covariance estimate between successive updates is easily seen.

In the next post, we’ll expand our state vector to include position and velocity and add image measurements.

1.
Trawny N. Indirect Kalman Filter for 3D Attitude Estimation. MARS Lab; 2005:24. http://www-users.cs.umn.edu/~trawny/Publications/Quaternions_3D.pdf.
2.
Labbe R. Kalman and Bayesian Filters in Python. None; 2017.

6 Comments

  1. Hi! thank you for your work! This is very helpful to me!

    Can I ask some?
    1. Can I get an IMU data we used?
    2. Did you try to add Magnetometer data?

    Thank you very much!

    Reply

  2. First of all, thank you very much for your work!
    This is very helpful to me!!

    Can you do me a favor?
    1. Can I get a raw data of this?
    2. Did you try to fuse Magnetometer data for yaw angle?

    Thank you

    Reply

    • Sorry, I don’t have the raw data anymore. I didn’t use magnetometer for yaw correction, which is a common technique to do yaw correction. I do show the math for the magnetometer measurement update.

      Reply

  3. Thank you very much for sharing the knowledge and this great post.
    Could you please open source the code for Sensor Fusion part 3 and part 4.

    Reply

Leave a Reply

Your email address will not be published.


*