卡尔曼滤波算法(C--C++两种实现代码)

1、卡尔曼滤波算法实现代码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&#

2、160;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;  CvMa

3、t* 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_E

4、D3D740F_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 */

5、* 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,

6、60;4, 0 );    state = cvCreateMat( 4, 1, CV_32FC1 );    process_noise = cvCreateMat( 4, 1, CV_32FC1 );    measurement = cvCreateMat( 4, 1, CV_32F

7、C1 );    int code = -1;        /* create matrix data */     const float A =     1, T, 0, 0,   0, 1,&

8、#160;0, 0,   0, 0, 1, T,   0, 0, 0, 1            const float H =      1, 0, 0, 0,    0, 0,

9、 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,

10、   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,  

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

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

13、     cvRandSetRange( &rng, 0, 0.1, 0 );    rng.disttype = CV_RAND_NORMAL;    cvRand( &rng, state );    memcpy( cvkalman->transition_matrix->data.f

14、l, 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-

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

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

17、160;state->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

18、.fl2=y;    cvkalman->state_post->data.fl3=yv; cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl0), 0 );    cvRand( &rng, process_noise );    CvPoint2D32f 

19、;kalman:get_predict(float x, float y)    /* update state with current position */    state->data.fl0=x;    state->data.fl2=y;    /* predict point position

20、 */    /* x'k=A鈥k+B鈥k       P'k=A鈥k-1*AT + Q */    cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl0), 0 );    cvRa

21、nd( &rng, measurement );         /* xk=A?xk-1+B?uk+wk */    cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman->state_post );    

22、0;   /* zk=H?xk+vk */    cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, measurement, measurement );    cvKalmanCorrect( cvkalman, measurement );    flo

23、at measured_value_x = measurement->data.fl0;    float measured_value_y = measurement->data.fl2;     const CvMat* prediction = cvKalmanPredict( cvkalman, 0 );    flo

24、at predict_value_x = prediction->data.fl0;    float predict_value_y = prediction->data.fl2;    return(cvPoint2D32f(predict_value_x,predict_value_y);void kalman:init_kalman(int x,int xv,int y,int yv)&

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

26、0;  cvkalman->state_post->data.fl2=y;    cvkalman->state_post->data.fl3=yv;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&#

27、160;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); &#

28、160;  b=malloc(l*l*sizeof(double);    for (i=0; i<=n-1; i+)      for (j=0; j<=n-1; j+)         ii=i*l+j; aii=0.0;       

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

30、160;for (j=0; j<=n-1; j+)         ii=i*n+j; pii=qii;          for (kk=0; kk<=n-1; kk+)            pii=pi

31、i+fi*n+kk*akk*l+j;            for (ii=2; ii<=k; ii+)       for (i=0; i<=n-1; i+)        for (j=0; j<=m-1; j+)&#

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

33、*hj*n+kk;                  for (i=0; i<=m-1; i+)        for (j=0; j<=m-1; j+)         

34、60; jj=i*m+j; ejj=rjj;            for (kk=0; kk<=n-1; kk+)              ejj=ejj+hi*n+kk*akk*l+j;       

35、0;          js=rinv(e,m);        if (js=0)            free(e); free(a); free(b); return(js);      &#

36、160; for (i=0; i<=n-1; i+)        for (j=0; j<=m-1; j+)           jj=i*m+j; gjj=0.0;            for

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

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

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

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

41、0;i<=n-1; i+)           jj=(ii-1)*n+i;            for (j=0; j<=m-1; j+)              xjj=

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

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

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

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

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