Revision 10:f2ef74678956, committed 2017-04-26

Comitter:

mfurukawa

Date:

Wed Apr 26 07:52:10 2017 +0000

Parent:

9:e700b2d586d6

Commit message:

public revision

Changed in this revision

diff -r e700b2d586d6 -r f2ef74678956 AHRS/MadgwickAHRS.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS/MadgwickAHRS.c	Wed Apr 26 07:52:10 2017 +0000
@@ -0,0 +1,227 @@
+//=====================================================================================================
+// MadgwickAHRS.c
+//=====================================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date         Author          Notes
+// 29/09/2011   SOH Madgwick    Initial release
+// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
+// 19/02/2012   SOH Madgwick    Magnetometer measurement is normalised
+//
+//=====================================================================================================
+
+//---------------------------------------------------------------------------------------------------
+// Header files
+
+#include "MadgwickAHRS.h"
+#include <math.h>
+
+//---------------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreq  512.0f      // sample frequency in Hz
+#define betaDef     0.1f        // 2 * proportional gain
+
+//---------------------------------------------------------------------------------------------------
+// Variable definitions
+
+volatile float beta = betaDef;                              // 2 * proportional gain (Kp)
+volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+float invSqrt(float x);
+
+//====================================================================================================
+// Functions
+
+//---------------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+    float hx, hy;
+    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
+    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
+        return;
+    }
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+        // Normalise accelerometer measurement
+        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;   
+
+        // Normalise magnetometer measurement
+        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+        mx *= recipNorm;
+        my *= recipNorm;
+        mz *= recipNorm;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0mx = 2.0f * q0 * mx;
+        _2q0my = 2.0f * q0 * my;
+        _2q0mz = 2.0f * q0 * mz;
+        _2q1mx = 2.0f * q1 * mx;
+        _2q0 = 2.0f * q0;
+        _2q1 = 2.0f * q1;
+        _2q2 = 2.0f * q2;
+        _2q3 = 2.0f * q3;
+        _2q0q2 = 2.0f * q0 * q2;
+        _2q2q3 = 2.0f * q2 * q3;
+        q0q0 = q0 * q0;
+        q0q1 = q0 * q1;
+        q0q2 = q0 * q2;
+        q0q3 = q0 * q3;
+        q1q1 = q1 * q1;
+        q1q2 = q1 * q2;
+        q1q3 = q1 * q3;
+        q2q2 = q2 * q2;
+        q2q3 = q2 * q3;
+        q3q3 = q3 * q3;
+
+        // Reference direction of Earth's magnetic field
+        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+        _2bx = sqrt(hx * hx + hy * hy);
+        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+        _4bx = 2.0f * _2bx;
+        _4bz = 2.0f * _2bz;
+
+        // Gradient decent algorithm corrective step
+        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+        // Apply feedback step
+        qDot1 -= beta * s0;
+        qDot2 -= beta * s1;
+        qDot3 -= beta * s2;
+        qDot4 -= beta * s3;
+    }
+
+    // Integrate rate of change of quaternion to yield quaternion
+    q0 += qDot1 * (1.0f / sampleFreq);
+    q1 += qDot2 * (1.0f / sampleFreq);
+    q2 += qDot3 * (1.0f / sampleFreq);
+    q3 += qDot4 * (1.0f / sampleFreq);
+
+    // Normalise quaternion
+    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+}
+
+//---------------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+        // Normalise accelerometer measurement
+        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;   
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0 = 2.0f * q0;
+        _2q1 = 2.0f * q1;
+        _2q2 = 2.0f * q2;
+        _2q3 = 2.0f * q3;
+        _4q0 = 4.0f * q0;
+        _4q1 = 4.0f * q1;
+        _4q2 = 4.0f * q2;
+        _8q1 = 8.0f * q1;
+        _8q2 = 8.0f * q2;
+        q0q0 = q0 * q0;
+        q1q1 = q1 * q1;
+        q2q2 = q2 * q2;
+        q3q3 = q3 * q3;
+
+        // Gradient decent algorithm corrective step
+        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+        s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+        s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+        // Apply feedback step
+        qDot1 -= beta * s0;
+        qDot2 -= beta * s1;
+        qDot3 -= beta * s2;
+        qDot4 -= beta * s3;
+    }
+
+    // Integrate rate of change of quaternion to yield quaternion
+    q0 += qDot1 * (1.0f / sampleFreq);
+    q1 += qDot2 * (1.0f / sampleFreq);
+    q2 += qDot3 * (1.0f / sampleFreq);
+    q3 += qDot4 * (1.0f / sampleFreq);
+
+    // Normalise quaternion
+    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+}
+
+//---------------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+float invSqrt(float x) {
+    float halfx = 0.5f * x;
+    float y = x;
+    long i = *(long*)&y;
+    i = 0x5f3759df - (i>>1);
+    y = *(float*)&i;
+    y = y * (1.5f - (halfx * y * y));
+    return y;
+}
+
+//====================================================================================================
+// END OF CODE
+//====================================================================================================
\ No newline at end of file

diff -r e700b2d586d6 -r f2ef74678956 AHRS/MadgwickAHRS.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS/MadgwickAHRS.h	Wed Apr 26 07:52:10 2017 +0000
@@ -0,0 +1,31 @@
+//=====================================================================================================
+// MadgwickAHRS.h
+//=====================================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date         Author          Notes
+// 29/09/2011   SOH Madgwick    Initial release
+// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
+//
+//=====================================================================================================
+#ifndef MadgwickAHRS_h
+#define MadgwickAHRS_h
+
+//----------------------------------------------------------------------------------------------------
+// Variable declaration
+
+extern volatile float beta;             // algorithm gain
+extern volatile float q0, q1, q2, q3;   // quaternion of sensor frame relative to auxiliary frame
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+
+#endif
+//=====================================================================================================
+// End of file
+//=====================================================================================================

diff -r e700b2d586d6 -r f2ef74678956 main.cpp
--- 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");
     }
 }