卡尔曼滤波 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；