kalman filter 卡爾曼濾波的例子

因爲在研究中使用了kalman 濾波，這是一個挺難理解的控制理論，我花了好長時間才瞭解一些基本的概念，opencv雖然提供了一個實例，但是這個例子基於c的，不容易看懂，也不好重用，後來我整理了一個簡單的類來，期間在論壇上有一個handsome & romantic 的法國小夥也在研究這個濾波，後來我給了他程序，他修改後發給了我，所以這裏的代碼也有他的部分，算是中法合作產品 :)

對於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