Real time Kalman filter on an ESP32 and sensor fusion.

Implementing a discrete-time Kalman filter (steady-state). A bit of theory and a practical demonstration. Uses the Arduino software. Acknowledgement to random nerd tutorials too. Measures pitch angle and will be suitable for some balancing projects such as a Segway or Quad copters. Uses sensor fusion of accelerometer and gyro readings. The steady state Kalman filter is implemented in closed-loop form and in state-space format. The relationship to a Wiener estimator I will show in more detail later. This requires a transfer function matrix but is not used here.

[@TJMoir]

Code: // Basic demo for accelerometer readings from Adafruit MPU6050
//Kalman filter added by T.J.Moir 8/5/2021 and angle calculation
// ESP32 Guide: RandomNerdTutorials.com/esp32-mpu-6050-accelerometer-gyroscope-arduino/
// ESP8266 Guide: RandomNerdTutorials.com/esp8266-nodemcu-mpu-6050-accelerometer-gyroscope-arduino/
// Arduino Guide: RandomNerdTutorials.com/arduino-mpu-6050-accelerometer-gyroscope/
#include
#include
#include
float angX,angY,dangZ1,dangZ2,dangZ3,angZ;
//Kalman filter data
float f11,f12,f21,f22,fc11,fc12,fc21,fc22;
float d11,d12,d21,d22;
//states
float xh1k,xh2k,xh1k_1,xh2k_1;
float s1,s2; //observations, angular position and angular velocity from accelerometer angles and gyro info
//H matrix is identity
//Filter gain matrix
float k11,k12,k21,k22;
//sampling freq and interval
float fs,Ts;
// for sampling freq
int sample_pin=16;
boolean running = false;
Adafruit_MPU6050 mpu;
void setup(void) {
pinMode(sample_pin,OUTPUT);
sample_pin=false;
///////////////////////////
//sampling frequency
fs=10000.0;
//sampling interval
Ts=1/fs;
f11=1;
f12=-Ts;
f21=0.0;
f22=1.0;
d11=Ts;
d12=-Ts*Ts*0.5;
d21=0.0;
d22=Ts;
//Kalman gains have been calculated offline for Q=I,R=I*1e-5;
k11=0.0311;
k12=-5.1556e-5;
k21=-4.8444e-5;
k22=0.0311;
//
//initialise state estimates
xh1k=0.0;
xh2k=0.0;
xh1k_1=0.0;
xh2k_1=0.0;
///////
//calculate Kalman filter Fc closed loop F matrix. Note H=I identity matrix Fc=F-KH (K has been found offline)
fc11=f11-k11;
fc12=f12-k12;
fc21=f21-k21;
fc22=f22=k22;
/////

Serial.begin(115200);
while (!Serial)
//delay(10); // will pause Zero, Leonardo, etc until serial console opens
//Serial.println("Adafruit MPU6050 test!");
// Try to initialize!
if (!mpu.begin()) {
//Serial.println("Failed to find MPU6050 chip");
while (1) {
delay(10);
}
}
//Serial.println("MPU6050 Found!");
mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
//Serial.print("Accelerometer range set to: ");
switch (mpu.getAccelerometerRange()) {
case MPU6050_RANGE_2_G:
// Serial.println("+-2G");
break;
case MPU6050_RANGE_4_G:
//Serial.println("+-4G");
break;
case MPU6050_RANGE_8_G:
//Serial.println("+-8G");
break;
case MPU6050_RANGE_16_G:
//Serial.println("+-16G");
break;
}
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
//Serial.print("Gyro range set to: ");
switch (mpu.getGyroRange()) {
case MPU6050_RANGE_250_DEG:
//Serial.println("+- 250 deg/s");
break;
case MPU6050_RANGE_500_DEG:
// Serial.println("+- 500 deg/s");
break;
case MPU6050_RANGE_1000_DEG:
//Serial.println("+- 1000 deg/s");
break;
case MPU6050_RANGE_2000_DEG:
//Serial.println("+- 2000 deg/s");
break;
}
mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
//Serial.print("Filter bandwidth set to: ");
switch (mpu.getFilterBandwidth()) {
case MPU6050_BAND_260_HZ:
//Serial.println("260 Hz");
break;
case MPU6050_BAND_184_HZ:
//Serial.println("184 Hz");
break;
case MPU6050_BAND_94_HZ:
//Serial.println("94 Hz");
break;
case MPU6050_BAND_44_HZ:
//Serial.println("44 Hz");
break;
case MPU6050_BAND_21_HZ:
//Serial.println("21 Hz");
break;
case MPU6050_BAND_10_HZ:
//Serial.println("10 Hz");
break;
case MPU6050_BAND_5_HZ:
//Serial.println("5 Hz");
break;
}
//Serial.println("");
delay(100);

}
void loop() {


/* Get new sensor events with the readings */
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
/* Print out the values */
/*
Serial.print("Acceleration X: ");
Serial.print(a.acceleration.x);
Serial.print(", Y: ");
Serial.print(a.acceleration.y);
Serial.print(", Z: ");
Serial.print(a.acceleration.z);
Serial.println(" m/s^2");*/
//dangZ1=sqrt(a.acceleration.z*a.acceleration.z+a.acceleration.x*a.acceleration.x);
dangZ2=sqrt(a.acceleration.z*a.acceleration.z+a.acceleration.y*a.acceleration.y);
//dangZ3=sqrt(a.acceleration.x*a.acceleration.x+a.acceleration.y*a.acceleration.y);
//angX=57.29*atan2(a.acceleration.y,dangZ1);
//Serial.println(angX); //roll
angY=57.29*atan2(a.acceleration.x,dangZ2);
// Serial.println(angY); //pitch angle in degrees only
//angZ=57.29*atan2(a.acceleration.z,dangZ3);
//Serial.println(angZ); //Pitch wrt 90 degrees
//Kalman filter here
s1= angY; //angular position from accelerometer calculation
s2=g.gyro.y;//from gyro angular velocity
//shuffle regressors of the states
xh1k_1=xh1k;
xh2k_1=xh2k;
xh1k=fc11*xh1k_1 +fc12*xh2k_1+k11*s1+k12*s2;
xh2k=fc21*xh1k_1 +fc22*xh2k_1+k21*s1+k22*s2;
/*
Serial.print("\t");
Serial.print(s1); //noisy angle estimate of pitch
Serial.print(" "); */
Serial.println(xh1k); //KF pitch angle estimate
// Serial.println(xh2k); //KF pitch angular velocity estimate
// Sets a flag at a precise time.
//////
// To measure the sample rate from pin 21
digitalWrite(sample_pin,running);
running=!running;
/*
Serial.print("Rotation X: ");
Serial.print(g.gyro.x);
Serial.print(", Y: ");
Serial.print(g.gyro.y);
Serial.print(", Z: ");
Serial.print(g.gyro.z);
Serial.println(" rad/s");
Serial.print("Temperature: ");
Serial.print(temp.temperature);
Serial.println(" degC");
Serial.println("");
*/
//delay(5);

}

[@jcalvarezg]

Very nice, thanks for the guidelines.I got the book yesterday and I like the way the examples are worked. Great job.

[@amineminou6046]

The book has thé cod??

[@satyabratasenapati7376]

Very useful 😂

[@KennyFairweather]

Nice

[@TJMoir]

I re did the other one in higher res too.

[@monishagowri1902]

can i get the code?

[@wilfriedpacomemenye739]

Please can you know how i can use this with gps to estimate the position in space? It's for my School project

[@TJMoir]

A spaceship uses a Gyro and Accelerometer. Each inertial measurement unit has three gyroscopes and three accelerometers -- one gyro and one accelerometer for each axis of the spacecraft. x y and z. From acceleration and velocity (angular for both) we can estimate position in each axis using a Kalman Filter. ie how far in each axis the spaceship has turned. You would need at least 6 states, two for each axis but possibly more (I think Apollo had 12). But this only gives a relative position to a past position, you need an absolute measurement so on earth we can use GPS and on say Mars you would need a star fix from a star tracker. When Apollo took off it had a fix for the Gyros from earth but from the Moon they must have used a star to fix absolute position (though all such measurements are relative of course and there are no absolutes in space). With no Kalman filter you would need to somehow combine the Gyro and Accelerometer readings using a cruder method. You would also need to integrate and this gives drift and errors.

[@ahmedbelhcine1441]

can I get the code? I have esp32 and MPU6050 and I wanna get real time tilt angle for my self balancing robot :( can u help?

[@TJMoir]

Is this just a hobby or a project from university?

[@stasjurgensons7008]

@@TJMoir Dear Tom Moir. I do for Uni. I am writing my bachelor paper about comparison of sensor fusion algorithms for UAV sensor fusion. At the same time we are building high power rocket in university and I am contributing as navigation system designer. For some reason I can not access your book "feedback" via uni library even though second one book "Rudiments of Signal Processing and Systems" is available. May be you can provide some useful materials that can contribute to my understanding of developing complex discrete Kalman filter to fuse all UAV sensors. As with my current understanding most likely I would cascade complementary filters.

[@TJMoir]

@@stasjurgensons7008 Send me an email at [e-mail] but remove cap letters.

[@TJMoir]

Code is in comment here by me. Latest comment

[@StevenCookFX]

There is no Arduino language. Its embedded C. Arduino is a IDE

[@TJMoir]

Yes fair enough agreed

[@AntiComunaBR]

no code, no like.

[@TJMoir]

Posted code in a comment. Too big to go in the description.