关于卡尔曼滤波，大家都过来看眼呀~~~~~~~~~~~求教

liu1guo2qiang3

都一个多月了，觉得滤波还是不行。现在求助大家帮我看看。不知道我的滤波行不行，首先用的是卡尔曼滤波程序

float Q_angle=0.001, Q_gyro=0.001, R_angle=0.0008, dt=0.010;

                        //注意：dt的取值为kalman滤波器采样时间;
float P[2][2] = {
                                                        { 1, 0 },
                                                        { 0, 1 }
                                                };
       
float Pdot[4] ={0,0,0,0};

const char C_0 = 1;

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)                        //gyro_m:gyro_measure
{
        angle+=(gyro_m-q_bias) * dt;//先验估计

        Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
        Pdot[1]=- P[1][1];
        Pdot[2]=- P[1][1];
        Pdot[3]=Q_gyro;
       
        P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
        P[0][1] += Pdot[1] * dt;
        P[1][0] += Pdot[2] * dt;
        P[1][1] += Pdot[3] * dt;
       
       
        angle_err = angle_m - angle;//zk-先验估计
       
       

        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;//Kk
        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;//输出值（后验估计）的微分 = 角速度

}



我通过中断采样，周期是10ms一次，陀螺仪和加速度器均采样一次
然后输入卡尔曼进行滤波。用的那个串口程序发送就要9ms左右，所以暂时没提高采样频率
其次我用是弧度制。觉得按理应该就是一个比例问题，不是很大。
有空弄成角度后也发上来给大伙看看。
我下列的图像，蓝色的加速度传感器实际测到的值
红色为滤波后加速度计的值对于程序中的angle
粉红色是陀螺仪采到的值
黄色是滤波后陀螺仪的值对于程序中的angle_dot

这张图中，白色圆圈的地方不知道怎么滤掉。我用手往前倒，加速度理想是增加的，这里却有个下降的过程。float Q_angle=0.001, Q_gyro=0.0015, R_angle=0.001, dt=0.010是此时的参数
我觉得这个时刻滤波出来的数据均由延迟，相对于实际都是有偏移的。

float Q_angle=0.008, Q_gyro=0.0015, R_angle=0.001, dt=0.010;这个时刻的参数。这个会靠近实际数据。
但仍像第一张图似的。开始倒下去的时候加速度有负值。
其他图大家也看看那个符合点。

然后我对陀螺仪有些疑问，陀螺仪积分是按手册中0.76度/mv/s的么。就是按时间转换？可是我在用手摇摆的时候，陀螺仪最大的时刻是差不多对应车子平衡位置。这样不就不是角度，而是对应角速度了么？可以通过积分到到角度么？

大伙都给点提示和支持呀。挣扎好久了呀。

liu1guo2qiang3

那个陀螺仪积分是不是每次都要累加的呀？如果是，那我就要推翻以上的东西。我就拿采一次值去弄。。。。。。。。

有-木-友

LJG

不行，毛刺太多，跟踪有延迟

yssdsz

延时，我觉得第二个图还是不错的。

wo520

其实不需要太过看重波形，只要达到要求就行！不滞后，趋势能跟踪上就好了！！

liu1guo2qiang3

LJG 发表于 2012-3-9 02:50
不行，毛刺太多，跟踪有延迟

嗯，是加速度么？
我之前那个陀螺仪没有累加要重新弄的
下次再传麻烦在看看了

liu1guo2qiang3

wo520 发表于 2012-3-10 19:24
其实不需要太过看重波形，只要达到要求就行！不滞后，趋势能跟踪上就好了！！

现在还是觉得陀螺仪有问题
会一直累加上去的
我上面的图没有累加
后来做了累加的
陀螺仪就会累加一直到最大值

wo520

liu1guo2qiang3 发表于 2012-3-11 21:48
现在还是觉得陀螺仪有问题
会一直累加上去的
我上面的图没有累加

陀螺仪累加上去，那是很正常的！！因为陀螺仪有积分累加误差，如果陀螺仪积分的角度是正确的话，那么就没必要用滤波得到一个新角度了！！

liu1guo2qiang3

wo520 发表于 2012-3-12 13:26
陀螺仪累加上去，那是很正常的！！因为陀螺仪有积分累加误差，如果陀螺仪积分的角度是正确的话，那么就没 ...

现在你看看那行不呀。有一个是陀螺仪会漂上去的。
还有的就是放大点的。
黄色是输出。对应angle_dot