kalman filter 卡尔曼滤波的例子
2011-02-23 13:37
357 查看
因为在研究中使用了kalman 滤波,这是一个挺难理解的控制理论,我花了好长时间才了解一些基本的概念,opencv虽然提供了一个实例,但是这个例子基于c的,不容易看懂,也不好重用,后来我整理了一个简单的类来,期间在论坛上有一个handsome & romantic 的法国小伙也在研究这个滤波,后来我给了他程序,他修改后发给了我,所以这里的代码也有他的部分,算是中法合作产品 :)
对于kalman的初学者来讲,像我这样没什么数学功底的人,看教科书真是很累,说实在的,我觉得老外的基础理论的书都很评议近人,不像国内那些教授搞得那么悬虚,
初学者可以参考
http://bbs.matwav.com/index.jsp 研学论坛有几篇通俗易懂的中文解释
http://www.cs.unc.edu/~welch/kalman/ 这里是老外综合的kalman基地,很不错的。
代码示例:
转载自:http://blog.csdn.net/hardVB/archive/2005/06/28/405582.aspx
对于kalman的初学者来讲,像我这样没什么数学功底的人,看教科书真是很累,说实在的,我觉得老外的基础理论的书都很评议近人,不像国内那些教授搞得那么悬虚,
初学者可以参考
http://bbs.matwav.com/index.jsp 研学论坛有几篇通俗易懂的中文解释
http://www.cs.unc.edu/~welch/kalman/ 这里是老外综合的kalman基地,很不错的。
代码示例:
============================kalman.h================================ // kalman.h: interface for the kalman class. // ////////////////////////////////////////////////////////////////////// #if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_) #define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_ #if _MSC_VER > 1000 #pragma once #endif // _MSC_VER > 1000 #include <math.h> #include "cv.h" class kalman { public: void init_kalman(int x,int xv,int y,int yv); CvKalman* cvkalman; CvMat* state; CvMat* process_noise; CvMat* measurement; const CvMat* prediction; CvPoint2D32f get_predict(float x, float y); kalman(int x=0,int xv=0,int y=0,int yv=0); //virtual ~kalman(); }; #endif // !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_) ============================kalman.cpp================================ #include "kalman.h" #include <stdio.h> /* tester de printer toutes les valeurs des vecteurs...*/ /* tester de changer les matrices du noises */ /* replace state by cvkalman->state_post ??? */ CvRandState rng; const double T = 0.1; kalman::kalman(int x,int xv,int y,int yv) { cvkalman = cvCreateKalman( 4, 4, 0 ); state = cvCreateMat( 4, 1, CV_32FC1 ); process_noise = cvCreateMat( 4, 1, CV_32FC1 ); measurement = cvCreateMat( 4, 1, CV_32FC1 ); int code = -1; /* create matrix data */ const float A[] = { 1, T, 0, 0, 0, 1, 0, 0, 0, 0, 1, T, 0, 0, 0, 1 }; const float H[] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }; const float P[] = { pow(320,2), pow(320,2)/T, 0, 0, pow(320,2)/T, pow(320,2)/pow(T,2), 0, 0, 0, 0, pow(240,2), pow(240,2)/T, 0, 0, pow(240,2)/T, pow(240,2)/pow(T,2) }; const float Q[] = { pow(T,3)/3, pow(T,2)/2, 0, 0, pow(T,2)/2, T, 0, 0, 0, 0, pow(T,3)/3, pow(T,2)/2, 0, 0, pow(T,2)/2, T }; const float R[] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }; cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI ); cvZero( measurement ); cvRandSetRange( &rng, 0, 0.1, 0 ); rng.disttype = CV_RAND_NORMAL; cvRand( &rng, state ); memcpy( cvkalman->transition_matrix->data.fl, A, sizeof(A)); memcpy( cvkalman->measurement_matrix->data.fl, H, sizeof(H)); memcpy( cvkalman->process_noise_cov->data.fl, Q, sizeof(Q)); memcpy( cvkalman->error_cov_post->data.fl, P, sizeof(P)); memcpy( cvkalman->measurement_noise_cov->data.fl, R, sizeof(R)); //cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5) ); //cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1)); //cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) ); /* choose initial state */ state->data.fl[0]=x; state->data.fl[1]=xv; state->data.fl[2]=y; state->data.fl[3]=yv; cvkalman->state_post->data.fl[0]=x; cvkalman->state_post->data.fl[1]=xv; cvkalman->state_post->data.fl[2]=y; cvkalman->state_post->data.fl[3]=yv; cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl[0]), 0 ); cvRand( &rng, process_noise ); } CvPoint2D32f kalman::get_predict(float x, float y){ /* update state with current position */ state->data.fl[0]=x; state->data.fl[2]=y; /* predict point position */ /* x'k=A鈥k+B鈥k P'k=A鈥k-1*AT + Q */ cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl[0]), 0 ); cvRand( &rng, measurement ); /* xk=A?xk-1+B?uk+wk */ cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman->state_post ); /* zk=H?xk+vk */ cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, measurement, measurement ); /* adjust Kalman filter state */ /* Kk=P'k鈥T鈥?H鈥'k鈥T+R)-1 xk=x'k+Kk鈥?zk-H鈥'k) Pk=(I-Kk鈥)鈥'k */ cvKalmanCorrect( cvkalman, measurement ); float measured_value_x = measurement->data.fl[0]; float measured_value_y = measurement->data.fl[2]; const CvMat* prediction = cvKalmanPredict( cvkalman, 0 ); float predict_value_x = prediction->data.fl[0]; float predict_value_y = prediction->data.fl[2]; return(cvPoint2D32f(predict_value_x,predict_value_y)); } void kalman::init_kalman(int x,int xv,int y,int yv) { state->data.fl[0]=x; state->data.fl[1]=xv; state->data.fl[2]=y; state->data.fl[3]=yv; cvkalman->state_post->data.fl[0]=x; cvkalman->state_post->data.fl[1]=xv; cvkalman->state_post->data.fl[2]=y; cvkalman->state_post->data.fl[3]=yv; }
转载自:http://blog.csdn.net/hardVB/archive/2005/06/28/405582.aspx
相关文章推荐
- kalman filter 卡尔曼滤波的例子
- kalman filter 卡尔曼滤波的例子
- kalman filter 卡尔曼滤波的例子
- kalman filter 卡尔曼滤波的例子【转载】
- 卡尔曼滤波(Kalman Filter)
- 卡尔曼滤波(Kalman Filter) 的进一步讨论
- 20170220. kalman filter(卡尔曼滤波)相关资料
- matlab实现卡尔曼滤波(Kalman filter)
- kalman filter的一个例子
- 卡尔曼滤波 – Kalman Filter (通俗的解释)
- 卡尔曼滤波 – Kalman Filter
- 卡尔曼滤波(Kalman Filter) 的进一步讨论
- 粒子滤波(PF:Particle Filter与卡尔曼滤波(Kalman Filter)相比较 (转载)
- 卡尔曼滤波 – Kalman Filter (通俗的解释)
- 卡尔曼滤波 – Kalman Filter
- 卡尔曼滤波-kalman filter
- SLAM学习笔记2:Kalman Filter(卡尔曼滤波) 与Least Square(最小二乘法) 的比较
- 卡尔曼滤波(Kalman Filter)
- 【SLAM】卡尔曼滤波(Kalman Filter)
- Kalman Filter : 理解卡尔曼滤波的三重境界