卡尔曼滤波方式研究

前段时间在论坛里简单地发了一些关于kalman的理解。
有很多网友顶贴的，趁着今天休息，整理一下前段时间的工作。

有些理解和说法可能不正确，以此抛砖引玉吧。

1，
在google上搜索卡尔曼滤波，很容易找到以下这个帖子：

http://blog.csdn.net/lanbing510/article/details/8828109

这里面很简单形象的解释了kalman的作用。

但是帖子后半段，将kalman回归到了一大堆数学推理、推导上，对非数学专业，或者数学基础尤其是概率和随机过程基础不好的同学来讲实在太过头痛。
帖子最后用matlab实现了kalman，其中很简单地用i，j，k来命名，可能不是太妥当，新手理解的时候，脑袋里面要运行着一个很深的堆栈。
当然，原作者毫无疑问对kalman有着极其深刻的理解，才能实现得如此游刃有余。

在此，只是对第一次接触kalman的同学简单介绍一下自己的理解。帮得到诸君为好，帮不到请勿喷。

2，
首先，kalman是一个数字滤波器。

我们可以用硬件搭建一个模拟滤波器，将叠加了噪声的模拟信号输入到滤波器中，滤波器给出一个响应。
我们把这个响应作为输入信号踢掉噪声之后的真值。
当然，可以通过调整滤波器参数，使得响应尽可能接近客观真值，亦即尽可能多地衰减噪声。

进一步讲，我们用AD将模拟信号数字化之后，因为模拟信号本身包含了噪声，即使AD没有误差，数字化之后的数字量也是含有噪声的。
况且，不可避免的，还要考虑AD的误差。我们把这种误差就叫做测量误差。

数字化之后，最简单的，我们可以测100组数据，排序，删掉前20个，删掉后20个，剩下60个取均值。这样会排除了一些偶然误差。
kalman滤波器和上面的均值方法工作模式类似，只不过他的工作过程比较复杂，通过算法里面的五条公式过后，会很好的给出真值。

网上很多的是关于多维kalman实现。理解多维的比较费劲。
参照网上的一些代码，实现了一个一维地滤波，对于有C语言基础的同学来讲，理解起来应该很容易了。

3，
百度百科里面有这个帖子：

http://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html

讲解的是一维kalman滤波器，但是最后printf出来的都是分立的值，看不出来什么。

我参考那段代码，改写成了下面这段代码，在labwindows里面绘制了一段曲线，效果就很直观了：

/*-------------------------------------------------------------------------------------------------------------*/
void KalmanFilter(unsigned int ResrcDataCnt,const double *ResrcData,double *FilterOutput,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
        unsigned int i;
        double R = MeasureNoise_R;
        double Q = ProcessNiose_Q;
        double x_last = *ResrcData;
        double x_mid = x_last;
        double x_now;
        double p_last = InitialPrediction;
        double p_mid ;
        double p_now;
        double kg;

        for(i=0;i<ResrcDataCnt;i++)
        {
                x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
                p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
                kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声
                x_now=x_mid+kg*(*(ResrcData+i)-x_mid);//估计出的最优值
                p_now=(1-kg)*p_mid;//最优值对应的covariance
               
                *(FilterOutput + i)  = x_now;

                p_last = p_now; //更新covariance值
                x_last = x_now; //更新系统状态值
        }

}
/*-------------------------------------------------------------------------------------------------------------*/

上面是滤波器部分，然后，我模拟了一段AD的数据，输给这个滤波器，然后看一下这个滤波器的响应，效果如下所示：


从图上可以看出，真值为5，,1000组数据，每组随机叠加10%的噪声，可以观察一下kalman的收敛速度和收敛精度。
收敛速度和收敛精度就是kalman的两个重要衡量指标。
收敛速度和收敛精度是矛盾着的。

4，
参考上面的代码，我优化了一下之后，运行在了STM32上面：

/*-------------------------------------------------------------------------------------------------------------*/
/*       
        Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
        R:测量噪声，R增大，动态响应变慢，收敛稳定性变好       
*/

double KalmanFilter(const double ResrcData,
                                        double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
        double R = MeasureNoise_R;
        double Q = ProcessNiose_Q;

        static        double x_last;

        double x_mid = x_last;
        double x_now;

        static        double p_last;

        double p_mid ;
        double p_now;
        double kg;       

        x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
        p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
        kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声
        x_now=x_mid+kg*(ResrcData-x_mid);//估计出的最优值
               
        p_now=(1-kg)*p_mid;//最优值对应的covariance       

        p_last = p_now; //更新covariance值
        x_last = x_now; //更新系统状态值

        return x_now;               
}

/*-------------------------------------------------------------------------------------------------------------*/

5，
我是希望用加速度计测出加速度，然后加速度积分得出速度，然后速度积分得位移。
硬件用的freescale的MMA7361，模拟加速度计，STM32 片上AD，如下：洞洞板右上角为MMA7361


LCD上面曲线为加速度计曲线，下面那条线为速度曲线，第三条没处理。

没有kalman可以看到加速度那条线很粗：


用了kalman之后，效果如下：



6，
kalman需要根据具体的应用来调整滤波器参数的。
主要是：测量噪声系数，系统噪声系数，初始估计。这三个。
我问了一下老师，老师的意思是，这些参数很多时候是经验值，没有很好地系统的估计方法。
我个人感觉这是kalman理论发展的方向，估计很多人在研究这个吧。

7，
附件是一个labwindows 仿真的C代码，一个STM32 的MDK工程代码，正点原子的板子，和一些文档。

感觉有个这些，再参考一下网上的资料，应用方面应该没什么问题了。

至于理论本质，实在力不能及，路漫漫其修远啊。

8，
听了小伙伴的意思，换成数字MPU6000了。