因为在研究中使用了kalman 滤波,这是一个挺难理解的控制理论,我花了好长时间才了解一些基本的概念,opencv虽然提供了一个实例,但是这个例子基于c的,不容易看懂,也不好重用,后来我整理了一个简单的类来,期间在论坛上有一个handsome & romantic 的法国小伙也在研究这个滤波,后来我给了他程序,他修改后发给了我,所以这里的代码也有他的部分,算是中法合作产品 :)
对于kalman的初学者来讲,像我这样没什么数学功底的人,看教科书真是很累,说实在的,我觉得老外的基础理论的书都很评议近人,不像国内那些教授搞得那么悬虚,
初学者可以参考
http://bbs.matwav.com/index.jsp 研学论坛有几篇通俗易懂的中文解释
http://www.cs.unc.edu/~welch/kalman/ 这里是老外综合的kalman基地,很不错的。
代码示例:
- ============================kalman.h================================
-
-
-
-
-
- #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);
-
-
-
- };
-
- #endif // !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_)
-
-
- ============================kalman.cpp================================
-
- #include "kalman.h"
- #include <stdio.h>
-
-
-
-
-
-
-
- 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;
-
-
- 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));
-
-
-
-
-
-
- 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){
-
-
-
- state->data.fl[0]=x;
- state->data.fl[2]=y;
-
-
-
-
-
- cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl[0]), 0 );
- cvRand( &rng, measurement );
-
-
- cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman->state_post );
-
-
- cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, measurement, measurement );
-
-
-
-
-
- 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