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.

Hardware

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

$\theta_s=atan\frac{a_x}{a_z}$$ ,

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

$\phi_s=atan\frac{a_y}{a_z}$ .

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

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
// float theta;
// float phi;

// Angular Velocities in x and y direction
float w_x;
float w_y;

// States predicted by model (regarding the Gyroscope measurements as inputs)
float theta_m = 0;
float phi_m = 0;

// States measured by sensor (accelerometer)
float theta_s;
float phi_s;

// Kalman Filter parameters and initailization
float theta_n; // a priori estimation of Theta
float theta_p = 0; // a posteriori estimation on Theta (set to zero for the initial time step k=0)
float phi_n; // a priori estimation of Phi
float phi_p = 0; // a posteriori estimation on Phi (set to zero for the initial time step k=0)

float P_theta_n; // a priori cobvariance of Theta
float P_phi_n; // a priori covariance of Phi
float P_theta_p = 0; // a posteriori covariance of Theta
float P_phi_p = 0; // a posteriori covariance of Phi

float K_theta; // Observer gain or Kalman gain for Theta
float K_phi; // Observer gain or Kalman gain for Phi

float Q = 0.1; // Covariance of disturbance (unknown disturbance affecting w_x and w_y)
float R = 4; // Covariance of noise (unknown noise affecting theta_s and phi_s)

#define SAMPLERATE_MS (100)
Adafruit_BNO055 myIMU = Adafruit_BNO055();

// Time step
float ds = SAMPLERATE_MS*0.001;
 
void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
}
 
void loop() {
  // put your main code here, to run repeatedly:
imu::Vector<3> acc = myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);  // Reading the acceleration data 
imu::Vector<3> gyro = myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); // Reading the angular velocities

// ------- Model-based Prediction ------
w_x = gyro.x(); // input
w_y = gyro.y(); // input

// Model
theta_m = theta_m - w_y*0.1;
phi_m = phi_m + w_x*0.1;

// Printing the model predictions
Serial.print(theta_m);
Serial.print(",");
Serial.print(phi_m);

// ------- Sensor Reading -------
theta_s=atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;
phi_s=atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;

Serial.print(",");
Serial.print(theta_s);
Serial.print(",");
Serial.print(phi_s);

// ------- Kalman Filter -------

P_theta_n = P_theta_p + Q;
K_theta = P_theta_n/(P_theta_n + R);
theta_n = theta_p - ds*w_y;
theta_p = theta_n + K_theta*(theta_s - theta_n);
P_theta_p = (1-K_theta)*P_theta_n;

P_phi_n = P_phi_p + Q;
K_phi = P_phi_n/(P_phi_n + R);
phi_n = phi_p + ds*w_x;
phi_p = phi_n + K_phi*(phi_s - phi_n);
P_phi_p = (1-K_phi)*P_phi_n;

Serial.print(",");
Serial.print(theta_p);
Serial.print(",");
Serial.println(phi_p);

delay(SAMPLERATE_MS);
}

100

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

// float theta;

// float phi;

// Angular Velocities in x and y direction

float w_x;

float w_y;

// States predicted by model (regarding the Gyroscope measurements as inputs)

float theta_m = 0;

float phi_m = 0;

// States measured by sensor (accelerometer)

float theta_s;

float phi_s;

// Kalman Filter parameters and initailization

float theta_n; // a priori estimation of Theta

float theta_p = 0; // a posteriori estimation on Theta (set to zero for the initial time step k=0)

float phi_n; // a priori estimation of Phi

float phi_p = 0; // a posteriori estimation on Phi (set to zero for the initial time step k=0)

float P_theta_n; // a priori cobvariance of Theta

float P_phi_n; // a priori covariance of Phi

float P_theta_p = 0; // a posteriori covariance of Theta

float P_phi_p = 0; // a posteriori covariance of Phi

float K_theta; // Observer gain or Kalman gain for Theta

float K_phi; // Observer gain or Kalman gain for Phi

float Q = 0.1; // Covariance of disturbance (unknown disturbance affecting w_x and w_y)

float R = 4; // Covariance of noise (unknown noise affecting theta_s and phi_s)

#define SAMPLERATE_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

// Time step

float ds = SAMPLERATE_MS*0.001;

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

}

void loop() {

// put your main code here, to run repeatedly:

imu::Vector<3> acc = myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER); // Reading the acceleration data

imu::Vector<3> gyro = myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); // Reading the angular velocities

// ------- Model-based Prediction ------

w_x = gyro.x(); // input

w_y = gyro.y(); // input

// Model

theta_m = theta_m - w_y*0.1;

phi_m = phi_m + w_x*0.1;

// Printing the model predictions

Serial.print(theta_m);

Serial.print(",");

Serial.print(phi_m);

// ------- Sensor Reading -------

theta_s=atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;

phi_s=atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;

Serial.print(",");

Serial.print(theta_s);

Serial.print(",");

Serial.print(phi_s);

// ------- Kalman Filter -------

P_theta_n = P_theta_p + Q;

K_theta = P_theta_n/(P_theta_n + R);

theta_n = theta_p - ds*w_y;

theta_p = theta_n + K_theta*(theta_s - theta_n);

P_theta_p = (1-K_theta)*P_theta_n;

P_phi_n = P_phi_p + Q;

K_phi = P_phi_n/(P_phi_n + R);

phi_n = phi_p + ds*w_x;

phi_p = phi_n + K_phi*(phi_s - phi_n);

P_phi_p = (1-K_phi)*P_phi_n;

Serial.print(",");

Serial.print(theta_p);

Serial.print(",");

Serial.println(phi_p);

delay(SAMPLERATE_MS);

}

Results

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.