卡爾曼濾波算法(CC兩種實(shí)現(xiàn)代碼)

1、卡爾曼濾波算法實(shí)現(xiàn)代碼C+實(shí)現(xiàn)代碼如下：=kalman.h=/kalman.h:interfaceforthekalmanclass./#if!defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_)#defineAFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_#if_MSC_VER>1000#pragmaonce#endif/_MSC_VER>1000#include<math.h>#include"cv.h&

2、quot;classkalmanpublic:voidinit_kalman(intx,intxv,inty,intyv);CvKalman*cvkalman;CvMat*state;CvMat*process_noise;CvMat*measurement;constCvMat*prediction;CvPoint2D32fget_predict(floatx,floaty);kalman(intx=0,intxv=0,inty=0,intyv=0);/virtualkalman();#endif/!defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8

3、BF57636F2c0_INCLUDED_)=kalman.cpp=#include"kalman.h"#include<stdio.h>/*testerdeprintertouteslesvaleursdesvecteurs*/*testerdechangerlesmatricesdunoises*/*replacestatebycvkalman->state_post?*/CvRandStaterng;constdoubleT=0.1;kalman:kalman(intx,intxv,inty,intyv)cvkalman=cvCreateKalman

4、(4,4,0);state=cvCreateMat(4,1,CV_32FC1);process_noise=cvCreateMat(4,1,CV_32FC1);measurement=cvCreateMat(4,1,CV_32FC1);intcode=-1;/*creatematrixdata*/constfloatA口=1,T,0,0,0,1,0,0,0,0,1,T,0,0,0,1;constfloatH口=1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0;constfloatP口=pow(320,2),pow(320,2)/T,0,0,pow(320,2)/T,pow(320

5、,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)；constfloatQ口=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;constfloatR口=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

6、(&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

7、.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);/*chooseinitialstate*/stat

8、e->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;cvkalman->state_post->data.fl2=y;cvkalman->state_post->data.fl3=yv;cvRandSetRange(&rng,0,sqrt(cvkalman->process_noise_co

9、v->data.fl0),0)；cvRand(&rng,process_noise);float x, float y)CvPoint2D32fkalman:get_predict(/*updatestatewithcurrentposition*/state->data.fl0=x;state->data.fl2=y;/*predictpointposition*/*x'k=A敏k+B敏kP'k=A敏k-1*AT+Q*/cvRandSetRange(&rng,0,sqrt(cvkalman->measurement_noise_cov-

10、>data.fl0),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);cvKalmanCorrect(cvkalman,measurement);

11、floatmeasured_value_x=measurement->data.fl0;floatmeasured_value_y=measurement->data.fl2;constCvMat*prediction=cvKalmanPredict(cvkalman,0);floatpredict_value_x=prediction->data.fl0;floatpredict_value_y=prediction->data.fl2;return(cvPoint2D32f(predict_value_x,predict_value_y);voidkalman:in

12、it_kalman(intx,intxv,inty,intyv)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;cvkalman->state_post->data.fl2=y;cvkalman->state_post->data.fl3=yv;c語言實(shí)現(xiàn)代碼如下#include"s

13、tdlib.h"#include"rinv.c"intlman(n,m,k,f,q,r,h,y,x,p,g)intn,m,k;doublef,q口,r,h,y口刈,p,g口;inti,j,kk,ii,l,jj,js;Idouble*e,*a,*b;e=malloc(m*m*sizeof(double);Il=m;if(l<n)l=n;Ia=malloc(l*l*sizeof(double);b=malloc(l*l*sizeof(double);for(i=0;i<=n-1;i+)Ifor(j=0;j<=n-1;j+)中申ii=i*l+j;aii

14、=0.0;Ifor(kk=0;kk<=n-1;kk+)aii=aii+pi*n+kk*fj*n+kk;卜for(i=0;i<=n-1;i+)Ifor(j=0;j<=n-1;j+)中申ii=i*n+j;pii=qii;for(kk=0;kk<=n-1;kk+)Ipii=pii+fi*n+kk*akk*l+j;卜Ifor(ii=2;ii<=k;ii+)中申for(i=0;i<=n-1;i+)Ifor(j=0;j<=m-1;j+)jj=i*l+j;a皿=0.0;for(kk=0;kk<=n-1;kk+)ajj=ajj+pi*n+kk*hj*n+kk;f

15、or(i=0;i<=m-1;i+)for(j=0;j<=m-1;j+)jj=i*m+j;ejj=rjj;for(kk=0;kk<=n-1;kk+)ejj=ejj+hi*n+kk*akk*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;gjj=0.0;for(kk=0;kk<=m-1;kk+)gjj=gjj+ai*l+kk*ej*m+kk;for(i=0;i<=n-1;i+)jj=(ii-1)*

16、n+i;xjj=0.0;for(j=0;j<=n-1;j+)xjj=xjj+fi*n+j*x(ii-2)*n+j;for(i=0;i<=m-1;i+)jj=i*l;bjj=y(ii-1)*m+i;for(j=0;j<=n-1;j+)bjj=bjj-hi*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+)xjj=xjj+gi*m+j*bj*l;卜)if(ii<k)部for(i=0;i<=n-1;i+)for(j=0;j<=n-1;j+)施jj=i*l+j;a皿=0.0;for(kk=0;kk<=m-1;kk+)ajj=ajj-gi*m+kk*hkk*n+j;Iif(i=j)ajj=1.0+ajj;卜)for(i=0;i<=n-1;i+)for(j=0;j<=n-1;j+)前jj=i*l+j;bjj=0.0;for(kk=0;kk<=n-1;kk+)bjj=bjj+ai*l+kk*pkk