|
|
先贴代码吧。
static float angle, angle_dot; //外部需要引用的变量
//Kalman滤波
//-------------------------------------------------------
//-------------------------------------------------------
static float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.5; //注意：dt的取值为kalman滤波器采样时间;
static float Pk[2][2] = { {1, 0 }, {0, 1 }};
static float Pdot[4] ={0,0,0,0};
static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m) //角度 角速度
{
angle = angle + (gyro_m-q_bias) * dt; //角度 = 角度 +（角速度测量值-偏移）*时间dt
// X(k|k-1) = X(k-1|k-1) + U(k) (1) // X(k|k-1)= X(k-1|k-1)+ U(k) (1)
Pdot[0]=Q_angle - Pk[0][1] - Pk[1][0];
Pdot[1]=- Pk[1][1];
Pdot[2]=- Pk[1][1];
Pdot[3]=Q_gyro;
Pk[0][0]=Pk[0][0] + Pdot[0] * dt;
//P(k|k-1)
Pk[0][1]=Pk[0][1] + Pdot[1] * dt;
Pk[1][0]=Pk[1][0] + Pdot[2] * dt;
Pk[1][1]=Pk[1][1] + Pdot[3] * dt;
angle_err = angle_m - angle; // Z(k)-H X(k|k-1)
//Z(k) - X(k|k-1)
PCt_0 = C_0 * Pk[0][0]; //P(k|k-1)*H’
PCt_1 = C_0 * Pk[1][0];
E = R_angle + C_0 * PCt_0;
// R + (H P(k|k-1) H’)
K_0 = PCt_0 / E;
//Kg(k) = P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * Pk[0][1];
Pk[0][0] =Pk[0][0] - K_0 * t_0;
Pk[0][1] =Pk[0][1] - K_0 * t_1;
Pk[1][0] =Pk[1][0] - K_1 * t_0;
Pk[1][1] =Pk[1][1] - K_1 * t_1;
angle =angle + K_0 * angle_err; //X(k|k)= X(k|k-1) + Kg(k)(Z(k)-H X(k|k-1)) ……… (3)
q_bias =q_bias + K_1 * angle_err;
angle_dot = gyro_m-q_bias;
}
第一句最好懂，就是卡尔曼的第一条公式，增加了q_bias 修正值。（这个值如何进行循环修正就不知道了）
Pdot数组和Pk数组就不太明白了，PK应该是协方差的意思吧。还有协方差到底是如何进行计算的也不太清楚。网上的资料太深了。
angle_err 好理解 公式三的部分值，。。。。。。。
我把自己觉得代码对于的公式都标出来了。当然也不一定对应了。。
其他的就没头绪了 ，网上资料也不好找，协方差和装置矩阵的计算也看不懂。。。
所以希望大家一起分析啦。就当学习咯。。。网上大牛的文章又看不懂，而且和代码对应不起来啊 |
|
发表于 2012-9-3 15:58:20
楼主
