File content as of revision 1:279026544175:

#include "mbed.h"
#include "MPU6050.h"

MPU6050 mpu(p9,p10);
Serial pc(USBTX,USBRX,9600);
#define PI 3.14159265358979323846f

int Gyro[3];
int Accel[3];

float dt = 0.01;

int main() {
    int i;//回数
    int ax,ay,az,sax,say,saz;//加速度補正
    sax=0,say=0,saz=0;
    int gx,gy,gz,sgx,sgy,sgz;//角速度補正
    sgx=0,sgy=0,sgz=0;
    
    double x,y,z,x1,y1,z1;//台形法
    double ax1,ay1,az1,ox,oy;//オイラー
    x=0,y=0,z=0;
    x1=0,y1=0,z1=0;
    
    double angleX,angleY,ox1,oy1;//相補
    ox1=0,oy1=0;
    
    //カルマンフィルタ
    double newAngle,newRate;
    double Q_angle  =  0.001;
    double Q_gyro   =  0.003;
    double R_angle  =  0.03;
    double x_angle;
    double x_bias = 0;
    double P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    double  y11, S;
    double K_0, K_1;
    double dt=0.005;
    
    //相補とカルマンフィルタの平均
    double AnX;
    
    //補正
    /*//なぜかおかしくなる
     for(i=0;i<100;i++){
        mpu.readAccelData(Accel);
        ax = Accel[0];
        ay = Accel[1];
        az = Accel[2];
        
        sax = sax + ax;
        say = say + ay;
        saz = saz + az;
        
        wait(dt);
        }
    
    sax = sax/100;
    say = say/100;
    saz = saz/100;
    */
    
    for(i=0;i<100;i++){
        mpu.readGyroData(Gyro);
        gx = Gyro[0];//x軸方向
        gy = Gyro[1];//y軸方向
        gz = Gyro[2];//z軸方向
        
        sgx = sgx + gx;
        sgy = sgy + gy;
        sgz = sgz + gz;
        
        wait(dt);
        }
    
    sgx = sgx/100;
    sgy = sgy/100;
    sgz = sgz/100;
    
    while(1){
        //加速度計測
        mpu.readAccelData(Accel);
        ax = Accel[0]-123;
        ay = Accel[1]+60;
        az = Accel[2]+110;
        //printf("%d,%d,%d\r\n",ax,ay,az);
        
        ax1 = (double)ax*0.000597964111328125;
        ay1 = (double)ay*0.000597964111328125;
        az1 = (double)az*0.000597964111328125;
        //printf("%.2f,%.2f,%.2f\r\n",ax1,ay1,az1); 
        
        //オイラー角
        ox = atan(ay1/az1)*180/PI;
        oy = atan(ax1/sqrt(ay1*ay1+az1*az1))*180/PI;
        //printf("%f,%f\r\n",ox,oy);
        
        //角速度計測
        mpu.readGyroData(Gyro);
        gx = Gyro[0]-sgx;//x軸方向
        gy = Gyro[1]-sgy;//y軸方向*0.291796151
        gz = Gyro[2]-sgz;//z軸方向
        //printf("%d,%d,%d\r\n",gx,gy,gz);
        
        double gX = (double)gx*250.0/32768.0;
        double gY = (double)gy*250.0/32768.0;
        double gZ = (double)gz*250.0/32768.0;
        //printf("%.2f,%.2f,%.2f\r\n",gX,gY,gZ);//角速度
        
        //台形法(これいらんな)
        /*
        x = x + (x1+(gX *180/ PI))*dt/2;
        y = y + (y1+(gY *180/ PI))*dt/2;
        z = z + (z1+(gZ *180/ PI))*dt/2;
        
        x1 = gX* 180/ PI;
        y1 = gY* 180/ PI;
        z1 = gZ* 180/ PI;
        //pc.printf("%.2f,%.2f,%.2f\r\n",x,y,z);
        */
        
        //相補フィルタ
        angleX = 0.95*(ox1+(gX*dt*180/PI))+0.05*ox;
        angleY = 0.95*(oy1+(gY*dt*180/PI))+0.05*oy;
        ox1 = ox;
        oy1 = oy; 
        //pc.printf("%f,%f\r\n",angleX,angleY);
        
        //カルマンフィルタ(？http://meerstern.seesaa.net/article/417550787.html)
        newAngle = ox;
        newRate = gx*dt;
        
        x_angle = x_angle + dt * (newRate - x_bias);
      
        P_00 = P_00 + dt * (dt * P_11 - P_01 - P_10 + Q_angle);
        P_01 = P_01 - dt * P_11;
        P_10 = P_10 - dt * P_11;
        P_11 = P_11 - Q_gyro * dt;

        y11 = newAngle - x_angle;
        S = P_00 + R_angle;
        K_0 = P_00 / S;
        K_1 = P_10 / S;

        x_angle = x_angle +  K_0 * y11;
        x_bias  = x_bias +  K_1 * y11;
        P_00 = P_00 - K_0 * P_00;
        P_01 = P_01 - K_0 * P_01;
        P_10 = P_10 - K_1 * P_00;
        P_11 = P_11 - K_1 * P_01;
        //printf("%f\r\n",x_angle);
        
        //xの平均角度
        AnX = (x_angle+angleX)/2;
        printf("%f\r\n",AnX);
        
        
        wait(dt);
        }
}