Blog Post:

IMU Signal Processing with Kalman Filter

Sensors play a pivotal role in gathering critical data from the world around us. One such sensor, the Inertial Measurement Unit (IMU), has gained prominence for its ability to provide real-time information about an object’s orientation, acceleration, and angular velocity. Yet, harnessing the full potential of an IMU sensor can be a challenging task as it is susceptible to noise and disturbances that can affect the accuracy of the collected data. This is where the Kalman Filter steps in as a powerful tool, offering a sophisticated solution for enhancing the precision of IMU sensor data. In this blog post, we’ll embark on a journey to explore the synergy between IMU sensors and the Kalman Filter, understanding how this dynamic duo can revolutionize applications ranging from robotics and drones to augmented reality and more. Join us as we delve into the intricacies of sensor fusion and filtering and unlock the secrets to reliable and accurate IMU data interpretation.


Here we use two simple and widely available hardware pieces to start our project. We chose the ADAFRUIT BNO055 development board as a quick way to interface with the Bosch BNO055 IMU chip. To collect data through I2C communication protocol, we use an Arduino Nano board, it is small and perfectly sized for our experiments. The wiring is also very straight forward. All you need is 4 wires for power and I2C communication and a USB cable (to connect your Arduino Nano to your PC/Laptop).

To properly complete I2C connection, one should:

  • Connect pin A4 from Arduino to SDA pin on BNO055
  • Connect pin A5 from Arduino to SCL pin on BNO055

To power the sensor, just connect its input voltage pin (Vin) to the output voltage pin (5V) on the Arduino and also connect the ground pin (GND) of the sensor to the ground pin (GND) on the Arduino.

Collecting Sensor Measurements and Interpreting Data

The Adafruit BNO055 sensor is a 9-axis IMU sensor that provides three vectors as:

  • Acceleration: linear acceleration in x, y, and z directions.
  • Gyro: rotational velocity in x, y, and z directions.
  • Magnetometer: magnetic field in x, y, and z directions.

For the sake of simplicity, here we only use the first two vectors, i.e., we only use the sensor to measure accelerations a_x, a_y, and a_z and rotational velocities \omega_x, \omega_y, and \omega_z.

When the sensor is at rest, or is moving with a constant velocity, the sensor can only sense the gravitational force. Hence, the sensed acceleration in each principal direction (the x-y-z coordinate system is attached to the sensor) gives us a clue to estimate the orientation. Accordingly, one can measure the yaw angle (the angle between the x-axis on the sensor and the horizontal plane) as


and in a similar way, the pitch (the angle between the y-axis and the horizontal plane) can be measured by:


Here we use the subscript “s” for both \theta and \phi to mention that these are sensor readings. As you know, the sensor readings are always corrupted with noise so it cannot be depended upon. For example, when the sensor is on a vibrating platform, it will sense random acceleration on all three directions. However, the orientation is almost unchanged!

A gyroscope which measures the rotational velocities provides another way to predict \theta and \phi. Assuming that at the start the attached coordinate of the sensor is aligned with the global coordinate system (\theta=0, \phi=0), we can predict these angles using the following model:

\begin{cases}\theta(k)=\theta(k-1)-\omega_y \Delta t \\ \phi(k)=\phi(k-1)+\omega_x \Delta t\end{cases}.

Generally, the measurements by the gyro are less corrupted by noise and are very reliable, which is a good thing. However, only using the model above to predict the orientation can be problematic as very small and negligible errors in measurement will be summed (it is the effect of integration) and in the long run cause a remarkable drift in our prediction, which is undesirable.

Formulating the Kalman Filter

The main idea of Kalman Filter is to combine the data that comes from the model with the measurements that comes from the sensor. Uncertainty in modeling and the unknown disturbances can lead to misleading prediction if we only rely on the model. On the other hand, considering only the sensor reading is also problematic as the sensor measurements can be very noisy. The Kalman Filter provides a way to combine these two sources and achieve the best prediction.

In the previous section, we showed that using the data that comes from the accelerometer, we can measure the orientation. We assume this as the sensor data, which as mentioned above, can be very noisy. On the other hand, we also have a model that takes the gyroscope reading as the input and predicts the orientation. We use the discrete-time Kalman Filter as a mathematical tool to combine these two (measurements and predictions) to generate a best estimation.

First, we need to formulate the a priori covariances for each state variable as

\begin{cases} P_{\theta}^-(k) = P_{\theta}^+(k-1) + Q_\theta \\ P_{\phi}^-(k) = P_{\phi}^+(k-1)+Q_\phi \end{cases}

Then we need to determine the Kalman gain

\begin{cases} K_\theta(k) = \frac{P_\theta^-(k)}{P_\theta^-(k)+R_\theta} \\ K_\phi(k) = \frac{P_\phi^-(k))}{P_\phi^-(k)+R_\phi} \end{cases}

Next, the a priori estimation, based on the model will be derived as:

\begin{cases} \hat{\theta}^-(k) = \hat{\theta}^+(k-1)-\Delta t \omega_y\\ \hat{\phi}^-(k) = \hat{\phi}^+(k-1)-\Delta t \omega_x \end{cases}

The optimal estimation, also called as the a posteriori estimation is then calculated by

\begin{cases} \hat{\theta}^+(k) = \hat{\theta}^-(k)+K_\theta[\theta_s(k)-\hat{\theta}^-(k)] \\ \hat{\phi}^+(k) = \hat{\phi}^-(k)+K_\phi[\phi_s(k)-\hat{\phi}^-(k)] \end{cases}

And finally, we need to update the a posteriori covariance, getting ready for the next time-step.

\begin{cases} P_\theta^+(k) = \big( 1-K_\theta(k) \big) P_\theta^-(k)\\ P_\phi^+(k) = \big( 1-K_\phi(k) \big) P_\phi^-(k) \end{cases}

Source Code for the Arduino


The graphs below show the results of implementing the Kalman Filter on the sensor reading from IMU (BNO055). Here, we can see that the estimations based on the dynamic model (\theta_m and \phi_m) suffer from a drift after about 15 seconds and this error will be carried over and becomes larger as times goes on. On the other hand, measurements by the sensor (\theta_s and \phi_s) suffer from random noise. Finally, the a posteriori estimation by Kalman filter (\theta^ and \phi^) cancels both the noise and the drift by combining the both the sensor measurements and the predictions made by the model.

“Powered by QuickLaTeX

Leave a Reply

Your email address will not be published. Required fields are marked *

Get in Touch

If you have a product design that you would like to discuss, a technical problem in need of a solution, or if you just wish you could add more capabilities to your existing engineering team, please contact us.