Embedded Kalman Filtering

[@DRAMBgo]

Thank you for posting this content, very useful and explicative

[@ZOLDAIOQUE88MADACI]

amazing as always sir. is it possible to show us the same application with myrio i will be greatly thankful

[@TJMoir]

I think in earlier videos I have that somewhere. I can't repeat it because I am now retired and don't have access to a myRio any more.

[@TheDailyMemesShow]

​@@TJMoirRetired?
What do you mean!?
Real Value does not know what 'retiring' means.

[@dreambigedge4139]

Really great video, cam I get the code plz?

[@TJMoir]

// 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);

}

[@yes-man1112]

Professor, Thanks very much, very useful and much interesting than my university professor