卡尔曼滤波

wsdyi100

void Kalman_Filter（float angle_m,float gyro_m） //gyro_m:陀螺仪实测值
{ angle+=（gyro_m-q_bias） × dt;
Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1]; Pdot[2]=- P[1][1]; Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] × dt;
P[0][1] += Pdot[1] × dt;
P[1][0] += Pdot[2] × dt;
P[1][1] += Pdot[3] × dt;
angle_err = angle_m - angle;
PCt_0 = C_0 × P[0][0]; PCt_1 = C_0 × P[1][0];
E = R_angle + C_0 × PCt_0;
K_0 = PCt_0 / E;K_1 = PCt_1 / E;
t_0 = PCt_0; t_1 = C_0 × P[0][1];
P[0][0] -= K_0 × t_0;
P[0][1] -= K_0 × t_1;
P[1][0] -= K_1 × t_0;  //更新协方差P[1][0]我觉得应该是P[1][0] -= K_1 * t_2;其中t_2 = C_0 * P[1][0];
P[1][1] -= K_1 × t_1;
angle += K_0 × angle_err;后验估计倾角误差
q_bias += K_1 × angle_err;//q_bias我认为是系统的角速度误差，既然是角速度误差，
那它为什么等于角度误差和卡尔曼增益的乘积
angle_dot = gyro_m-q_bias;

你是我/sun

沙发

wsdyi100

高手来解答一下啊，不然我的贴要石沉大海了

wangyao19920204

楼主，现在你可以用卡尔曼算法来融合角度了吗？

124694679

学习学习！

xiongcaifei

楼主，我看到有好几种卡尔曼滤波的程序，有的有平方求协差的。不知道他们有什么不同啊？？

wsdyi100

xiongcaifei 发表于 2013-1-22 14:28
楼主，我看到有好几种卡尔曼滤波的程序，有的有平方求协差的。不知道他们有什么不同啊？？

几种卡尔曼滤波？  我值看到了这种