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??

[@sebastiantreu8243]

please correct me if I'm wrong but this works only on ground. This may not work on an airplane for coordinated turns where you are moving forward in space but also G force vector would produce unstable values...

[@TJMoir]

The first real application of the Kalman filter was in the Apollo space programme. There is a NASA report somewhere - old one that gives the equations for inertial navigation. They had 12 states I think and it was implemented digitally (surprising for the 1960s) with a custom computer. The KF had the equations of the Apollo craft built in as well. This is a simplified steady state Kalman filter I have here but should work ok in a quadcopter. Not tried it though, only tried it on simple self balancing machines.

[@phillipmaser132]

Project requires counting sections of pipe. If I shoot missile like object down a PVC pipe; it count sections of pipe as it passes the joints? I want to use acceleration and Gyro to find these section as a peak in travel. Possibly a Kalman filter can help? I need to know the velocity traveled so I know when to stop at a distance. Threshold could vary between sections not good.

[@TJMoir]

If you can measure noisy position you can get velocity and acceleration via a Kalman filter. You will need the equations of motion of course in your KF. Counting the sections I suppose is position.

[@monishagowri1902]

can i get the code?

[@satyabratasenapati7376]

Very useful 😂

[@KennyFairweather]

Nice

[@TJMoir]

I re did the other one in higher res too.

[@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

[@andresilva_bjj]

no code, no like.

[@TJMoir]

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