カルマンフィルタリング(2)

一、カルマンアルゴリズム理解実はkalmanアルゴリズムがどのように来たのかを考えなければ、私たちは以下のいくつかの式があることを知るだけでいい.具体的な意味はwikipediaを見ることができる.
二カルマンフィルタリングアルゴリズムの実現ここで私のアルゴリズムはavr単片機で実行されているので、c言語で書かれています.次のコードはavrのタイマに配置してテストのリフレッシュを中断します.オシロスコープでテストしたところ、このアルゴリズムは16 Mの結晶振動下での動作時間が0.35 ms必要であるのに対し、データ収集には3 ms程度必要であるため、選択タイマー時間は8 msである.以前にも1階のkalmanアルゴリズムを書いたことがあるが、自己平衡車に運用すると、こちらは3階で、主にマトリクス演算である.

//kalman.c

float dtTimer   = 0.008;

float xk[9] = {0,0,0,0,0,0,0,0,0};

float pk[9] = {1,0,0,0,1,0,0,0,0};

float I[9]  = {1,0,0,0,1,0,0,0,1};

float R[9]  = {0.5,0,0,0,0.5,0,0,0,0.01};

float Q[9] = {0.005,0,0,0,0.005,0,0,0,0.001};



void matrix_add(float* mata,float* matb,float* matc){

    uint8_t i,j;

    for (i=0; i<3; i++){

       for (j=0; j<3; j++){

          matc[i*3+j] = mata[i*3+j] + matb[i*3+j];

       }

    }

}



void matrix_sub(float* mata,float* matb,float* matc){

    uint8_t i,j;

    for (i=0; i<3; i++){

       for (j=0; j<3; j++){

          matc[i*3+j] = mata[i*3+j] - matb[i*3+j];

       }

    }

}



void matrix_multi(float* mata,float* matb,float* matc){

  uint8_t i,j,m;

  for (i=0; i<3; i++)

  {

    for (j=0; j<3; j++)

   {

      matc[i*3+j]=0.0;

      for (m=0; m<3; m++)

     {

        matc[i*3+j] += mata[i*3+m] * matb[i*3+j];

      }

    }

 }

}



void KalmanFilter(float* am_angle_mat,float* gyro_angle_mat){

uint8_t i,j;

float yk[9];

float pk_new[9];

float K[9];

float KxYk[9];

float I_K[9];

float S[9];

float S_invert[9];

float sdet;



//xk = xk + uk

matrix_add(xk,gyro_angle_mat,xk);

//pk = pk + Q

matrix_add(pk,Q,pk);

//yk =  xnew - xk

matrix_sub(am_angle_mat,xk,yk);

//S=Pk + R

matrix_add(pk,R,S);

//S  invert

sdet = S[0] * S[4] * S[8]

          + S[1] * S[5] * S[6]

          + S[2] * S[3] * S[7]

          - S[2] * S[4] * S[6]

          - S[5] * S[7] * S[0]

          - S[8] * S[1] * S[3];



S_invert[0] = (S[4] * S[8] - S[5] * S[7])/sdet;

S_invert[1] = (S[2] * S[7] - S[1] * S[8])/sdet;

S_invert[2] = (S[1] * S[7] - S[4] * S[6])/sdet;



S_invert[3] = (S[5] * S[6] - S[3] * S[8])/sdet;

S_invert[4] = (S[0] * S[8] - S[2] * S[6])/sdet;

S_invert[5] = (S[2] * S[3] - S[0] * S[5])/sdet;



S_invert[6] = (S[3] * S[7] - S[4] * S[6])/sdet;

S_invert[7] = (S[1] * S[6] - S[0] * S[7])/sdet;

S_invert[8] = (S[0] * S[4] - S[1] * S[3])/sdet;

//K = Pk * S_invert

matrix_multi(pk,S_invert,K);

//KxYk = K * Yk

matrix_multi(K,yk,KxYk);

//xk = xk + K * yk

matrix_add(xk,KxYk,xk);

//pk = (I - K)*(pk)

matrix_sub(I,K,I_K);

matrix_multi(I_K,pk,pk_new);

//update pk

//pk = pk_new;

for (i=0; i<3; i++){

    for (j=0; j<3; j++){

        pk[i*3+j] = pk_new[i*3+j];

    }

  }

}

三カルマンフィルタを用いるここでkalmanフィルタは離散デジタルフィルタであり,反復演算が必要である.ここではアルゴリズムをavrのタイマ割り込みに入れて実行し,再帰演算を行う.

//isr.c

#include "kalman.h"

float mpu_9dof_values[9]={0};

float am_angle[3];

float gyro_angle[3];

float am_angle_mat[9]={0,0,0,0,0,0,0,0,0};

float gyro_angle_mat[9]={0,0,0,0,0,0,0,0,0};



//8MS

ISR(TIMER0_OVF_vect){

//          

TCNT0 = 130 ;  //8ms

sei();

//      

//mpu_9dof_values    g  /s

//        

mpu_getValue6(&mpu_9dof_values[0],&mpu_9dof_values[1],&mpu_9dof_values[2],&mpu_9dof_values[3],&mpu_hmc_values[4],&mpu_hmc_values[5]);

//     

compass_mgetValues(&mpu_9dof_values[6],&mpu_9dof_values[7],&mpu_9dof_values[8]);



accel_compass2angle(&mpu_9dof_values[0],&mpu_9dof_values[6],am_angle);

gyro2angle(&mpu_9dof_values[3],gyro_angle);



am_angle_mat[0] = am_angle[0];

am_angle_mat[4] = am_angle[1];

am_angle_mat[8] = am_angle[2];



gyro_angle_mat[0] = gyro_angle[1];

gyro_angle_mat[4] = - gyro_angle[0];

gyro_angle_mat[8] = - gyro_angle[2];



//   

KalmanFilter(am_angle_mat,gyro_angle_mat);



//      

//xk[0] xk[4] xk[8]

}