卡爾曼濾波簡介＋ 算法實作代碼(C/C++)(轉)

原帖位址：http://www.cnblogs.com/jason-jiang/archive/2007/01/13/619643.html

    最佳線性濾波理論起源于 40 年代美國科學家 Wiener 和前蘇聯科學家Ｋ олмогоров 等人的研究工作，後人統稱為維納濾波理論。從理論上說，維納濾波的最大缺點是必須用到無限過去的資料，不适用于實時處理。為了克服這一缺點， 60 年代 Kalman 把 狀态空間模型引入濾波理論，并導出了一套遞推估計算法，後人稱之為卡爾曼濾波理論。卡爾曼濾波是以最小均方誤差為估計的最佳準則，來尋求一套遞推估計的算 法，其基本思想是：采用信号與噪聲的狀态空間模型，利用前一時刻地估計值和現時刻的觀測值來更新對狀态變量的估計，求出現時刻的估計值。它适合于實時處理 和計算機運算。

現設線性時變系統的離散狀态防城和觀測方程為：

X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

Y(k) = H(k)·X(k)+N(k)

其中

X(k)和Y(k)分别是k時刻的狀态矢量和觀測矢量

F(k,k-1)為狀态轉移矩陣

U(k)為k時刻動态噪聲

T(k,k-1)為系統控制矩陣

H(k)為k時刻觀測矩陣

N(k)為k時刻觀測噪聲

則卡爾曼濾波的算法流程為：

1. 預估計X(k)^= F(k,k-1)·X(k-1)  

2. 計算預估計協方差矩陣

   C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'

   Q(k) = U(k)×U(k)'  

3. 計算卡爾曼增益矩陣

   K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)

   R(k) = N(k)×N(k)'  

4. 更新估計

   X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]  

5. 計算更新後估計協防差矩陣

   C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'  

6. X(k+1) = X(k)~

   C(k+1) = C(k)~

   重複以上步驟

其c語言實作代碼如下：

#include  " stdlib.h "

  #include  " rinv.c "

   int  lman(n,m,k,f,q,r,h,y,x,p,g)

   int  n,m,k;

   double  f[],q[],r[],h[],y[],x[],p[],g[];

{  int  i,j,kk,ii,l,jj,js;

     double   * e, * a, * b;

    e = malloc(m * m * sizeof ( double ));

    l = m;

     if  (l < n) l = n;

    a = malloc(l * l * sizeof ( double ));

    b = malloc(l * l * sizeof ( double ));

     for  (i = 0 ; i <= n - 1 ; i ++ )

       for  (j = 0 ; j <= n - 1 ; j ++ )

{ ii = i * l + j; a[ii] = 0.0 ;

           for  (kk = 0 ; kk <= n - 1 ; kk ++ )

            a[ii] = a[ii] + p[i * n + kk] * f[j * n + kk];

        }

     for  (i = 0 ; i <= n - 1 ; i ++ )

       for  (j = 0 ; j <= n - 1 ; j ++ )

{ ii = i * n + j; p[ii] = q[ii];

           for  (kk = 0 ; kk <= n - 1 ; kk ++ )

            p[ii] = p[ii] + f[i * n + kk] * a[kk * l + j];

        }

     for  (ii = 2 ; ii <= k; ii ++ )

{  for  (i = 0 ; i <= n - 1 ; i ++ )

         for  (j = 0 ; j <= m - 1 ; j ++ )

{ jj = i * l + j; a[jj] = 0.0 ;

             for  (kk = 0 ; kk <= n - 1 ; kk ++ )

              a[jj] = a[jj] + p[i * n + kk] * h[j * n + kk];

          }

         for  (i = 0 ; i <= m - 1 ; i ++ )

         for  (j = 0 ; j <= m - 1 ; j ++ )

{ jj = i * m + j; e[jj] = r[jj];

             for  (kk = 0 ; kk <= n - 1 ; kk ++ )

              e[jj] = e[jj] + h[i * n + kk] * a[kk * l + j];

          }

        js = rinv(e,m);

         if  (js == 0 ) 

{ free(e); free(a); free(b);  return (js);}

         for  (i = 0 ; i <= n - 1 ; i ++ )

         for  (j = 0 ; j <= m - 1 ; j ++ )

{ jj = i * m + j; g[jj] = 0.0 ;

             for  (kk = 0 ; kk <= m - 1 ; kk ++ )

              g[jj] = g[jj] + a[i * l + kk] * e[j * m + kk];

          }

         for  (i = 0 ; i <= n - 1 ; i ++ )

{ jj = (ii - 1 ) * n + i; x[jj] = 0.0 ;

             for  (j = 0 ; j <= n - 1 ; j ++ )

              x[jj] = x[jj] + f[i * n + j] * x[(ii - 2 ) * n + j];

          }

         for  (i = 0 ; i <= m - 1 ; i ++ )

{ jj = i * l; b[jj] = y[(ii - 1 ) * m + i];

             for  (j = 0 ; j <= n - 1 ; j ++ )

              b[jj] = b[jj] - h[i * n + j] * x[(ii - 1 ) * n + j];

          }

         for  (i = 0 ; i <= n - 1 ; i ++ )

{ jj = (ii - 1 ) * n + i;

             for  (j = 0 ; j <= m - 1 ; j ++ )

              x[jj] = x[jj] + g[i * m + j] * b[j * l];

          }

         if  (ii < k)

{  for  (i = 0 ; i <= n - 1 ; i ++ )

             for  (j = 0 ; j <= n - 1 ; j ++ )

{ jj = i * l + j; a[jj] = 0.0 ;

                 for  (kk = 0 ; kk <= m - 1 ; kk ++ )

                  a[jj] = a[jj] - g[i * m + kk] * h[kk * n + j];

                 if  (i == j) a[jj] = 1.0 + a[jj];

              }

             for  (i = 0 ; i <= n - 1 ; i ++ )

             for  (j = 0 ; j <= n - 1 ; j ++ )

{ jj = i * l + j; b[jj] = 0.0 ;

                 for  (kk = 0 ; kk <= n - 1 ; kk ++ )

                  b[jj] = b[jj] + a[i * l + kk] * p[kk * n + j];

              }

             for  (i = 0 ; i <= n - 1 ; i ++ )

             for  (j = 0 ; j <= n - 1 ; j ++ )

{ jj = i * l + j; a[jj] = 0.0 ;

                 for  (kk = 0 ; kk <= n - 1 ; kk ++ )

                  a[jj] = a[jj] + b[i * l + kk] * f[j * n + kk];

              }

             for  (i = 0 ; i <= n - 1 ; i ++ )

             for  (j = 0 ; j <= n - 1 ; j ++ )

{ jj = i * n + j; p[jj] = q[jj];

                 for  (kk = 0 ; kk <= n - 1 ; kk ++ )

                  p[jj] = p[jj] + f[i * n + kk] * a[j * l + kk];

              }

          }

      }

    free(e); free(a); free(b);

     return (js);

  }

C++實作代碼如下：

============================ 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 >

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));

     // cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5) );    

     // cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1));

  // cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) );

    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;

}