国外网站上找到的简单的卡尔曼滤波算法

ailibuli · 发表于 2011-8-21 02:02:02

折腾了一晚上，搜索了好久也没能整明白卡尔曼滤波里的几个变量都具体起什么作用。
稀里糊涂的算有效果了。
国内网站大家都是你转载我的，我转载你的，没几个说的明白的，更有甚者，转载都出现错误。
没办法，试试去外网找，还真找着好用的了。呵呵。贴出来大家参考下。

/** 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 [diff:%.3f]\n",z_measured,fabs(z_real-z_measured));
      printf("Kalman position: %6.3f [diff:%.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 这两个噪声量是干嘛的吧！它们的大小会影响什么？

xsh2005105326 · 发表于 2011-8-21 02:09:55

mark

zhikai_wu · 发表于 2011-8-21 06:57:13

wy2000 · 发表于 2011-8-21 07:18:20

mark

ssaweee · 发表于 2011-8-21 07:52:29

怎么国内资料这么难找？

cbeeee · 发表于 2011-8-21 07:53:53

这就是楼主说的那个被无数人转来转去的那个温度的例子吧……你去外国网站还是找来了这个例子
Q是系统噪声，R是测量噪声，大概意思就是说该信谁多一点，如果Q=0就最后完全信预测结果，R=0则完全信测量结果

编辑加入：搜这类东西用google scholar吧，很好用的，不过不要期望现成的代码就是了

lijieamd · 发表于 2011-8-21 09:12:22

这类东西想搜索到实用的代码是想都不用想的,国内外都一样
代码确实是有
国内的一般是学生用来做作业的代码,选的例子虚无缥缈暂且不说,要么是MATLAB的验证型代码,若是C或C++的,代码风格和效率那简直就是悲剧,这类代码抄来抄去非常普遍.
而国外的代码,一般都是二次简化的代码,何谓二次简化?
第一次,将例子降维,比如三维多变量系统,他们会把它变成一维单变量的,比如姿态的卡尔曼滤波简化为单轴的卡尔曼滤波
第二次,将代码简化,主要是为了提高效率,但简化后的代码基本没有可更改性,可读性也是大大降低,甚至看不懂,但是代码的编写基础却是十分严格复杂的推导

要想针对应用写这类代码,只能通过自己理解再慢慢的积累起来,比如先针对硬件写一个高效的数学库,然后通过这个库来建立算法
而算法的基础理论,还是老老实实的啃一啃书,啃一啃资料,老外的书和资料还是毫不含糊的

p4s5j6 · 发表于 2011-8-21 09:27:35

mark

fanwt · 发表于 2011-8-21 09:41:45

mark
~

02015678 · 发表于 2011-8-21 09:42:47

楼主可以看看这个《Some of my Experiences with Kalman Filters》By Ronald Yannone
http://www.megasociety.org/noesis/138/kalman.html
这篇文章说了作者的一些经验，并且在最后给出了
R、Q、P取值不当对系统的影响以及对策。

02015678 · 发表于 2011-8-21 10:04:33

还有我刚刚从Google Books上找到的一本书，部分预览，平均每10页抽掉2页，基本还能看
http://books.google.com/books?id=J_fqMHOCzB8C&source=gbs_navlinks_s

Kalman filtering: theory and practice using MATLAB

asha · 发表于 2011-8-21 10:09:12

一个是过程噪声，一个是观测噪声

FlashNuk · 发表于 2011-8-21 11:07:39

这个好！

MouseCat · 发表于 2011-8-21 11:26:55

MARK

ailibuli · 发表于 2011-8-21 12:29:45

回复【5楼】cbeeee
-----------------------------------------------------------------------

恩，想想确实是这样。

谢谢！

ailibuli · 发表于 2011-8-21 12:30:29

回复【6楼】lijieamd
-----------------------------------------------------------------------

有道理，研究挺深啊。

ailibuli · 发表于 2011-8-21 12:35:01

回复【9楼】02015678 周光
-----------------------------------------------------------------------

确实不错！谢谢！

3050311118 · 发表于 2011-8-21 13:43:10

MARK一下

cgbabc · 发表于 2011-8-21 14:17:08

mark

yigang8a · 发表于 2011-8-26 19:47:04

mark!

ksh84222 · 发表于 2011-10-5 00:42:02

不错

ghostxdy · 发表于 2011-10-5 01:41:47

先MARK
日后用得着

xizi · 发表于 2011-10-5 02:53:16

mark

kneken · 发表于 2011-10-5 08:35:27

mark

gotofly100 · 发表于 2011-10-5 08:37:28

mark

lxliang09 · 发表于 2011-10-9 19:13:28

mark

xiaoding818 · 发表于 2011-10-9 20:29:21

学习了一些东西，虽然也一样没弄明白卡尔曼滤波，但知道有Google Scholar和Google Books这两个好东东。

b_u_g · 发表于 2011-10-11 21:26:36

mark，太高深，看来数学是基石啊，以前还以为数学
除了算账，学的没啥用。。。

dujun168 · 发表于 2011-10-18 22:25:38

mark

workinging2 · 发表于 2011-10-20 12:11:46

找卡尔曼本人问问最好

denglu · 发表于 2011-10-22 20:48:33

Mark

HYZ1989 · 发表于 2011-10-22 22:09:46

mark

nirvanasyl · 发表于 2011-11-2 13:45:14

MARK

nirvanasyl · 发表于 2011-11-2 13:58:36

MARK卡尔曼

jjl3 · 发表于 2011-11-2 15:56:02

mark

yaowujun123 · 发表于 2011-11-2 18:52:47

回复【楼主位】ailibuli 爱理不理
-----------------------------------------------------------------------

很好

theloong · 发表于 2011-11-2 19:04:10

mark

luweixuan · 发表于 2011-11-2 23:15:42

mark

oaixuw · 发表于 2011-11-2 23:25:58

mark

double_forever · 发表于 2011-11-3 09:04:57

mark

avrwoo · 发表于 2011-11-3 09:18:39

MARK

tianxuelinan · 发表于 2011-11-3 17:45:57

建议lz先看看kalman的书把那5个公式弄懂再看程序就比较简单了

Dalong357 · 发表于 2011-11-5 18:30:42

mark

FpvCamera · 发表于 2011-11-5 18:55:43

mark

136420962 · 发表于 2011-11-13 16:10:00

mark

wisology · 发表于 2011-11-13 21:31:02

mark...

nil0 · 发表于 2011-11-14 09:18:36

mark!

3466756555 · 发表于 2011-11-14 10:23:28

mark

haigerl · 发表于 2011-11-14 11:44:58

mark

hitor · 发表于 2011-12-9 01:10:59

mark

zclcom79 · 发表于 2011-12-9 09:17:39

mark

linlin990 · 发表于 2011-12-9 12:04:20

MARK

whyjld · 发表于 2011-12-9 12:53:01

mark

rain_299901 · 发表于 2011-12-13 10:15:24

看看

ligongxiaobie · 发表于 2012-2-1 16:36:10

MARK

changqing · 发表于 2012-2-16 17:16:13

其实一维的卡尔曼滤波很简单，只需要五个基本的公式，（两个时间更新的，三个状态更新的）套公式就可以解决；不过要明白原理就要多多看看了；一篇论文名字是《An Introduction to the Kalman Filter》,讲得较为详细。

changqing · 发表于 2012-2-16 17:28:00

回复【楼主位】ailibuli 爱理不理
-----------------------------------------------------------------------
根据我看的一片论文《An Introduction to the Kalman Filter》中介绍的公式，你所列出的程序是一个验证性的，而不是应用性的，不过思路很清晰；在应用性的程序中z_measured = z_real + frand()*0.09; //the real measurement plus noise 是不必要的，因为计算更新状态的表达式应用的本身就是测量值，在这里是为了验证程序最后能够得到z-real的值，以验证算法的正确性；R值一般是离线测量，而Q值一般是取较小的值；

goofye · 发表于 2012-5-27 11:16:42

mark一个，留着学习

scp_tian · 发表于 2012-6-13 16:49:06

学习了。

brb2010 · 发表于 2012-6-13 17:28:45

终于可以注册新账号了。mark

xuepeng · 发表于 2012-6-13 18:04:02

国内的资料基本不敢看，看来看去大家都一个样。Google Scholar 还是不错的

just_be_fine · 发表于 2012-6-13 20:33:56

顶一下 ~ 有机会试试看~

mypc16888 · 发表于 2012-6-14 09:31:58

不错啊，看看

phidiawang · 发表于 2012-6-15 14:28:33

卡尔曼滤波，mark

McBee · 发表于 2012-6-17 23:28:40

mark 一下卡曼滤波

chenyixing · 发表于 2012-7-4 23:09:25

mark

john800422 · 发表于 2012-7-5 02:44:47

學習了~

411298932 · 发表于 2012-7-17 16:59:01

mark

fm007 · 发表于 2012-7-20 14:39:42

卡尔曼的Mark一下

1535414824 · 发表于 2012-7-20 14:44:38

mark

szy19811114 · 发表于 2012-7-20 15:21:01

marrrrrrkkkkk

xml2028 · 发表于 2012-8-7 19:29:34

这个学习要

Fulai · 发表于 2012-8-9 00:45:40

的确蛮简洁的~~~~

Freezing_ · 发表于 2012-8-11 18:47:58

翻了2页帖子，大部分都是mark……

Fulai · 发表于 2012-8-11 19:12:55

个人认为卡尔曼算法应用在ARM或者MCU中的简化方式，是杜绝浮点的。

fengtianzhifeng · 发表于 2012-8-21 15:54:34

mark

liuchuanxhu · 发表于 2012-8-27 18:43:07

嗯，要多加强数学的学习

AkeBest · 发表于 2012-9-21 15:17:32

卡尔曼滤波的，我都mark一下

chenfzg · 发表于 2012-9-22 01:03:10

谢谢了

linhonggui2007 · 发表于 2012-9-27 17:03:50

传感器的测量值加速度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 ;

请问这个正确吗

30zero · 发表于 2012-12-21 13:41:46

MARK 卡尔曼，学习学习了~

xiong57785 · 发表于 2013-5-14 21:03:23

mark

WUST_LJS · 发表于 2013-5-14 23:10:15

卡尔曼。。

残忆视觉 · 发表于 2013-11-3 16:33:16

smurfjack · 发表于 2013-11-17 22:14:41

不错

qqtanzhu · 发表于 2013-12-9 16:29:41

mark 以后学习用

yhybear0 · 发表于 2013-12-9 19:30:25

感谢楼主，学习一下！

xuyulei008 · 发表于 2013-12-9 22:04:43

静不下心来看啊。。。

翼殇擎月 · 发表于 2013-12-12 15:15:27

感谢楼主

6091820503 · 发表于 2013-12-13 13:35:53

国内风气真心不如国外啊，好东西还是得上英文论坛找啊,可惜还有gfw

ltd62340177 · 发表于 2013-12-13 17:44:10

卡尔曼滤波，毕业论文写信息融合的几个算法的时候就有这个，就没弄懂是什么，太虚无缥缈了

Eric321 · 发表于 2013-12-13 17:55:02

mark一下

chushibinsaobao · 发表于 2013-12-20 16:17:35

怎么感觉没有太多使用的呢，大家老师MARK，讲讲道理吗？

jorry · 发表于 2016-11-29 21:28:01

Q：预测误差，R：测量误差吧

xml2028 · 发表于 2021-8-28 07:30:38

先看看吧

国外网站上找到的简单的卡尔曼滤波算法

阿莫论坛20周年了！感谢大家的支持与爱护！！

本帖子中包含更多资源