我的自平衡小车D2——加速度计与陀螺仪获取姿态参数的差异

萧芸凤 · 发表于 2012-9-21 17:59:19

黑马发表于 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] ;

复制代码

yimenwang · 发表于 2012-9-26 20:35:04

楼主,给个能用的代码,可以不,照顾一下小白嘛

gzwsc2007 · 发表于 2012-12-26 20:45:26

马克了！楼主我也在用你用的那个板子 3合一~哇哈哈~ 我正在给我的航模飞机弄一个惯性参考系统~

滤波的算法有时间再研究了= = 好高深的样子~ 我现在用的是最最最弱智的方法——取加权平均值。。我用加速度仪和陀螺仪各算出一个角度，然后给算加权平均。。。然后每隔一段时间，只要陀螺仪的读数没变化（飞机静止或匀速运动），就用加速度仪的读数来校正~~

伟创电子工作室 · 发表于 2013-5-9 18:45:15

简直就是神器啊

火火火 · 发表于 2013-6-16 23:48:31

不错，这几天我也在搞这个传感器，顶下！！

沙尔0906 · 发表于 2013-11-30 11:25:30

楼主好厉害，最近也在做这个东东，感觉好难

☆与☆ · 发表于 2014-2-16 20:51:01

陀螺仪偏移量是怎么得到的？

chd77903499 · 发表于 2014-7-28 14:26:43

darkorigin 发表于 2012-9-2 15:29
谢谢~~~呵呵，我用了另外的一个开源的东西搞起来了，很强大。

请问是你用的是什么啊？

darkorigin · 发表于 2014-7-29 09:01:26

chd77903499 发表于 2014-7-28 14:26
请问是你用的是什么啊？

MWC开源代码~~~

kaizhiyu · 发表于 2014-9-10 19:21:30

超级厉害佩服佩服多谢分享

siecnbj · 发表于 2015-11-26 22:03:58

很好的资料

MWD--文化易人 · 发表于 2018-3-5 22:30:22

好贴，顶，本人水平有限，学习了收藏了

帐号		自动登录	找回密码
密码			注册