卡尔曼滤波简介

1、卡尔曼滤波简介 算法实现代码    最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。现设线性时变

2、系统的离散状态防城和观测方程为：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)&#

3、215;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)'

4、+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

5、0;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);&

6、#160;   for (i=0; i<=n-1; i+)      for (j=0; j<=n-1; j+)         ii=i*l+j; aii=0.0;          for (kk=0; kk<

7、;=n-1; kk+)            aii=aii+pi*n+kk*fj*n+kk;            for (i=0; i<=n-1; i+)      for (j=0; j<=n-1; j+)&#

8、160;        ii=i*n+j; pii=qii;          for (kk=0; kk<=n-1; kk+)            pii=pii+fi*n+kk*akk*l+j;    

9、60;       for (ii=2; ii<=k; ii+)       for (i=0; i<=n-1; i+)        for (j=0; j<=m-1; j+)        

10、;   jj=i*l+j; ajj=0.0;            for (kk=0; kk<=n-1; kk+)              ajj=ajj+pi*n+kk*hj*n+kk;      

11、            for (i=0; i<=m-1; i+)        for (j=0; j<=m-1; j+)           jj=i*m+j; ejj=rjj;  &

12、#160;         for (kk=0; kk<=n-1; kk+)              ejj=ejj+hi*n+kk*akk*l+j;               &

13、#160;  js=rinv(e,m);        if (js=0)            free(e); free(a); free(b); return(js);        for (i=0; i<=n-1; 

14、;i+)        for (j=0; j<=m-1; j+)           jj=i*m+j; gjj=0.0;            for (kk=0; kk<=m-1; kk+) 

15、             gjj=gjj+ai*l+kk*ej*m+kk;                  for (i=0; i<=n-1; i+)        

16、60;  jj=(ii-1)*n+i; xjj=0.0;            for (j=0; j<=n-1; j+)              xjj=xjj+fi*n+j*x(ii-2)*n+j;      

17、;            for (i=0; i<=m-1; i+)           jj=i*l; bjj=y(ii-1)*m+i;            for (j=0; j

18、<=n-1; j+)              bjj=bjj-hi*n+j*x(ii-1)*n+j;                  for (i=0; i<=n-1; i+)    

19、       jj=(ii-1)*n+i;            for (j=0; j<=m-1; j+)              xjj=xjj+gi*m+j*bj*l;     

20、;             if (ii<k)           for (i=0; i<=n-1; i+)            for (j=0; j<=n

21、-1; j+)               jj=i*l+j; ajj=0.0;                for (kk=0; kk<=m-1; kk+)      &

22、#160;           ajj=ajj-gi*m+kk*hkk*n+j;                if (i=j) ajj=1.0+ajj;             

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

24、   jj=i*l+j; bjj=0.0;                for (kk=0; kk<=n-1; kk+)                  bjj=bjj+ai*l+

25、kk*pkk*n+j;                          for (i=0; i<=n-1; i+)            for (j=0; j&

26、lt;=n-1; j+)               jj=i*l+j; ajj=0.0;                for (kk=0; kk<=n-1; kk+)     &#

27、160;            ajj=ajj+bi*l+kk*fj*n+kk;                          for (i=0; i<=n-1; i+) 

28、;           for (j=0; j<=n-1; j+)               jj=i*n+j; pjj=qjj;             

29、60;  for (kk=0; kk<=n-1; kk+)                  pjj=pjj+fi*n+kk*aj*l+kk;                  

30、60;               free(e); free(a); free(b);    return(js);  C+实现代码如下：=kalman.h=/ kalman.h: interface for the kalman class./#if !defined(AFX_KALMAN_H_

31、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 kalma

32、n  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,

33、 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 

34、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;ka

35、lman:kalman(int x,int xv,int y,int yv)         cvkalman = cvCreateKalman( 4, 4, 0 );    state = cvCreateMat( 4, 1, CV_32FC1 );    proce

36、ss_noise = cvCreateMat( 4, 1, CV_32FC1 );    measurement = cvCreateMat( 4, 1, CV_32FC1 );    int code = -1;        /* create matrix

37、0;data */     const float A =     1, T, 0, 0,   0, 1, 0, 0,   0, 0, 1, T,   0, 0, 0, 1      

38、60;     const float H =      1, 0, 0, 0,    0, 0, 0, 0,   0, 0, 1, 0,   0, 0, 0, 0      &#

39、160;       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,  

40、; 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, 

41、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,&#

42、160;1, 0,   0, 0, 0, 0              cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );    cvZero( measurement );    

43、60;   cvRandSetRange( &rng, 0, 0.1, 0 );    rng.disttype = CV_RAND_NORMAL;    cvRand( &rng, state );    memcpy( cvkalman->transition_matrix->data.fl, A

44、, 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.

45、fl, P, sizeof(P);    memcpy( cvkalman->measurement_noise_cov->data.fl, R, sizeof(R);    /cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5) );        /cvSetIde

46、ntity( cvkalman->error_cov_post, cvRealScalar(1); /cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) );    /* choose initial state */    state->data.fl0=x;    state

47、->data.fl1=xv;    state->data.fl2=y;    state->data.fl3=yv;    cvkalman->state_post->data.fl0=x;    cvkalman->state_post->data.fl1=xv;    cvkalman->state_post->data.fl2=y;&#

48、160;   cvkalman->state_post->data.fl3=yv; cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl0), 0 );    cvRand( &rng, process_noise );        &#

49、160;CvPoint2D32f kalman:get_predict(float x, float y)        /* update state with current position */    state->data.fl0=x;    state->data.fl2=y;   &

50、#160;    /* 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.fl0), 0 );    cvRand( &rng, measurement );         /* xk=A?xk-1+B?uk+wk */    cvMatMulAdd( cvkalman->transition_matrix, state,