β

[转载]卡尔曼滤波(c、c++实现)

阿元 26 阅读
c c++
原文地址:卡尔曼滤波(c、c++实现)作者:肖蜡       调了一段时间的四轴飞行器,从最初的芯片数据处理,到后来发现数据波动,然后到滤波~这一路走得真蛋疼。看别人的教程,总觉得应该自己写写,总觉得自己能比别人写得好,结果不然~记得上一篇,结果就是每个人看了后都会遇到一堆问题~无语。所以这次我会写得跟菜鸟,老鸟可以飘过了~     先来一幅我最后的效果图:
    效果看到后你就知道卡尔曼滤波是不是你想要的了!当然我对卡尔曼仅仅是简单的运用,还没有深入,听某大神说,还有更nb的卡尔曼~如果有幸接触我也会帖出来的!     好废话不多说了------卡尔曼单系统,处理一个数据的时候、无控制量,经过一些列的简化公式之后(我们不需要理解为啥是这些公式,因为我们的目的是使用它,除非你是搞科研的,当然搞科研的就用不上看我这篇教程了。): 核心代码:(在看这篇教程的时候最好带上一篇有卡尔曼公式的文档)   m_xMid=m_xLast;  //将上一次的系统最优赋值给一个中间变量   m_pMid=m_pLast+m_Q;//将上一次的covariance值加上系统噪声(m_Q)赋值给一个中间变量(系统白噪声在你调试的过程中 换着变量赋值,但绝对不能是零,在不断的测试过程中你修改这个值,得到你要的最佳效果) //    m_kg=m_pMid/(m_pMid+m_R); //计算kg的公式~就不解释了,我解释不清楚 //    m_xNow=m_xMid+m_kg*(nMersure-m_xMid);    //通过计算得到这次的系统最优 nMersure为系统采样 //    m_pNow=(1-m_kg)*m_pMid;    //计算当前的covariance值 //    m_xLast=m_xNow;     //更新系统最优   m_pLast=m_pNow;   //更新covariance值
    好了,这就完了!不要惊讶,简单的运用就这么多了,卡尔曼这样就入门了。一个好的算法,核心代码无非就是几个数学公式,不能太复杂,这样效率低了不能满足一些处理的要求,就被淘汰了~
    初始化的时候注意:刚开始我用的波动sin(就是随便产生了一个随机数),初始化第一次系统的时候,赋值为0,结果怎么都是错的~结果单步跟踪发现公式有个地方为零的话,数据会溢出,应该是产生了除以0的情况,所以初始化的时候,不应该给系统赋值为0;也就是两个不能赋值为零,一个是第一次的系统采样(在公式中第一次采样扮演的是m_xLast上一次的值),第二个是白噪声m_Q;
好了 上面的我是写成类实现的,要代码的可以邮件我~:chennengbin@foxmail.com


c c++
作者:阿元
原文地址:[转载]卡尔曼滤波(c、c++实现), 感谢原作者分享。

发表评论