国外网站上找到的简单的卡尔曼滤波算法
折腾了一晚上,搜索了好久也没能整明白卡尔曼滤波里的几个变量都具体起什么作用。稀里糊涂的算有效果了。
国内网站大家都是你转载我的,我转载你的,没几个说的明白的,更有甚者,转载都出现错误。
没办法,试试去外网找,还真找着好用的了。呵呵。贴出来大家参考下。
/** A simple kalman filter example by Adrian Boeing
www.adrianboeing.com
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
double frand() {
return 2*((rand()/(double)RAND_MAX) - 0.5);
}
int main() {
//initial values for the kalman filter
float x_est_last = 0;
float P_last = 0;
//the noise in the system
float Q = 0.022;
float R = 0.617;
float K;
float P;
float P_temp;
float x_temp_est;
float x_est;
float z_measured; //the 'noisy' value we measured
float z_real = 0.5; //the ideal value we wish to measure
srand(0);
//initialize with a measurement
x_est_last = z_real + frand()*0.09;
float sum_error_kalman = 0;
float sum_error_measure = 0;
for (int i=0;i<30;i++) {
//do a prediction
x_temp_est = x_est_last;
P_temp = P_last + Q;
//calculate the Kalman gain
K = P_temp * (1.0/(P_temp + R));
//measure
z_measured = z_real + frand()*0.09; //the real measurement plus noise
//correct
x_est = x_temp_est + K * (z_measured - x_temp_est);
P = (1- K) * P_temp;
//we have our new system
printf("Ideal position: %6.3f \n",z_real);
printf("Mesaured position: %6.3f \n",z_measured,fabs(z_real-z_measured));
printf("Kalman position: %6.3f \n",x_est,fabs(z_real - x_est));
sum_error_kalman += fabs(z_real - x_est);
sum_error_measure += fabs(z_real-z_measured);
//update our last's
P_last = P;
x_est_last = x_est;
}
printf("Total error if using raw measured:%f\n",sum_error_measure);
printf("Total error if using kalman filter: %f\n",sum_error_kalman);
printf("Reduction in error: %d%% \n",100-(int)((sum_error_kalman/sum_error_measure)*100));
return 0;
}//在此未对原程序做任何改动
高手指点一下 Q 和 R 这两个噪声量是干嘛的吧!它们的大小会影响什么? mark 收藏 mark 怎么国内资料这么难找? 这就是楼主说的那个被无数人转来转去的那个温度的例子吧……你去外国网站还是找来了这个例子
Q是系统噪声,R是测量噪声,大概意思就是说该信谁多一点,如果Q=0就最后完全信预测结果,R=0则完全信测量结果
编辑加入:搜这类东西用google scholar吧,很好用的,不过不要期望现成的代码就是了 这类东西想搜索到实用的代码是想都不用想的,国内外都一样
代码确实是有
国内的一般是学生用来做作业的代码,选的例子虚无缥缈暂且不说,要么是MATLAB的验证型代码,若是C或C++的,代码风格和效率那简直就是悲剧,这类代码抄来抄去非常普遍.
而国外的代码,一般都是二次简化的代码,何谓二次简化?
第一次,将例子降维,比如三维多变量系统,他们会把它变成一维单变量的,比如姿态的卡尔曼滤波简化为单轴的卡尔曼滤波
第二次,将代码简化,主要是为了提高效率,但简化后的代码基本没有可更改性,可读性也是大大降低,甚至看不懂,但是代码的编写基础却是十分严格复杂的推导
要想针对应用写这类代码,只能通过自己理解再慢慢的积累起来,比如先针对硬件写一个高效的数学库,然后通过这个库来建立算法
而算法的基础理论,还是老老实实的啃一啃书,啃一啃资料,老外的书和资料还是毫不含糊的 mark mark
~ 楼主可以看看这个《Some of my Experiences with Kalman Filters》By Ronald Yannone
http://www.megasociety.org/noesis/138/kalman.html
这篇文章说了作者的一些经验,并且在最后给出了
R、Q、P取值不当对系统的影响以及对策。 还有我刚刚从Google Books上找到的一本书,部分预览,平均每10页抽掉2页,基本还能看
http://books.google.com/books?id=J_fqMHOCzB8C&source=gbs_navlinks_s
Kalman filtering: theory and practice using MATLAB 一个是过程噪声,一个是观测噪声 这个好! MARK 回复【5楼】cbeeee
-----------------------------------------------------------------------
恩,想想确实是这样。
谢谢! 回复【6楼】lijieamd
-----------------------------------------------------------------------
有道理,研究挺深啊。 回复【9楼】02015678 周光
-----------------------------------------------------------------------
确实不错!谢谢! MARK一下 mark mark! 不错 先MARK
日后用得着 mark mark mark mark 学习了一些东西,虽然也一样没弄明白卡尔曼滤波,但知道有Google Scholar和Google Books这两个好东东。 mark,太高深,看来数学是基石啊,以前还以为数学
除了算账,学的没啥用。。。 mark 找卡尔曼本人问问最好 Mark mark MARK MARK卡尔曼 mark 回复【楼主位】ailibuli 爱理不理
-----------------------------------------------------------------------
很好 mark mark mark mark MARK 建议lz先看看kalman的书把那5个公式弄懂再看程序就比较简单了 mark mark mark mark... mark! mark mark mark mark MARK mark 看看 MARK 其实一维的卡尔曼滤波很简单,只需要五个基本的公式,(两个时间更新的,三个状态更新的)套公式就可以解决;不过要明白原理就要多多看看了;一篇论文名字是《An Introduction to the Kalman Filter》,讲得较为详细。 回复【楼主位】ailibuli爱理不理
-----------------------------------------------------------------------
根据我看的一片论文《An Introduction to the Kalman Filter》中介绍的公式,你所列出的程序是一个验证性的,而不是应用性的,不过思路很清晰;在应用性的程序中z_measured = z_real + frand()*0.09; //the real measurement plus noise 是不必要的,因为计算更新状态的表达式应用的本身就是测量值,在这里是为了验证程序最后能够得到z-real的值,以验证算法的正确性;R值一般是离线测量,而Q值一般是取较小的值; mark一个,留着学习 学习了。 终于可以注册新账号了。mark{:biggrin:} 国内的资料基本不敢看,看来看去大家都一个样。Google Scholar 还是不错的
顶一下 ~ 有机会试试看~ 不错啊,看看 卡尔曼滤波,mark mark 一下卡曼滤波 mark{:smile:} 學習了~{:smile:}{:smile:} mark 卡尔曼的Mark一下 mark{:handshake:} marrrrrrkkkkk 这个学习要 的确蛮简洁的~~~~ 翻了2页帖子,大部分都是mark…… 个人认为卡尔曼算法应用在ARM或者MCU中的简化方式,是杜绝浮点的。 mark 嗯,要多加强数学的学习 卡尔曼滤波的,我都mark一下 谢谢了 传感器的测量值 加速度Acc_Mess 陀螺仪 Gyro_Mess
卡尔曼滤波器增益 Kg
卡尔曼滤波后的真实角度值 Acc_Real
系统时钟 Time_Loop
最优角度值的偏差 L
高斯白噪声 T
中间变量 Acc_Media
Q = 0.022;
R = 0.617;
Acc_Media=Acc_Real+Gyro_Mess*Time_Loop ;//计算瞬时角度值
T=sqrt(Q*Q+L*L); //计算高斯白噪声
Kg=T*sqrt(1/(T*T+R*R)) ; //计算卡尔曼滤波器的增益
Acc_Real=Acc_Media+Kg*(Acc_Mess-Acc_Media); //计算真实角度值
L=sqrt(1-Kg)*T; //计算真实角度值的偏差
return Acc_Real ;
请问这个正确吗
MARK 卡尔曼,学习学习了~ mark 卡尔曼。。 收藏{:smile:} {:smile:}{:smile:}{:smile:}{:smile:}{:smile:}不错 mark以后学习用 感谢楼主,学习一下! 静不下心来看啊。。。 感谢楼主 国内风气真心不如国外啊,好东西还是得上英文论坛找啊,可惜还有gfw 卡尔曼滤波,毕业论文写信息融合的几个算法的时候就有这个,就没弄懂是什么,太虚无缥缈了 mark一下 怎么感觉没有太多使用的呢,大家老师MARK,讲讲道理吗?
Q:预测误差,R:测量误差吧 先看看吧
页:
[1]