感谢Malc的意见，搜了一下互补滤波，我的理解是这种滤波方式就是按一定的权重对两个值进行叠加，很直观的 ...
好不容易找找到了卡尔曼滤波的介绍，可是等到把它转化成arduino语言时发现不知道该如何用来滤波，没有输结果，而且整个过程中都是行列式，有限的几个输出也是行列式。
代码如下，只有函数，没有主程序，现在不知道该如何。加速度计计算的角度是x，陀螺仪是u，那么输出的结果应该也是个角度，但是不知道该如何进行。
KalmanInit(); /////参数初始化
float KalmanFileter(float x,float u )
{
float temp6,temp8;
float temp1,temp2,temp5,temp7,temp9,;
float temp3,temp4,temp10,temp11;
//////第一步：计算预测的现在状态X_estimate//////////
/*
| a11 a12 | | X1 | | T1 |
| a21 a22 | X | X2 | = | T2 |
*/
for( i = 0 ; i < 2 ; i++ )
{
for( j = 0 ; j < 2 ; j++ )
{
temp1 = temp + A * X_optimal; //A［2X2］X X_optimal［2X1］＝ temp1［2X1］ 结果为列矩阵
}
}
/*
| t1 | | T1 |
| t2 | X g = | T2 |
*/
for( i = 0 ; i < 2 ; i++ )
{
temp2 = B * u; //temp2 为［2X1］列矩阵
}
/*
| t1 | | T1 | | x1 |
| t1 | + | T2 | = | x2 |
*/
for ( i = 0 ; i < 2 ; i++ )
{
X_estimate = temp1 + temp2; //X_estimate列矩阵
}
/////第二步：计算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 = temp3 + A * P_optimal; //A X P_optimal = temp3
}
}
}
/*
| 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 = temp4 + temp3 * A; //temp3 XAT = temp4 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 = temp4 + Q; //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 = temp5 + H * P_estimate; //H[1X2］X P_estimate［2X2］＝ temp5［1X2］结果为行矩阵
}
}
/*
| P1 |
| H2 H2 | X | P2 | = R
*/
for ( i = 0 ; i < 2 ; i++ )
{
temp6 = temp6 + temp5 * H ; //［1X2］X［2X1］ ＝ R 结果为实数
}
temp6 = temp6 + R ;
for( i = 0 ; i < 2 ; i++ )
{
for( j = 0 ; j < 2 ; j++ )
{
temp7 = temp7 + P_estimate * H; // X = 结果为列矩阵
}
}
for ( i = 0 ; i < 2 ; i++)
{
Kg = temp7 / temp6;
}
////////////第四步：计算最优状态值X_optimal/////////
for ( i = 0 ; i < 2 ; i++ )
{
temp8 = temp8 + H * X_estimate ; //［1X2］X［2X1］ ＝ R 结果为实数
}
temp8 = x - temp8;
for ( i = 0 ; i < 0 ; i++ )
{
temp9 = temp8 * Kg;
X_optimal = X_estimate + temp9;
}
//////////第五步：最优值的协方差////////////
/*
| k1 | | T11 T12 |
| k2 | X | H1 H2 | = | T21 T22 |
*/
for( i = 0 ; i < 2 ; i++ )
{
for( j = 0 ; j < 2 ; j++ )
{
temp10 = Kg * H; // X =
}
}
/*
| a11 a12 | | X1 | | T1 |
| a21 a22 | X | X2 | = | T2 |
*/
for( i = 0 ; i < 2 ; i++ )
{
for( j = 0 ; j < 2 ; j++ )
{
temp11 = 1 - temp10;
}
}
/*
| 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 = P_optimal + temp11 * P_estimate;
}
}
}
////////////////完成///////////////////////////////
}
void KalmanFileter2( float x,u)
{
//KalmanFileter(accangle ,gyrangle);
kalmanangle = KalmanFileter(x ,u);
kalmanangle = KalmanFileter(x ,u);
kalmanangle = KalmanFileter(x ,u);
}
void KalmanInit(void)
{
float T=0.02;
float A={ { 1, -0.02},{ 0, 1} };
float B={ 10.2,0};
float Q={ { 0.01, 0 },{ 0, 0.005 } };
float H = { 0.01,0.05 };
float R=0.056;
float X_optimal={ 0,0 }
float P_optimal={ { 0.005, 0.005 },{ 0.005, 0.005 } }
}
float T;
float A;
float B;
float Q;
float H;
float R;
float X_optimal;X_estimate;
float P_optimal,P_estimate;
float X_optimal,P_optimal,Kg ;
楼主,给个能用的代码,可以不,照顾一下小白嘛:'( 马克了！楼主我也在用你用的那个板子 3合一~哇哈哈~ 我正在给我的航模飞机弄一个惯性参考系统~:)
滤波的算法有时间再研究了= = 好高深的样子~ 我现在用的是最最最弱智的方法——取加权平均值。。我用加速度仪和陀螺仪各算出一个角度，然后给算加权平均。。。 然后每隔一段时间，只要陀螺仪的读数没变化（飞机静止或匀速运动），就用加速度仪的读数来校正~~ 简直就是神器啊 不错，这几天我也在搞这个传感器，顶下！！ 楼主好厉害，最近也在做这个东东，感觉好难 陀螺仪偏移量是怎么得到的？ darkorigin 发表于 2012-9-2 15:29 static/image/common/back.gif
谢谢~~~呵呵，我用了另外的一个开源的东西搞起来了，很强大。
请问是你用的是什么啊？ chd77903499 发表于 2014-7-28 14:26 static/image/common/back.gif
请问是你用的是什么啊？
MWC开源代码~~~ 超级厉害佩服佩服多谢分享 很好的资料 好贴，顶，本人水平有限，学习了收藏了
页:
1
[2]