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

卡尔曼滤波算法C语言实现.zip

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

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

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

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

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

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

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

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

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

/*-------------------------------------------------------------------------------------------------------------*/
/*
Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
R:测量噪声，R增大，动态响应变慢，收敛稳定性变好
*/
double KalmanFilter(const double ResrcData,
double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
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;
}
/*-------------------------------------------------------------------------------------------------------------*/

复制代码

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

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

用了kalman之后，效果如下：

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

本文转自某莫论坛

赵勇 · 2017-9-12 13:49:43

支持楼主!!!!!!!!!!!!!!!!!!

zhangda1230 · 2017-9-30 14:03:29

你好，最后一个形参InitialPrediction是干什么用的，程序里并没有对这个参数使用啊，

kirk · 2017-10-18 16:05:27

学习，，，，，，，，，，，，，，，，，，，，，，，，，，，

kirk · 2017-10-19 13:39:41

x_last 没给初值？？？？？？？？？？？？？？？？？

airuoshahen · 2017-10-19 14:40:40

还是没有看懂卡尔曼滤波，真心希望楼主再讲讲~！

猎国倾城 · 2017-10-20 21:12:06

x_mid=x_last; //x_last=x(k-1|k-2),x_mid=x(k|k-1)

猎国倾城 · 2017-10-20 21:21:16

p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-2),Q=噪声

ITCore168 · 2017-10-20 23:46:31

学习一下，不错

tony1020 · 2017-11-6 21:19:41

谢谢了。正在研究中。。。

赵永明 · 2017-11-13 16:51:32

谢谢分享

maxwang · 2017-11-23 21:23:55

楼主能分享你的程序吗？

sunflowerzt · 2017-12-8 17:40:03

好帖子，值得赞一个，楼主辛苦了

RuiQi Jiang · 2017-12-19 19:58:14

学到了，正在写这个，谢谢楼主~~

duowei1987 · 2018-1-5 15:42:18

下来学习一下

HuangTzuYan · 2018-5-8 14:16:55

66666，学习一把！

孙滋昂 · 2018-6-11 19:29:49

谢谢分享留着好好研究

renwensheng · 2018-6-12 19:56:29

nohao ahodfhjoasdfasdfasdf

wangchidzfsy · 2018-6-21 08:29:49

楼主厉害，不错，高手

h1654155210.2107 · 2018-7-19 21:24:52

你那几个参数怎么用？都没说清楚啊

李小三 · 2018-7-23 17:44:20

谢谢分享，谢谢分享

搜索历史

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

相关推荐

107 条评论

评论

精选推荐

站长推荐 /5