ad 卡尔曼_卡尔曼滤波算法C语言实现(转2)
本帖最后由 jackk 于 2014-3-5 13:00 编辑
前段时间在论坛里简单地发了一些关于kalman的理解。
有很多网友顶贴的,趁着今天休息,整理一下前段时间的工作。
有些理解和说法可能不正确,以此抛砖引玉吧。
1,
在google上搜索卡尔曼滤波,很容易找到以下这个帖子:
http://blog.csdn.net/lanbing510/article/details/8828109
这里面很简单形象的解释了kalman的作用。
但是帖子后半段,将kalman回归到了一大堆数学推理、推导上,对非数学专业,或者数学基础尤其是概率和随机过程基础不好的同学来讲实在太过头痛。
帖子最后用matlab实现了kalman,其中很简单地用i,j,k来命名,可能不是太妥当,新手理解的时候,脑袋里面要运行着一个很深的堆栈。
当然,原作者毫无疑问对kalman有着极其深刻的理解,才能实现得如此游刃有余。
在此,只是对第一次接触kalman的同学简单介绍一下自己的理解。帮得到诸君为好,帮不到请勿喷。
2,
首先,kalman是一个数字滤波器。
我们可以用硬件搭建一个模拟滤波器,将叠加了噪声的模拟信号输入到滤波器中,滤波器给出一个响应。
我们把这个响应作为输入信号踢掉噪声之后的真值。
当然,可以通过调整滤波器参数,使得响应尽可能接近客观真值,亦即尽可能多地衰减噪声。
进一步讲,我们用AD将模拟信号数字化之后,因为模拟信号本身包含了噪声,即使AD没有误差,数字化之后的数字量也是含有噪声的。
况且,不可避免的,还要考虑AD的误差。我们把这种误差就叫做测量误差。
数字化之后,最简单的,我们可以测100组数据,排序,删掉前20个,删掉后20个,剩下60个取均值。这样会排除了一些偶然误差。
kalman滤波器和上面的均值方法工作模式类似,只不过他的工作过程比较复杂,通过算法里面的五条公式过后,会很好的给出真值。
网上很多的是关于多维kalman实现。理解多维的比较费劲。
参照网上的一些代码,实现了一个一维地滤波,对于有C语言基础的同学来讲,理解起来应该很容易了。
3,
百度百科里面有这个帖子:
http://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html
讲解的是一维kalman滤波器,但是最后printf出来的都是分立的值,看不出来什么。
我参考那段代码,改写成了下面这段代码,在labwindows里面绘制了一段曲线,效果就很直观了:
/*-------------------------------------------------------------------------------------------------------------*/
void KalmanFilter(unsigned int ResrcDataCnt,const double *ResrcData,double *FilterOutput,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
unsigned int i;
double R = MeasureNoise_R;
double Q = ProcessNiose_Q;
double x_last = *ResrcData;
double x_mid = x_last;
double x_now;
double p_last = InitialPrediction;
double p_mid ;
double p_now;
double kg;
for(i=0;i
{
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+i)-x_mid);//估计出的最优值
p_now=(1-kg)*p_mid;//最优值对应的covariance
*(FilterOutput + i) = x_now;
p_last = p_now; //更新covariance值
x_last = x_now; //更新系统状态值
}
}
/*-------------------------------------------------------------------------------------------------------------*/
上面是滤波器部分,然后,我模拟了一段AD的数据,输给这个滤波器,然后看一下这个滤波器的响应,效果如下所示:
从图上可以看出,真值为5,,1000组数据,每组随机叠加10%的噪声,可以观察一下kalman的收敛速度和收敛精度。
收敛速度和收敛精度就是kalman的两个重要衡量指标。
收敛速度和收敛精度是矛盾着的。
4,
参考上面的代码,我优化了一下之后,运行在了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;
}
/*-------------------------------------------------------------------------------------------------------------*/
5,
我是希望用加速度计测出加速度,然后加速度积分得出速度,然后速度积分得位移。
硬件用的freescale的MMA7361,模拟加速度计,STM32 片上AD,如下:洞洞板右上角为MMA7361
LCD上面曲线为加速度计曲线,下面那条线为速度曲线,第三条没处理。
没有kalman可以看到加速度那条线很粗:
用了kalman之后,效果如下:
6,
kalman需要根据具体的应用来调整滤波器参数的。
主要是:测量噪声系数,系统噪声系数,初始估计。这三个。
我问了一下老师,老师的意思是,这些参数很多时候是经验值,没有很好地系统的估计方法。
我个人感觉这是kalman理论发展的方向,估计很多人在研究这个吧。
7,
附件是一个labwindows 仿真的C代码,一个STM32 的MDK工程代码,正点原子的板子,和一些文档。
感觉有个这些,再参考一下网上的资料,应用方面应该没什么问题了。
至于理论本质,实在力不能及,路漫漫其修远啊。
8,
听了小伙伴的意思,换成数字MPU6000了。
吃饭啦,就这样吧。希望能帮到大家。
我是学生党,在学习,很多东西做的很粗糙,勿喷。
ad 卡尔曼_卡尔曼滤波算法C语言实现(转2)相关推荐
- ad 卡尔曼_卡尔曼滤波剪影__Kalman Filtering · Make Intuitive
铺垫用の引论 Kalman Filtering 算法是一种最优滤波估计算法,有了它之后,就算只知道观测对象的少量不准确观测信息也有可能对这一对象实现完全控制,它的Advantages和Disadvan ...
- 卡尔曼滤波算法c语言stm32,卡尔曼滤波算法及C语言实现_源代码
a往南向北 2019-01-16 20:39:20 11340 收藏 111 分类专栏: C语言嵌入式 文章标签: 卡尔曼滤波 C代码 卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是 ...
- matlab温度数据怎么滤波_卡尔曼滤波算法思想理解 Kalman filter 第一篇
卡尔曼滤波算法思想理解 Kalman filter 第一篇 最近在初步的理解目标跟踪的领域, 其中一个非常经典的算法卡尔曼滤波Kalman filter是需要有很好的理解才行, 由于已经脱离了学校,懂 ...
- ad 卡尔曼_对Kalman(卡尔曼)滤波器的理解
1.简介(Brief Introduction) 在学习卡尔曼滤波器之前,首先看看为什么叫"卡尔曼".跟其他著名的理论(例如傅立叶变换,泰勒级数等等)一样,卡尔曼也是一个人的名字, ...
- 卡尔曼_卡尔曼滤波最完整公式推导
卡尔曼滤波是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法.由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程.上面一段话来自百度百科,其实最 ...
- 匀速运动小车卡尔曼_卡尔曼滤波(Kalman Filter)
一.引言 以下我们引用文献[1]中的一段话作为本文的開始: 想象你在黄昏时分看着一仅仅小鸟飞行穿过浓密的丛林.你仅仅能隐隐约约.断断续续地瞥见小鸟运动的闪现.你试图努力地猜測小鸟在哪里以及下一时刻它会 ...
- ad 卡尔曼_理解卡尔曼五个方程
1 .简介 (Brief Introduction) 在学习卡尔曼滤波器之前,首先看看为什么叫"卡尔曼".跟其他 著名的理论(例如傅立叶变换,泰勒级数等等)一样,卡尔曼也是一个 人 ...
- 卡尔曼滤波算法及C语言实现_源代码
卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是,实际的硬件板子上还是需要C语言,当然可以自动代码生成,还有一种就是直接手动编写C语言. 1.前言 在google上搜索卡尔曼滤波,很容易找 ...
- 卡尔曼滤波算法及c语言代码,卡尔曼滤波算法及C语言代码
卡尔曼滤波算法及C语言代码 卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码(C,C++分别实现) 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣.所以在这里希望能尽自己能力跟大家讨论一些力 ...
最新文章
- jupyter的下载安装
- 清华学霸直博全自主写CPU、操作系统、编译器!刘知远点赞!(附链接)
- 《快学Scala》第二章 控制结构和函数
- modelsim-altera
- c++ 13.二维数组
- LwIP应用开发笔记之七:LwIP无操作系统HTTP服务器
- springboot系列——redisTemplate和stringRedisTemplate对比、redisTemplate几种序列化方式比较
- js 判断图片是否加载完成
- 史上最全的MSSQL复习笔记
- Linux 命令(107)—— systemctl 命令
- 用Java+Html+MySQL 实现注册、登录(servlet框架)-(一)
- 《Two Dozen Short Lessons in Haskell》学习(三)
- Atitit.100% 多个子元素自适应布局属性
- webstorm汉化后乱码现象解决
- pta编程题c语言答案,PTA选择题答案_pta题库答案,pta答案
- raw格式转换成qcow2格式
- Vue禁止浏览器自动填充
- excel平均值公式_必学的6个Excel平均值公式
- 单峰数组求峰值,二分思想,LeetCode862
- web前端开发和岗位职责