// 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);
Thank you for posting this content, very useful and explicative
amazing as always sir. is it possible to show us the same application with myrio i will be greatly thankful
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.
@@TJMoirRetired?
What do you mean!?
Real Value does not know what 'retiring' means.
Really great video, cam I get the code plz?
// 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);
}
Professor, Thanks very much, very useful and much interesting than my university professor