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

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

200802030103 发表于 2012-5-11 14:34
我做的卡拉曼滤波效果图

觉得有点滞后。不过感觉可以直立试试。

200802030103

我做的卡拉曼滤波效果图

liu1guo2qiang3

有-木-友 发表于 2012-3-9 01:45

上面哪副还算比较靠谱的呀？？

liu1guo2qiang3

玩ㄝ★卟恭 发表于 2012-3-17 14:00
楼主 想要显示波形 是不是一定要按一定的格式发数据呢

嗯。必须按它的规则去发~

玩ㄝ★卟恭

liu1guo2qiang3 发表于 2012-3-16 16:22
首先你看看串口连好了没。最好用别的上位机看看能不能接收字符~~

楼主 想要显示波形 是不是一定要按一定的格式发数据呢  

玩ㄝ★卟恭

liu1guo2qiang3 发表于 2012-3-16 16:22
首先你看看串口连好了没。最好用别的上位机看看能不能接收字符~~

谢谢楼主

liu1guo2qiang3

玩ㄝ★卟恭 发表于 2012-3-15 18:43
想问一下楼主 怎么通过那个visual scope 获得数据的图形的  为什么我用串口接收 却没有波形显示呢 是系统的 ...

首先你看看串口连好了没。最好用别的上位机看看能不能接收字符~~

玩ㄝ★卟恭

想问一下楼主 怎么通过那个visual scope 获得数据的图形的  为什么我用串口接收 却没有波形显示呢 是系统的原因吗  

liu1guo2qiang3

wo520 发表于 2012-3-13 10:04
不行，明显的滞后了！

是陀螺仪滞后还是加速度呀，。应该是加速度吧，