因爲在研究中使用了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