发 帖  
张飞软硬开源:基于MST32的BLDC直流无刷电机驱动器,👉戳此立抢👈

[经验] 教你如何用C语言实现卡尔曼滤波

2015-6-11 16:30:11  37199 C语言
分享
19
1. 首先,kalman是一个数字滤波器

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

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

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

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

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


2. 一维的卡尔曼滤波器
我参考了这个论文
卡尔曼滤波算法C语言实现.zip (99.89 KB, 下载次数: 2685)

这个主要讲解的是一维kalman滤波器,但是最后printf出来的都是分立的值,看不出来什么。
我参考那段代码,改写成了下面这段代码,运行在STM32上,效果就很不错了
  1. /*-------------------------------------------------------------------------------------------------------------*/
  2. /*      
  3.         Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏
  4.         R:测量噪声,R增大,动态响应变慢,收敛稳定性变好      
  5. */

  6. double KalmanFilter(const double ResrcData,
  7.                                         double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
  8. {
  9.         double R = MeasureNoise_R;
  10.         double Q = ProcessNiose_Q;

  11.         static        double x_last;

  12.         double x_mid = x_last;
  13.         double x_now;

  14.         static        double p_last;

  15.         double p_mid ;
  16.         double p_now;
  17.         double kg;      

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

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

  26.         return x_now;               
  27. }

  28. /*-------------------------------------------------------------------------------------------------------------*/
复制代码
3. 我的测试
我是希望用加速度计测出加速度,然后加速度积分得出速度,然后速度积分得位移。
硬件用的freescale的MMA7361,模拟加速度计,STM32 片上AD,如下:洞洞板右上角为MMA7361
11.jpg
LCD上面曲线为加速度计曲线,下面那条线为速度曲线,第三条没处理。
没有kalman可以看到加速度那条线很粗:
12.jpg
用了kalman之后,效果如下:
13.jpg

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

本文转自某莫论坛



caohaixia2015 2015-6-12 08:22:00
楼主真好;正需要这个呢
回复

举报

方宏道 2015-6-12 21:36:36
谢谢分享,顶顶顶
回复

举报

ye819200878 2015-6-14 15:24:21
                                    
谢谢分享
回复

举报

kuanglf 2015-6-25 16:58:57
学习记号备用,谢谢分享
回复

举报

隐秘的逗号 2015-6-25 20:40:54
学习了,正好在找这个,早点看到就好了
回复

举报

justicelee 2015-6-26 16:54:26
学习记号备用,谢谢分享
回复

举报

dennisa 2015-6-27 23:41:13
3Q......................................
回复

举报

dennisa 2015-6-27 23:41:58
3Q.............................
回复

举报

嗷嗷~嗷呜~ 2015-7-23 14:14:39
好东西,顶一个
回复

举报

嗷嗷~嗷呜~ 2015-7-24 19:39:44
顶楼主一个,拿走啦
回复

举报

重子 2015-7-29 21:37:57
谢谢楼主的分享,正在学习中
回复

举报

黄彬lavide 2015-7-29 22:56:17

下载学习一下
回复

举报

digua1108 2015-8-10 16:29:23
相当不错,学习学习了。
回复

举报

jiajia1106 2015-8-21 08:14:15
不错   学习了   谢谢楼主
回复

举报

q327468761 2015-8-21 14:02:40
标记 以后研究好看 谢谢
回复

举报

2890653902 2015-8-21 21:56:12
谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢分享
回复

举报

smallsmlie 2015-8-21 22:36:23
谢谢分享               谢谢分享                         谢
           谢谢分享                 谢谢分享                         谢
        谢谢分享                    谢谢分享                         分
       谢谢分享                    谢谢分享                          享
      谢谢分享                     谢谢分享                          谢
    谢谢分享                       谢谢分享                          谢
回复

举报

2890653902 2015-8-23 19:18:00
谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢
回复

举报

spiderman003 2016-1-11 10:49:38
这个xlast和plast没有定义初值,调用卡尔曼滤波的话,是不是初值都是0了?这样的话,结果不对吧,如果定义初值,那么该怎么实现实时滤波呢,就是对每一个测出的加速度值能滤波出一个最优的估计值?
回复

举报

评论

高级模式
您需要登录后才可以回帖 登录 | 注册

发经验
课程
    关闭

    站长推荐 上一条 /8 下一条

    快速回复 返回顶部 返回列表