MPU-9250 with Kalman Filter
Dependencies: ADXL362-helloworld MPU9250_SPI mbed
Fork of ADXL362-helloworld by
main.cpp
- Committer:
- mfurukawa
- Date:
- 2017-04-26
- Revision:
- 10:f2ef74678956
- Parent:
- 9:e700b2d586d6
File content as of revision 10:f2ef74678956:
/** * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp * * June 17, 2016 * * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch2) * **/ #include "mbed.h" #include "MPU9250.h" #include "AHRS/MadgwickAHRS.h" /* MOSI (Master Out Slave In) p5 MISO (Master In Slave Out p6 SCK (Serial Clock) p7 ~CS (Chip Select) p8 */ // https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp 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]; 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(0.1); printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros wait(0.1); printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G)); //Set full scale range for accs wait(0.1); printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami()); wait(0.1); imu[i]->AK8963_calib_Magnetometer(); wait(0.1); } } void eventFunc(void) { count--; for(int i=0; i<2; i++) { imu[0]->deselect(); imu[1]->deselect(); imu[i]->select(); imu[i]->read_acc(); imu[i]->read_rot(); 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 ; if (count == 0){ thetaOffset[i][0] = x; thetaOffset[i][1] = y; thetaOffset[i][2] = z; 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 ", imu[i]->Temperature, 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], imu[i]->Magnetometer[0], imu[i]->Magnetometer[1], imu[i]->Magnetometer[2] );*/ //myled = 0; //wait(0.5); } }