卡尔曼滤波算法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<ResrcDataCnt;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了。
吃饭啦,就这样吧。希望能帮到大家。
我是学生党,在学习,很多东西做的很粗糙,勿喷。
不错,卡尔曼滤波,mark下 赞一个,一会试试看效果。 很好楼主辛苦了 先赞一个,明天再试试 新人,学习一下 Mark {:biggrin:} 数据处理确实很厉害啊,我也想学了 牛人啊~!!!顶起~!!! wingerchen 发表于 2014-3-5 12:29
赞一个,一会试试看效果。
效果怎么样。 嗯说下我的学院派的补充吧……
首先呢,卡尔曼滤波是个现代控制理论领域的内容,虽然似乎不少人认为现代控制理论没什么用但实际上他们已经在用卡尔曼滤波了,卡尔曼本人也被认为是现代控制理论重要奠基人之一(不过他好像也没有其他的重大贡献)。“数字滤波器”一般是个信号处理领域的词(较真来说卡尔曼滤波确实属于数字滤波器,主要是因为信号领域里“滤波器”这个词含义很宽,广义的滤波器差不多包括所有输出取决于输入的系统)。我还是觉得从状态空间模型、状态观测器的角度去理解卡尔曼滤波比较直观。
误差系数的取值当然是有方法的。测量误差靠查手册啊或者自己用已知真值的试验测出来。系统误差要分析状态转移方程与实际的差别,比如楼主位加速度计那段程序是的状态转移矩阵是1,那么如果应用在动物体上,系统误差主要是物体运动产生的,这就要对物体运动程度做预先估计。当然这时候的系统误差不是白噪声,所以观测器已经不是线性最优的了。
多说一句卡尔曼滤波做为最优线性观测器,严格来讲是没有“参数”的,最优的东西是不应该受参数而转移的,系统误差矩阵和测量误差矩阵应该叫“模型”,它们的确定属于模型辨识问题而不是参数整定问题。就卡尔曼滤波本身而言这已经不算是发展方向了。
像楼主位5±10%例子,Q就是0,R就是噪声方差,这是卡尔曼滤波对这两个量的定义,不容调试的,改变了就不是卡尔曼滤波了……虽然具体应用下可能次优状态观测更有利于系统其他的部分比如控制,但既然已经破坏了卡尔曼滤波的线性最优性的话,何必要用它呢,传统滤波器不也挺好的么……
初始估计无所谓的啊,因为初始值的方差都会设置成很大,进入卡尔曼滤波迭代之后就对后面没有影响了。 K.O.Carnivist 发表于 2014-3-9 15:19
嗯说下我的学院派的补充吧……
首先呢,卡尔曼滤波是个现代控制理论领域的内容,虽然似乎不少人认为现代控 ...
最优的东西是不应该受参数而转移的。
这句话确实说的很出彩。
简单地了解了卡尔曼的一些学院里的东西,实在感觉到力不从心,里面涉及到太复杂的推导。
另一方面,可能很大程度上,对工程师来讲,不需要纠结一个理论的具体内容。
如果有充分的证据表明这个理论是够用的,我感觉就足够了。
站在巨人的肩膀上,才能看的更远,看的更省心。 算法类的在运用上很有用啊 jackk 发表于 2014-3-8 20:09
效果怎么样。
谢谢你的代码,经确认,卡尔曼不是很适合我的应用要求。{:lol:}
不过我会参考你的代码,作为APP里的一个滤波插件来作为一个可选项来调用。 mark下!慢慢学! mark~ 卡尔曼滤波
感谢楼主分享 这个文件解压不对啊 这个文件解压不对啊 这个文件解压不对啊 卡尔曼需要简历精确的模型,然后通过现代控制理论和矩阵运算得出准确的公式,还有很多地方需要自己去简化,才可以,特别是对噪声的处理,有色还是无色, 谢谢分享!学习了 卡尔曼滤波算法C语言实现 这个必须顶!!!!!!!!!! 学习学习 好贴顶之! 不错,卡尔曼滤波,明天试一下 看了楼主的代码,有些两个疑问:
其一,
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;
}
中的红色参数InitialPrediction,完全没有使用,那究竟有何用?
还有一个红色的参数p_last没有赋初值,那么这个参数就是初始化为0值?
看你的main里面开始的时候有做一段dummy的采集,这个p_last的值应该是那时候开始累积的。对否?
其二,
能说一下你设定的那三个参数(宏定义)是怎么来的么?
当初你设定的时候是否有经验公式?还是说调试出来的? 对于有纹波的AD检测,用卡尔曼过滤器是很理想的。 落叶知秋 发表于 2014-4-15 11:36
看了楼主的代码,有些两个疑问:
其一,
1,InitialPredictio在函数的实现中确实没用到。学院派的一种说法是:卡尔曼的收敛性与最初的系统估计关系不是很大,只要不为0,最终都会有比较理想的收敛效果。在最终的收敛完场之后,无论最初赋值为多少,都基本会稳定到某一个值。
2,那三个值需要根据不同的实现来尝试调节。没有经验公式,至少我不知道经验公式。实际上,即使存在所谓经验公式,有时间去理解经验公式,可能已经尝试出来了。
落叶知秋 发表于 2014-4-15 11:36
看了楼主的代码,有些两个疑问:
其一,
p_last类型是 static的,会随着迭代调用的进行而进行同时迭代。理解C语言中的static关键字就会理解p_last的初值问题了。实际上,每次的你所谓初值,即为上次函数退出时的值。 K.O.Carnivist 发表于 2014-3-9 15:19
嗯说下我的学院派的补充吧……
首先呢,卡尔曼滤波是个现代控制理论领域的内容,虽然似乎不少人认为现代控 ...
我对卡尔曼滤波一直有个疑问,卡尔曼滤波需要噪声模型,需要对噪声作估计,那么卡尔曼滤波可以对非平稳噪声进行滤波吗? lologame 发表于 2014-4-18 18:33
我对卡尔曼滤波一直有个疑问,卡尔曼滤波需要噪声模型,需要对噪声作估计,那么卡尔曼滤波可以对非平稳噪 ...
应该是只要求噪声是是独立的高斯分布,没有要求平稳性。噪声协方差阵可以是时变的。 K.O.Carnivist 发表于 2014-4-18 19:18
应该是只要求噪声是是独立的高斯分布,没有要求平稳性。噪声协方差阵可以是时变的。 ...
噪声协方差如何时变呢?没有在卡尔曼方程里面见到对噪声协方差矩阵的更新呀 学习学习。很有用的东西必须马克 挺详细的卡尔曼资料 不错不错! 以前也学了卡尔曼滤波,不过学得不够啊,现在做飞控感觉还是很难 Mark ..学习了 顶!给力!! 学习!学好数理化,走遍天下都不怕 楼主有做过多维的情况吗? 通俗易懂,点个赞! 顶,,mark lologame 发表于 2014-4-18 19:35
噪声协方差如何时变呢?没有在卡尔曼方程里面见到对噪声协方差矩阵的更新呀 ...
噪声协方差是另外算的。
给您个例子吧:比如观测一个物体在一维直线上运动,观测量是上一次观测到本次观测之间物体运动的距离(实际上就是速度),误差是测量值的5%;这时观测噪声的分布就是时变的,用卡尔曼滤波的话每次迭代前观测噪声协方差就要用本次的观测量算出来。 看了楼主的程序,有几个问题,希望楼主解答!
1.我采集的三轴角度传感器的X Y Z三个通道的AD值,用DMA传输,kalman代码是copy你程序里的,但是输出的值却和我均值滤波后的不一样!
楼主的KALMAN_Q=0.0000001,KALMAN_R=10.0000,而我的确实反过来是对的!
接触kalman没几天,不知道我的参数对与不对?
2.关于程序可里“Kalman初期迭代过程中的不稳定数据,删除之”,我测试了影响并不是很大,或许是我的硬件滤波已经做的很好了,软件可以有很小的压力吧?
声明下,我不是做广告的啊!!!我用的是3+2角速度加速度传感器,这个硬件做的很好!个人观点!
3.我的是一个2维数组,但是我是把每一个数据都进行了滤波,对么?还是说不行? Mark 正在学习呀 正需要这个Mark Mark 正在学习呀 标记,以后可能用到 挺好,赞一个!
我用6050做的卡尔曼,四元数做状态,但是有时会有加速度计跳变导致在卡尔曼滤波里修正的那一步上,使状态(四元数)跳变,进而反解的姿态角也跳变,你是怎么处理的? 好学生,,,,感谢哈 mark。。。不错 留下记号 正在学习。谢谢分享卡尔曼滤波算法。 讲得还是比较通俗易懂,学习了 厉害!学习了 这个学习了.正好在作气压计的程序.读数总是不稳.试试这个滤波 看到上面的讨论,好多数学的东西都淡忘了,惭愧。 本帖最后由 SMC 于 2014-6-2 21:33 编辑
K.O.Carnivist 发表于 2014-3-9 15:19
嗯说下我的学院派的补充吧……
首先呢,卡尔曼滤波是个现代控制理论领域的内容,虽然似乎不少人认为现代控 ...
大神,可以看下Pixhawk的EKF融合姿态吗? 看完后给个短评呗。
Pixhawk的姿态融合有两种可选,一种是EKF,一种是四元数叉乘互补;我看完了后者,发现和坛子里的一模一样,但EKF就无能为力了。
这是网址:https://github.com/PX4/Firmware/tree/master/src/modules/attitude_estimator_ekf 楼主,卡尔曼滤波貌似是对随机信号的处理,楼主用在哪里? 是否有必要讨论下状态量和观测量的问题?
其一,我们需要的结果,并不符合一个线性的关系。
其二,若我用陀螺仪去计算我的角度,在这个公式中如何用加速度计当作观测量来滤波。
看了这个公式,我想仅可以实现单个变量的滤波,并无法实现数据融合,陀螺仪和加计还是无法去相互纠正。
真的困惑了。。。理论帝不吝指点啊。。。 先马克一记 先收藏mark了 mark!!!!!!!!!!!!!!!!!!!!!!!!! 学生就能做到这个程度,不错。 不错。很好1 请问下卡尔曼滤波器的测量噪声系数和系统噪声系数如何确定啊,如mems陀螺仪还有温漂和零漂如何处理呢 梦中花雨 发表于 2014-6-28 10:32
请问下卡尔曼滤波器的测量噪声系数和系统噪声系数如何确定啊,如mems陀螺仪还有温漂和零漂如何处理呢 ...
试。。。 赞一个,楼主
赞一个,楼主 标记,卡尔曼滤波 好贴。。码一下。。慢慢研究。。 相当不错 膜拜,mark,{:smile:}{:smile:}{:smile:} 本帖最后由 wiser803 于 2014-8-3 16:28 编辑
看了网络上许多对于KALMAN滤波的应用,都是将其当一个传感器或一个信号的后端滤波器使用。这是杀鸡用牛刀,是对KALMAN滤波器重大误解造成的。也许,造成这种局面是高校在教了KALMAN滤波原理后,为了联系实际应用給学生一个示例,用单片机KALMAN滤波对信号进行简单的滤波,而学生误以为KALMAN滤波就是干这个用的。
其实,对于传感器或信号的后端滤波处理,已经有大量很成熟、很好的数字滤波方案可用,相对KALMAN滤波繁杂的代码轻巧很多,其滤波效果甚至比KALMAN滤波直接用来处理信号好很多。
KALMAN博士推导出这套滤波方法是基于带正态噪声的线性系统模型状态变量的最优估计器(观测器),或称动力学状态最优估计器。由此可知,KALMAN滤波适用于系统模型下的滤波测量和最优估计,信号经适当处理后一般可用于高精度反馈控制。与一般信号滤波器设计参数相对独立不同,KALMAN滤波必须根据系统模型参数,来导出滤波器其他参数。因此,KALMAN滤波的前端信号预处理还是需要的,并非只要KALMAN滤波就OK了。当然,如果将KALMAN滤波器简化到仅仅类似一般信号滤波器,由于阶次的退化其滤波效果预计不会超过专用滤波器的。 感觉LZ的例子里面,如果只是白噪声干扰,用RC或者滑动平均,波形就很好看了 很好的东西,学习了 wiser803 发表于 2014-8-3 16:25
看了网络上许多对于KALMAN滤波的应用,都是将其当一个传感器或一个信号的后端滤波器使用。这是杀鸡用 ...
请给个其他的滤波的例子啊 不错,讲的相对详细,学习下 不错,讲的相对详细,学习下! 领教了,多谢楼主。。。 学习了..........不错............. 学习了,再仔细看看 码下来。。慢慢学。。 MARK!厉害! 不错,学习了! MARK卡尔曼滤波算法C #在这里快速回复#不错,卡尔曼滤波,mark下 不理解,楼主为什么不在函数内部循环而是采用静态变量的做法,在外部控制循环呢 好厉害的样子,,,一点都不懂{:sad:} ËÄÖá·ÉÐÐ?¿¨¶ûÂü 好东西,留名收藏 好资料,效果太好了
谢谢分享。。。。。。 好贴留名,备用 很好,标记一下!收藏! 谢谢分享 {:shy:}学习了 学习了,谢谢楼主分享!! 这个飞控上应该很有用。