卡尔曼滤波算法(C--C++两种实现代码)(可编辑修改word版).pdf

《卡尔曼滤波算法(C--C++两种实现代码)(可编辑修改word版).pdf》由会员分享，可在线阅读，更多相关《卡尔曼滤波算法(C--C++两种实现代码)(可编辑修改word版).pdf（9页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、=kalman.h=/kalman.h:interface for the kalman class./#if!defined(AFX_KALMAN_H ED3D740F_01D2_4616_8B74_8BF57636F2C0 IN CLUDED_)#define AFX_KALMAN_H ED3D740F_01D2_4616_8B74_8BF57636F2C0 INCLU DED_#if _MSC_VER 1000#pragma once#endif/_MSC_VER 1000#include#include cv.h class kalman public:void init_kalman

2、(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);卡尔曼滤波算法实现代码 C+实现代码如下：kalman(int x=0,int xv=0,int y=0,int yv=0);/virtual kalman();#endif/!defined(AFX_KALMAN_H ED3D740F_01D2_4616_8B74_8BF57

3、636F2C 0 INCLUDED_)=kalman.cpp=#include kalman.h#include /*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);s

4、tate=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,

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);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 ;state-data.fl3=yv;cvkalman-state_post-data.fl0=x;cvkalman-state_post-data.fl1=xv;cvkalman-state_post-data.fl2=y;cvkalm

6、an-state_post-data.fl3=yv;cvRandSetRange(&rng,0,sqrt(cvkalman-process_noise_cov-data.fl0),0);cvRand(&rng,process_noise);CvPoint2D32f kalman:get_predict(float x,float y)/*update state with current position*/state-data.fl0=x;state-data.fl2=y;/*predict point position*/*xk=A 鈥 k+B 鈥 k Pk=A 鈥k-1*AT+Q*/cv

7、RandSetRange(&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,meas urement,measurement);cvKa

8、lmanCorrect(cvkalman,measurement );float measured_value_x=measurement-data.fl0;float measured_value_y=measurement-data.fl2;const CvMat*prediction=cvKalmanPredict(cvkalman,0);float predict_value_x=prediction-data.fl0;float predict_value_y=prediction-data.fl2;return(cvPoint2D32f(predict_value_x,predic

9、t_value_y);void kalman:init_kalman(int x,int xv,int y,int yv)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;#include stdlib.h#include rinv.c int lman(n

10、,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(ln)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;aii=0.0;for(kk=0;kk=n-1;kk+)aii=aii+pi*n+kk*fj*n+kk;for(i=0;i=n-1;i+

11、)for(j=0;j=n-1;j+)ii=i*n+j;pii=qii;for(kk=0;kk=n-1;kk+)pii=pii+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+)jj=i*l+j;ajj=0.0;c 语言实现代码如下：for(kk=0;kk=n-1;kk+)ajj=ajj+pi*n+kk*hj*n+kk;for(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

12、=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)*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*

13、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(iik)for(i=0;i=n-1;i+)for(j=0;j=n-1;j+)jj=i*l+j;ajj=0.0;for(kk=0;kk=m-1;kk+)ajj=ajj-gi*m+kk*hkk*n+j;if(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*n+j;for(i=0;i=n-1;i+)for(j=0;j=n-1;j+)jj=i*l+j;ajj=0.0;for(kk=0;kk=n-1;kk+)ajj=ajj+bi*l+kk*fj*n+kk;for(i=0;i=n-1;i+)for(j=0;j=n-1;j+)jj=i*n+j;pjj=qjj;for(kk=0;kk=n-1;kk+)pjj=pjj+fi*n+kk*aj*l+kk;free(e);free(a);free(b);return(js);