智能车制作

标题: 卡尔曼滤波 [打印本页]

作者: wsdyi100 时间: 2012-11-13 22:23
标题: 卡尔曼滤波
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 时间: 2012-11-13 23:02
沙发

作者: wsdyi100 时间: 2012-11-14 10:14
高手来解答一下啊，不然我的贴要石沉大海了

作者: wangyao19920204 时间: 2013-1-7 20:18
楼主，现在你可以用卡尔曼算法来融合角度了吗？

作者: 124694679 时间: 2013-1-8 22:13
学习学习！

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

作者: wsdyi100 时间: 2013-2-21 20:29

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

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

欢迎光临智能车制作 (http://dns.znczz.com/)