Think of yourself as an FBI agent attempting to crack a mysterious case. Although you have some hints, they are not always accurate. The “Kalman filter” is a unique tool that can be used in situations like these. Similar to a detective, the Kalman filter helps in making sense of chaotic and unreliable data to provide a more precise picture of what is actually taking place. The Kalman filter is a kind of hidden weapon in the field of embedded and electrical engineering that guarantees the accuracy of our measurements. we’ll look at how to use the Kalman filter in Arduino

To move to that subsection, click the topic.

1. Kalman filter works by using two equations
2. Kalman filter in arduino
   • code 1.0
   • code 2.0
3. Kalman filter libraries

Let’s first talk about the Kalman filter. then we can talk about the use case of the Kalman filter in arduino

The Kalman filter is an algorithm that determines the true state of a process using a set of measurements taken over time, including statistical noise and other errors.

Rudolf E. Kálmán, who first published the algorithm in 1960, is remembered by the term Kalman filter.

The Kalman filter is a recursive algorithm, allowing for the updating of the process state estimate as fresh data are received.

The Kalman filter is frequently employed in situations where a process’s state changes over time, for as when tracking a moving object’s position or determining a system’s temperature.

Here are a few uses for the Kalman filter

Tracking: The position of moving objects, such as automobiles, ships, and airplanes, is tracked using the Kalman filter.

The Kalman filter is used in navigation to determine a vehicle’s position and speed, such as that of an aeroplane or a car.

Kalman filters are used to estimate a system’s state, such as the temperature of a room or a gas’s pressure.

Control: The Kalman filter is used to regulate a system’s state, such as a car’s speed or a room’s temperature. Let’s look at the Kalman filter’s principles before discussing usage scenarios in Arduino.

The Kalman filter works by using two equations

• Using the past state and the dynamics of the system and the prediction equation forecasts the state of the system at the following time step.
• the update equation, which takes into consideration the most recent measurement and forecasts to update the state estimate.

prediction equation

Predicted state at time k = (State transition matrix) *(Predicted state at time k-1) + (System noise)

• The best estimate of the system’s state at time k based on the current state and the dynamics of the system is called the predicted state at time k.
• A matrix called a “state transition matrix” illustrates how the state of the system shifts from one-time step to the next.
• Based on the previous state and the dynamics of the system, the predicted state at time k-1 is the best estimate of the system’s state at time k-1.
• The uncertainty in the dynamics of the system is represented by the noise term known as “system noise.”

update equation

Predicted state at time k = Predicted state at time k + Kalman gain * (New measurement – Measurement matrix * Predicted state at time k)

• The best estimate of the system’s state at time k based on the current state and the dynamics of the system is called the predicted state at time k.
• The fresh measurement and the anticipated state are weighted using a matrix called the Kalman gain.
• The system’s most recent measurement is known as the new measurement.
• A measurement matrix is a matrix that illustrates the relationship between the measurement and the system’s state.

Kalman gain at time k = Prediction error covariance matrix at time k * Measurement matrix transpose * (Measurement matrix * Prediction error covariance matrix at time k * Measurement matrix transpose + Process noise covariance matrix) inverse

• The new measurement and the projected state are weighted using a matrix called the Kalman gain at time k.
• The uncertainty in the projected state is represented by a matrix called the prediction error covariance matrix at time k.
• A measurement matrix is a matrix that illustrates the relationship between the measurement and the system’s state.
• A matrix called the “process noise covariance matrix” describes the uncertainty in the dynamics of the system.

these three equations are used in the Kalman filter in arduino

Kalman filter in arduino

we typically employ the Kalman filter in arduino, which provides functions dealing with 3-space.when using MPU6050,BNO055,ADXL335…etc.

this article focuses on MPU6050 and how the use of the Kalman filter in Arduino. I use an esp32 board, but any arduino board will work. the board has the capability of i2c communication.

back to topic list

first, we look at the simplest Kalman filter in arduino

code 1.0

// This code implements a simple Kalman filter for estimating the position of a moving object.

// The Kalman filter has the following parameters:

// - `Q` is the process noise covariance matrix.
// - `R` is the measurement noise covariance matrix.
// - `x` is the state vector.
// - `P` is the error covariance matrix.

// The Kalman filter works by iteratively updating the state vector and error covariance matrix based on new measurements.

// The following code shows how to initialize the Kalman filter and then update it based on a new measurement.

float Q = 0.1 * 0.1;
float R = 0.01 * 0.01;
float x[2] = {0, 0};
float P[2][2] = {{1, 0}, {0, 1}};

void setup() {

  // Initialize the serial port.
  Serial.begin(9600);

}

void loop() {

  // Get a new measurement.
  float measurement = analogRead(A0);

  // Update the Kalman filter.
  updateKalmanFilter(measurement);

  
  delay(1);
}

void updateKalmanFilter(float measurement) {

  // Calculate the Kalman gain.
  float K = P[0][0] * R / (P[0][0] * R + Q);

  // Update the state vector.
  x[0] = x[0] + K * (measurement - x[0]);
  x[1] = x[1] + K * (measurement - x[1]);

  // Update the error covariance matrix.
  P[0][0] = (1 - K) * P[0][0];
  P[1][1] = (1 - K) * P[1][1];

  // Print the estimated position and raw values.
  Serial.print("Estimated position: ");
  Serial.print(x[0]);
  Serial.print(", ");
  Serial.print(x[1]);
  Serial.print(" | Raw values: ");
  Serial.print(measurement);
  Serial.println();
}

you can see the graph changes slowly when we change resister values by using the potentiometer.

This difference likewise has no effect on the Kalman filter’s final value when the resister values are varied in small increments along the potentiometer scale.

also, you can understand the basic principle of the uses of Kalman filter in arduino by understanding the above code.

let’s move to a little bit advanced use of the Kalman function in arduino

back to topic list

code 2.0

Let’s consider an MPU6050 module. and ESP32 device. (Note: you can use any kind of arduino board).

visit to online simulator…

/* circuitprofessor.com   28 may 2023*/

#include <Wire.h>
float RateRoll, RatePitch, RateYaw;
float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;
int RateCalibrationNumber;
float AccX, AccY, AccZ, AccZwithG;
float AngleRoll, AnglePitch,AngleYaw;
uint32_t LoopTimer;
float KalmanAngleRoll=0, KalmanUncertaintyAngleRoll=2*2;
float KalmanAnglePitch=0, KalmanUncertaintyAnglePitch=2*2;
float KalmanAngleYaw=0, KalmanUncertaintyAngleYaw=2*2;
float Kalman1DOutput[]={0,0};

void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement) {
  KalmanState=KalmanState+0.004*KalmanInput;
  KalmanUncertainty=KalmanUncertainty + 0.004 * 0.004 * 4 * 4;
  float KalmanGain=KalmanUncertainty * 1/(1*KalmanUncertainty + 3 * 3);
  KalmanState=KalmanState+KalmanGain * (KalmanMeasurement-KalmanState);
  KalmanUncertainty=(1-KalmanGain) * KalmanUncertainty;
  Kalman1DOutput[0]=KalmanState; 
  Kalman1DOutput[1]=KalmanUncertainty;
}
void gyro_signals(void) {
  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(); 
  Wire.requestFrom(0x68,6);
  int16_t AccXLSB = Wire.read() << 8 | Wire.read();
  int16_t AccYLSB = Wire.read() << 8 | Wire.read();
  int16_t AccZLSB = Wire.read() << 8 | Wire.read();
  Wire.beginTransmission(0x68);
  Wire.write(0x1B); 
  Wire.write(0x8);
  Wire.endTransmission();     
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission();
  Wire.requestFrom(0x68,6);
  int16_t GyroX=Wire.read()<<8 | Wire.read();
  int16_t GyroY=Wire.read()<<8 | Wire.read();
  int16_t GyroZ=Wire.read()<<8 | Wire.read();
  RateRoll=(float)GyroX/65.5;
  RatePitch=(float)GyroY/65.5;
  RateYaw=(float)GyroZ/65.5;
  AccX=(float)AccXLSB/4096;
  AccY=(float)AccYLSB/4096;
  AccZ=(float)AccZLSB/4096;
//  AccZ=AccZwithG-0.98; //if you want to remove the gravity on the z axis uncoment this 
  AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*AccZ))*1/(3.142/180);
  AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*AccZ))*1/(3.142/180);
  AngleYaw = atan(AccZ/sqrt(AccX*AccX+AccY*AccY))*1/(3.142/180);

}
void setup() {
  Serial.begin(115200);
  Wire.setClock(400000);
  Wire.begin();
  delay(250);
  Wire.beginTransmission(0x68); 
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  for (RateCalibrationNumber=0; RateCalibrationNumber<2000; RateCalibrationNumber ++) 
{
    gyro_signals();
    RateCalibrationRoll+=RateRoll;
    RateCalibrationPitch+=RatePitch;
    RateCalibrationYaw+=RateYaw;
    delay(1);
  }
  RateCalibrationRoll/=2000;
  RateCalibrationPitch/=2000;
  RateCalibrationYaw/=2000;
  LoopTimer=micros();
}
void loop() {
  gyro_signals();
  RateRoll-=RateCalibrationRoll;
  RatePitch-=RateCalibrationPitch;
  RateYaw-=RateCalibrationYaw;

  kalman_1d(KalmanAngleRoll, KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);
  KalmanAngleRoll=Kalman1DOutput[0]; 
  KalmanUncertaintyAngleRoll=Kalman1DOutput[1];

  kalman_1d(KalmanAnglePitch, KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
  KalmanAnglePitch=Kalman1DOutput[0]; 
  KalmanUncertaintyAnglePitch=Kalman1DOutput[1];

  kalman_1d(KalmanAngleYaw, KalmanUncertaintyAngleYaw, RateYaw, AngleYaw);
  KalmanAngleYaw=Kalman1DOutput[0]; 
  KalmanUncertaintyAngleYaw=Kalman1DOutput[1];

  // Serial.print("Roll Angle [°] ");
  Serial.print(KalmanAngleRoll);
  Serial.print(",");
  // Serial.print(" Pitch Angle [°] ");
  Serial.print(KalmanAnglePitch);
  Serial.print(",");
  // Serial.print(" Yaw Angle [°] ");
  Serial.println(KalmanAngleYaw);

  while (micros() - LoopTimer < 4000);
  LoopTimer=micros();
}

let’s look at some key points in this code

kalman_1d function explanation

void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement) {
  KalmanState=KalmanState+0.004*KalmanInput;
  KalmanUncertainty=KalmanUncertainty + 0.004 * 0.004 * 4 * 4;
  float KalmanGain=KalmanUncertainty * 1/(1*KalmanUncertainty + 3 * 3);
  KalmanState=KalmanState+KalmanGain * (KalmanMeasurement-KalmanState);
  KalmanUncertainty=(1-KalmanGain) * KalmanUncertainty;
  Kalman1DOutput[0]=KalmanState; 
  Kalman1DOutput[1]=KalmanUncertainty;
}

You gave a method in the code called kalman_1d(). Four inputs are required.

• KalmanState: The current state of the Kalman filter.
• KalmanUncertainty: The current uncertainty of the Kalman filter.
• KalmanInput: The new input to the Kalman filter.
• KalmanMeasurement: The new measurement from the environment.

Update the state of the Kalman filter

KalmanState = KalmanState + 0.004 * KalmanInput;

In this stage, the new input is added to the existing state and multiplied by 0.004 to update the state of the Kalman filter.

Update the uncertainty of the Kalman filter

KalmanUncertainty = KalmanUncertainty + 0.004 * 0.004 * 4 * 4;

In this stage, the new input is added to the existing state and multiplied by 0.004 to update the state of the Kalman filter.

Calculate the Kalman gain

float KalmanGain = KalmanUncertainty / (KalmanUncertainty + 3 * 3);

When updating the state of the Kalman filter, the new input and the current state are weighted according to the Kalman gain. The Kalman gain is determined by dividing the current uncertainty by the total uncertainty of the new input and the current uncertainty.

Update the state of the Kalman filter

KalmanState = KalmanState + KalmanGain * (KalmanMeasurement - KalmanState);

The new input is added to the existing state, multiplied by the Kalman gain, in this phase to update the state of the Kalman filter.

Update the uncertainty of the Kalman filter

KalmanUncertainty = (1 - KalmanGain) * KalmanUncertainty;

The current uncertainty is multiplied by 1 minus the Kalman gain in this phase to update the Kalman filter’s uncertainty.

Return the new state and uncertainty of the Kalman filter

Kalman1DOutput[0] = KalmanState;
Kalman1DOutput[1] = KalmanUncertainty;

The function then returns the new state and uncertainty of the Kalman filter.

The function gyro_signals()

The function gyro_signals() in the code you gave calculates the rate of rotation and the angle of rotation on three axes using data from a gyroscope and an accelerometer.

The source code uses the Wire library for communication with an I2C device.
The program reads data from the I2C device and transforms it to floating point values.
The roll, pitch, and yaw angles are calculated by the code using the gyroscope and accelerometer data.

• The angles at which the object is rotated about the x, y, and z axes are known as roll, pitch, and yaw, respectively.
• Angles are expressed as degrees. The atan() function is used in the code to determine the angles.
• The y-coordinate and the x-coordinate are the two inputs that the atan() method needs.
• An angle in radians is returned by the atan() function.
• The code multiplies the angle’s radian value by 180 / 3.142 to convert it to degrees.
• The I2C bus receives a start condition from the Wire.beginTransmission() method before receiving the address of the I2C device.
• The I2C device receives a byte of data sent by the Wire.write() function.
• The I2C bus is stopped by the Wire.endTransmission() method.
• The Wire.requestFrom() function asks the I2C device for a specific number of bytes of data.
• The I2C device’s byte of data is read via the Wire.read() function.
• The arctangent of an integer is determined via the atan() function.
• The sqrt() function determines a number’s square root.

most of the time when we use sensors and kalman filter in arduino ,we use wire libraries to take inputs.

then lets explain about loop section in the code

RateRoll-=RateCalibrationRoll;
RatePitch-=RateCalibrationPitch;
RateYaw-=RateCalibrationYaw;

These lines of code subtract the rates of rotation from the calibration values. This is done to make up for any calibration problems.

kalman_1d(KalmanAngleRoll, KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);
KalmanAngleRoll=Kalman1DOutput[0]; 
KalmanUncertaintyAngleRoll=Kalman1DOutput[1];

The kalman_1d function is called in this line of code. The new measurement, the rate of rotation, the uncertainty of the estimate, and the existing estimate of the angle are all inputs to the kalman_1d function. The function then returns a fresh estimation of the angle along with its level of uncertainty.

same kalman_1d() function is used to for roll,pitch and yaw calculations

 while (micros() - LoopTimer < 4000);
  LoopTimer=micros();

4000 microseconds will pass before the program starts running. The micros() function gives the amount of time since the Arduino board was reset in microseconds.

The time at the start of the loop remains stored in the LoopTimer variable. As long as the time difference between the current time and the LoopTimer variable is less than 4000 microseconds, the while loop will keep running.

The while loop will end once the difference reaches 4000 microseconds, and execution will go on to the following line of code.

back to topic list

This is a more advanced use of the kalman filter in arduino. and there are more libraries that we can employ to make our code simpler.

here is 3 libraries which help to use kalman filter in aruino.

1. KalmanFilter– This package implements a common Kalman filter in an easy and lightweight manner. It is suitable for applications with comparatively simple computing needs. GitHub link…
2. SimpleKalmanFilter – Another little package that offers an easy Kalman filter implementation. It is ideal for basic sensor fusion jobs and is simple to use. GitHub link …
3. Kalman – This package provides a more advanced Kalman filter implementation. Along with it, it offers other features like adaptive noise and process covariance estimation. On the Arduino Library Manager, the library is accessible. GitHub link…

The library can be found on the Arduino Library Manager or on GitHub.these are the libraries for kalman filter in arduino.

as asummery

• It would help if you first consider how the data will be collected. To collect data from sensors in Aruino, we often use the i2c communication technique.
• then it would be best if you considered what specific data should be more accurate .which means you have to consider which data line is suitable for the use of the Kalman filter in arduino.
• then consider the procedure used to apply the Kalman filter within the code. (use Kalman filter libraries or code everything in code…etc)
• it is more helpful to create separate functions to read data inputs and apply the Kalman filter in arduino.
• High baud rates are preferable. For instance, the baud rate will be 9600 if you are using an Arduino Uno board, but 115200 if you are using an esp32. the output data from using the Kalman filter in Arduino are a bit slow at low baud rates.
• Actually, the Kalman filter in Arduino is a more valuable filter in dealing with real-time data.