现在智能小车里用的卡尔曼滤波算法基本是这个.doc

现在智能小车里用的卡尔曼滤波算法基本是这个：void Kalman_Filter(float angle_m,float gyro_m)? ? ? ? ? ? ? ? ? ? ? ? //gyro_m:gyro_measure{? ? ? ? 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][1] -= K_1 * t_1;? ? ? ? ? ? ? ? ? ? ? ? angle? ? ? ? += K_0 * angle_err;? ? ? ? q_bias? ? ? ? += K_1 * angle_err;? ? ? ? angle_dot = gyro_m-q_bias;}这个也不错，但我看不懂，因为我不知道怎么调参数，所以我不相信其输出精度。独轮车做的不太好，原来一直在用别人的驱动，后来感觉还是自己做一下比较好，所以还在做驱动。 现在智能小车里用的卡尔曼滤波算法基本是这个：void Kalman_Filter(float angle_m,float gyro_m)? ? ? ? ? ? ? ? ? ? ? ? //gy ... 这个参数不好调节，就目前的参数而言，滞后严重。但是加大参数了，输出的角速度值收敛太慢，影响控制。还是楼主的公式也不是最直观的，建议直接用公式，那5个公式一步步的写，输出很精准，参数的调节也一目了然。就是现在的几个程序。把卡尔曼的算法弄复杂了再发一些我认为不错的文档吧！首先是一阶互补滤波算法：1、一阶互补滤波这种滤波的原理是对AngleGyro进行高通滤波，而对AngleAccel进行低通滤波，最后再将两者加合，输出姿态角估计值。其公式如下：angle = a * (angle + AngleRateGyro * dt) + (1 - a) * AngleAccel;高通滤波的目的是过滤掉长期的变量，而让短期的变量通过，这可以用来去掉陀螺仪的漂移；低通滤波的目的是过滤掉短期的变量，而让长期的变量通过，这可以用来去掉加速度计的噪声。为了确定滤波系数a，得确定一个时间常数tau，tau = a * dt / (1 - a);a = tau / (tau + dt);dt为滤波取样时间。tau代表的是你对陀螺仪的信任度和对加速度计的信任度的界限。当时间小于tau，AngleGyro处于优先地位，AngleAccel的噪声将被过滤。当时间大于tau时，AngleAccel的平均值将被优先考虑，这时的AngleGyro将产生漂移。tau的选择将取决于AngleGyro何时产生漂移，观察数据拟合曲线即可确定。我们选取tau = 1s，带入以下程序中：void ComplementaryFilter(float AngleRateGyro,float AngleAccel){//tau is a time constant//a = tau / (tau + dt);//angle = a * (angle + AngleRateGyro * dt) + (1 - a) * AngleAccelfloat tau =