欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

C语言实现卡尔曼滤波

程序员文章站 2022-03-05 16:53:24
...
  1. 首先,kalman是一个数字滤波器。
    我们将叠加了噪声的模拟信号输入到滤波器中,滤波器给出一个响应。这个响应就是输入信号去掉噪声之后的真值。当然,我们可以通过调整滤波器参数,使得响应尽可能接近客观真值。

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

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

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

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

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

#define KALMAN_Q 0.02

#define KALMAN_R 7.0000

/* 卡尔曼滤波处理 */

static double KalmanFilter(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
{

    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;

}
相关标签: 卡尔曼滤波