MPU-9250 with Kalman Filter
Dependencies: ADXL362-helloworld MPU9250_SPI mbed
Fork of ADXL362-helloworld by
Diff: main.cpp
- Revision:
- 10:f2ef74678956
- Parent:
- 9:e700b2d586d6
--- a/main.cpp Fri Jun 17 06:07:57 2016 +0000 +++ b/main.cpp Wed Apr 26 07:52:10 2017 +0000 @@ -10,6 +10,7 @@ #include "mbed.h" #include "MPU9250.h" +#include "AHRS/MadgwickAHRS.h" /* MOSI (Master Out Slave In) p5 @@ -20,65 +21,161 @@ // https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp -int main() -{ - - Serial pc(USBTX, USBRX); - pc.baud(115200); - SPI spi(p5, p6, p7); +class KalmanFilter { + private: + float P, K, xhat, Q, R; + public: + KalmanFilter(); + KalmanFilter(float _Q, float _R); + float update(float obs); + +}; +KalmanFilter::KalmanFilter(){ + P = 0.0; // Convariance Matrix + K = 0.0; // Kalman Gain + xhat = 0.0;// Initial Predicted Value + Q = 1e-3; // Error + R = 0.01; + } + + // override +KalmanFilter::KalmanFilter(float _Q, float _R){ + P = 0.0; // Convariance Matrix + K = 0.0; // Kalman Gain + xhat = 0.0;// Initial Predicted Value + Q = _Q; // Error + R = _R; + } + +float KalmanFilter::update(float obs){ + // Predict + float xhat_m = xhat ; // xhat[k-1] + float P_m = P + Q; // P[k-1] + + // Update + float S = P_m + R; // Remained Error + K = P_m / S; // Update Kalman Gain + xhat = xhat_m + K * (obs - xhat_m); // predicted value + P = (1 - K) * P_m; // Convariance Matrix of Error BTW True Value and Predicted True Value + + return xhat; +} - //define the mpu9250 object - mpu9250_spi *imu[2]; +//define the mpu9250 object +mpu9250_spi *imu[2]; +Serial pc(USBTX, USBRX); +SPI spi(p5, p6, p7); +KalmanFilter *kf[12]; +Ticker ticker; +float theta[2][3],thetaOffset[2][3],x,y,z; + +// Calibration wait +int count = 100; + +// Sampling Term +float smplT = 10; //ms + +void init(void){ + theta[0][0] = 0; + theta[0][1] = 0; + theta[0][2] = 0; + theta[1][0] = 0; + theta[1][1] = 0; + theta[1][2] = 0; + thetaOffset[0][0] = 0; + thetaOffset[0][1] = 0; + thetaOffset[0][2] = 0; + thetaOffset[1][0] = 0; + thetaOffset[1][1] = 0; + thetaOffset[1][2] = 0; + + pc.baud(115200); + imu[0] = new mpu9250_spi(spi, p8); imu[1] = new mpu9250_spi(spi, p9); - + + for(int i=0; i<12; i++) + kf[i] = new KalmanFilter(1e-3, 0.001); + for(int i=0; i<2; i++) { - + imu[0]->deselect(); imu[1]->deselect(); imu[i]->select(); if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250 printf("\nCouldn't initialize MPU9250 via SPI!"); + wait(90); } printf("\nWHOAMI=0x%2x\n",imu[i]->whoami()); //output the I2C address to know if SPI is working, it should be 104 - wait(1); + wait(0.1); printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros - wait(1); + wait(0.1); printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G)); //Set full scale range for accs - wait(1); + wait(0.1); printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami()); wait(0.1); imu[i]->AK8963_calib_Magnetometer(); wait(0.1); } - imu[0]->select(); - imu[1]->deselect(); - while(1) { + +} - //myled = 1; +void eventFunc(void) +{ + count--; + + for(int i=0; i<2; i++) { + + imu[0]->deselect(); + imu[1]->deselect(); - //wait_us(1); + imu[i]->select(); + imu[i]->read_acc(); + imu[i]->read_rot(); - for(int i=0; i<2; i++) { + x = kf[i*6 ]->update(imu[i]->gyroscope_data[0]) - thetaOffset[i][0]; + y = kf[i*6+1]->update(imu[i]->gyroscope_data[1]) - thetaOffset[i][1]; + z = kf[i*6+2]->update(imu[i]->gyroscope_data[2]) - thetaOffset[i][2]; + theta[i][0] += x * smplT / 1000 ; // x(n) = x(n-1) + dx*dt + theta[i][1] += y * smplT / 1000 ; + theta[i][2] += z * smplT / 1000 ; - imu[0]->deselect(); - imu[1]->deselect(); + if (count == 0){ + thetaOffset[i][0] = x; + thetaOffset[i][1] = y; + thetaOffset[i][2] = z; - imu[i]->select(); - imu[i]->read_acc(); - imu[i]->read_rot(); - - printf("%10.3f,%10.3f,%10.3f %10.3f,%10.3f,%10.3f ", - imu[i]->gyroscope_data[0], - imu[i]->gyroscope_data[1], - imu[i]->gyroscope_data[2], - imu[i]->accelerometer_data[0], - imu[i]->accelerometer_data[1], - imu[i]->accelerometer_data[2] + theta[i][0] = 0; + theta[i][1] = 0; + theta[i][2] = 0; + } + if(count < 0) + + printf("%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f ", + x,y,z, + kf[i*6+3]->update(imu[i]->accelerometer_data[0]), + kf[i*6+4]->update(imu[i]->accelerometer_data[1]), + kf[i*6+5]->update(imu[i]->accelerometer_data[2]), + theta[i][0], + theta[i][1], + theta[i][2] ); + + } + printf("\n"); +} +int main() +{ + + init(); + + ticker.attach_us(eventFunc, smplT * 1000); // 10ms = 100Hz + + while(1) { + /* imu[i]->read_all(); printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ", @@ -95,7 +192,5 @@ );*/ //myled = 0; //wait(0.5); - } - printf("\n"); } }