黑马 发表于 2012-3-23 13:39
感谢Malc的意见,搜了一下互补滤波,我的理解是这种滤波方式就是按一定的权重对两个值进行叠加,很直观的 ...
好不容易找找到了卡尔曼滤波的介绍,可是等到把它转化成arduino语言时发现不知道该如何用来滤波,没有输结果,而且整个过程中都是行列式,有限的几个输出也是行列式。
代码如下,只有函数,没有主程序,现在不知道该如何。加速度计计算的角度是x,陀螺仪是u,那么输出的结果应该也是个角度,但是不知道该如何进行。 - KalmanInit(); /////参数初始化
- float KalmanFileter(float x,float u )
- {
- float temp6,temp8;
- float temp1[2],temp2[2],temp5[2],temp7[2],temp9[2],;
- float temp3[2][2],temp4[2][2],temp10[2][2],temp11[2][2];
- //////第一步:计算预测的现在状态X_estimate//////////
- /*
- | a11 a12 | | X1 | | T1 |
- | a21 a22 | X | X2 | = | T2 |
- */
- for( i = 0 ; i < 2 ; i++ )
- {
- for( j = 0 ; j < 2 ; j++ )
- {
- temp1[i] = temp[i] + A[i][j] * X_optimal[j]; //A[2X2]X X_optimal[2X1]= temp1[2X1] 结果为列矩阵
- }
- }
- /*
- | t1 | | T1 |
- | t2 | X g = | T2 |
- */
- for( i = 0 ; i < 2 ; i++ )
- {
- temp2[i] = B[i] * u; //temp2 为[2X1]列矩阵
- }
- /*
- | t1 | | T1 | | x1 |
- | t1 | + | T2 | = | x2 |
- */
- for ( i = 0 ; i < 2 ; i++ )
- {
- X_estimate[i] = temp1[i] + temp2[i]; //X_estimate[2X1]列矩阵
- }
- /////第二步:计算X_estimate的协方差//////
- /*
- | a11 a12 | | p11 p12 | | T11 T12 |
- | a21 a22 | X | p21 p22 | = | T21 T22 |
- */
- for( i = 0 ; i < 2 ; i++ )
- {
- for( j = 0 ; j < 2 ; j++ )
- {
- for( k = 0 ; k < 2 ; k++ )
- {
- temp3[i][j] = temp3[i][j] + A[i][k] * P_optimal[k][j]; //A[2X2] X P_optimal[2X2] = temp3[2X2]
- }
- }
- }
- /*
- | t11 t12 | | A11 A21 | | T11 T12 |
- | t21 t22 | X | A12 A22 | = | T21 T22 |
- */
- for( i = 0 ; i < 2; i++ )
- {
- for( j = 0 ; j < 2; j++ )
- {
- for( k = 0 ; k < 2 ; k++ )
- {
- temp4[i][j] = temp4[i][j] + temp3[i][k] * A[j][k]; //temp3[2X2] XAT[2X2] = temp4[2X2] AT为A的转置矩阵
- }
- }
- }
- /*
- | t11 t12 | | Q11 Q12 | | P11 P12 |
- | t21 t22 | + | Q21 Q22 | = | P21 P22 |
- */
- for( i = 0 ; i < 2 ; i++ )
- {
- for( j = 0 ; j < 2 ; j++ )
- {
- P_estimate[i][j] = temp4[i][j] + Q[i][j]; //P_estimate[2X2]矩阵
- }
- }
- /////////////第三步:计算得出卡尔曼增益///////////////
- /*
- | P11 P21 |
- | H2 H2 | X | P21 P22 | = | T1 T2 |
- */
- for( i = 0 ; i < 2 ; i++ )
- {
- for( j = 0 ; j < 2 ; j++ )
- {
- temp5[i] = temp5[i] + H[j] * P_estimate[j][i]; //H[1X2]X P_estimate[2X2]= temp5[1X2]结果为行矩阵
- }
- }
- /*
- | P1 |
- | H2 H2 | X | P2 | = R
- */
- for ( i = 0 ; i < 2 ; i++ )
- {
- temp6 = temp6 + temp5[i] * H[i] ; //[1X2]X[2X1] = R 结果为实数
- }
- temp6 = temp6 + R ;
-
- for( i = 0 ; i < 2 ; i++ )
- {
- for( j = 0 ; j < 2 ; j++ )
- {
- temp7[i] = temp7[i] + P_estimate[i][j] * H[j]; // [2X2] X [2x1] = [2x1] 结果为列矩阵
- }
- }
- for ( i = 0 ; i < 2 ; i++ )
- {
- Kg[i] = temp7[i] / temp6;
- }
-
- ////////////第四步:计算最优状态值X_optimal/////////
- for ( i = 0 ; i < 2 ; i++ )
- {
- temp8 = temp8 + H[i] * X_estimate[i] ; //[1X2]X[2X1] = R 结果为实数
- }
-
- temp8 = x - temp8;
- for ( i = 0 ; i < 0 ; i++ )
- {
- temp9[i] = temp8 * Kg[i];
- X_optimal[i] = X_estimate[i] + temp9[i];
- }
-
-
- //////////第五步:最优值的协方差////////////
- /*
- | k1 | | T11 T12 |
- | k2 | X | H1 H2 | = | T21 T22 |
- */
- for( i = 0 ; i < 2 ; i++ )
- {
- for( j = 0 ; j < 2 ; j++ )
- {
- temp10[i][j] = Kg [i]* H[j]; //[2X1] X [1X2] = [2X2]
- }
- }
- /*
- | a11 a12 | | X1 | | T1 |
- | a21 a22 | X | X2 | = | T2 |
- */
- for( i = 0 ; i < 2 ; i++ )
- {
- for( j = 0 ; j < 2 ; j++ )
- {
- temp11[i][j] = 1 - temp10[i][j];
- }
- }
- /*
- | a11 a12 | | p11 p12 | | T11 T12 |
- | a21 a22 | X | p21 p22 | = | T21 T22 |
- */
- for( i = 0 ; i < 2 ; i++ )
- {
- for( j = 0 ; j < 2 ; j++ )
- {
- for( k = 0 ; k < 2 ; k++ )
- {
- P_optimal[i][j] = P_optimal[i][j] + temp11[i][k] * P_estimate[k][j];
- }
- }
- }
-
-
-
- ////////////////完成///////////////////////////////
- }
- void KalmanFileter2( float x[3],u[3])
- {
-
- //KalmanFileter(accangle[0] ,gyrangle[1]);
- kalmanangle[0] = KalmanFileter(x[0] ,u[1]);
- kalmanangle[1] = KalmanFileter(x[1] ,u[0]);
- kalmanangle[2] = KalmanFileter(x[2] ,u[2]);
- }
- void KalmanInit(void)
- {
-
- float T=0.02;
- float A[2][2]={ { 1, -0.02},{ 0, 1} };
- float B[2]={ 10.2,0 };
- float Q[2][2]={ { 0.01, 0 },{ 0, 0.005 } };
- float H[2] = { 0.01,0.05 };
- float R=0.056;
- float X_optimal[2]={ 0,0 }
- float P_optimal[2][2]={ { 0.005, 0.005 },{ 0.005, 0.005 } }
-
- }
- float T;
- float A[2][2];
- float B[2];
- float Q[2][2];
- float H[2];
- float R;
- float X_optimal[2];X_estimate[2];
- float P_optimal[2][2],P_estimate[2][2];
- float X_optimal[2],P_optimal[2][2],Kg[2] ;
复制代码 |