kalman filter 卡尔曼滤波的例子

2008-11-01 02:02

因为在研究中使用了kalman 滤波，这是一个挺难理解的控制理论，对于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__INCL UDED_)

#define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_

#if _MSC_VER > 1000

#pragma once

#endif // _MSC_VER > 1000

#include

#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__INCLU DED_)

============================kalman.cpp=============================== =

#include "kalman.h"

#include

/* 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

}

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;

}