教你如何用C语言实现卡尔曼滤波

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

我们将叠加了噪声的模拟信号输入到滤波器中，滤波器给出一个响应。这个响应就是输入信号去掉噪声之后的真值。当然，我们可以通过调整滤波器参数，使得响应尽可能接近客观真值。

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

数字化之后，是最简单的。我们可以测100组数据，排序，删掉前20个，删掉后20个，剩下60个取均值。这样会排除了一些偶然误差。

kalman滤波器和上面的均值方法工作模式类似，只不过他的工作过程比较复杂，通过算法里面的五条公式过后，会很好的给出真值。

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

2. 一维的卡尔曼滤波器
我参考了这个论文

卡尔曼滤波算法C语言实现.zip (99.89 KB, 下载次数: 3684 )

这个主要讲解的是一维kalman滤波器，但是最后printf出来的都是分立的值，看不出来什么。我参考那段代码，改写成了下面这段代码，运行在STM32上，效果就很不错了

1. /*-------------------------------------------------------------------------------------------------------------*/
   
2. /*      
   
3.         Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
   
4.         R:测量噪声，R增大，动态响应变慢，收敛稳定性变好      
   
5. */
   
6. 
   
7. double KalmanFilter(const double ResrcData,
   
8.                                         double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
   
9. {
   
10.         double R = MeasureNoise_R;
    
11.         double Q = ProcessNiose_Q;
    
12. 
    
13.         static        double x_last;
    
14. 
    
15.         double x_mid = x_last;
    
16.         double x_now;
    
17. 
    
18.         static        double p_last;
    
19. 
    
20.         double p_mid ;
    
21.         double p_now;
    
22.         double kg;      
    
23. 
    
24.         x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
    
25.         p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
    
26.         kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声
    
27.         x_now=x_mid+kg*(ResrcData-x_mid);//估计出的最优值
    
28.                
    
29.         p_now=(1-kg)*p_mid;//最优值对应的covariance      
    
30. 
    
31.         p_last = p_now; //更新covariance值
    
32.         x_last = x_now; //更新系统状态值
    
33. 
    
34.         return x_now;               
    
35. }
    
36. 
    
37. /*-------------------------------------------------------------------------------------------------------------*/

复制代码

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

LCD上面曲线为加速度计曲线，下面那条线为速度曲线，第三条没处理。
没有kalman可以看到加速度那条线很粗：

用了kalman之后，效果如下：

4.结论
kalman需要根据具体的应用来调整滤波器参数的。
主要是：测量噪声系数，系统噪声系数，初始估计。
感觉这些参数很多时候是经验值，没有很好地系统的估计方法，这个也是现在四轴飞行器调参的难点吧

本文转自某莫论坛

楼主真好；正需要这个呢

谢谢分享，顶顶顶

                                    
谢谢分享

学习记号备用，谢谢分享

学习了，正好在找这个，早点看到就好了

学习记号备用，谢谢分享

3Q......................................

3Q.............................

好东西，顶一个

顶楼主一个，拿走啦

谢谢楼主的分享，正在学习中

下载学习一下

相当不错，学习学习了。

不错   学习了   谢谢楼主

标记 以后研究好看 谢谢

谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢分享

谢谢分享               谢谢分享                         谢
           谢谢分享                 谢谢分享                         谢
        谢谢分享                    谢谢分享                         分
       谢谢分享                    谢谢分享                          享
      谢谢分享                     谢谢分享                          谢
    谢谢分享                       谢谢分享                          谢

谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢

这个xlast和plast没有定义初值，调用卡尔曼滤波的话，是不是初值都是0了？这样的话，结果不对吧，如果定义初值，那么该怎么实现实时滤波呢，就是对每一个测出的加速度值能滤波出一个最优的估计值？