2022年2022年卡尔曼滤波算法 .pdf

《2022年2022年卡尔曼滤波算法 .pdf》由会员分享，可在线阅读，更多相关《2022年2022年卡尔曼滤波算法 .pdf（10页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

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 once #endif/ _MSC_VER 1000#include #include cv.h class

2、 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); 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 1 页，共 10 页

3、- - - - - - - - - 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 /* tester de printer toutes les valeurs des vecteurs*/* tester de changer les matrices du noises */*

4、replace state by cvkalman-state_post ? */CvRandState rng; constdouble 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

5、 = -1; 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 2 页，共 10 页 - - - - - - - - - /* create matrix data */constfloat A = 1, T , 0, 0, 0, 1, 0, 0, 0, 0, 1, T, 0, 0, 0, 1 ; constfloat H = 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 ; constfloat P = pow(320,2), pow(

6、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) ; constfloat 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 ; 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - -

7、- - - 名师精心整理 - - - - - - - 第 3 页，共 10 页 - - - - - - - - - constfloat 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-transi

8、tion_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-p

9、rocess_noise_cov, cvRealScalar(1e-5) ); /cvSetIdentity( cvkalman-error_cov_post, cvRealScalar(1); /cvSetIdentity( cvkalman-measurement_noise_cov, cvRealScalar(1e-1) ); /* choose initial state */ state-data.fl0=x; state-data.fl1=xv; state-data.fl2=y; 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - -

10、 - - - 名师精心整理 - - - - - - - 第 4 页，共 10 页 - - - - - - - - - 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_cov-data.fl0), 0 ); cvRand( &rng, proce

11、ss_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 */ cvRandSetRange( &rng, 0, sqrt(cvkalman-measurement_noise_cov-data.fl0), 0 ); cvRand( &rng, measuremen

12、t ); /* xk=A?xk-1+B?uk+wk */ cvMatMulAdd( cvkalman-transition_matrix, state, process_noise, cvkalman-state_post ); /* zk=H?xk+vk */名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 5 页，共 10 页 - - - - - - - - - cvMatMulAdd( cvkalman-measurement_matrix, cvkalman-state_pos

13、t, measurement, measurement ); cvKalmanCorrect( 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 = predic

14、tion-data.fl2; return(cvPoint2D32f(predict_value_x,predict_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; cvkalm

15、an-state_post-data.fl3=yv; 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 6 页，共 10 页 - - - - - - - - - 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

16、*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+) for (j=0; j=n-1; j+) ii=i*n+j; pii=qii; for (kk=0; kk=n-1; kk+) pii=pii+fi*n

17、+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; 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 7 页，共 10 页 - - - - - - - - - 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

18、*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)*n+i; xjj=0.0; for (j=0; j=n-1; j

19、+) 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; 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 8 页，共 10 页 - - - - - - - - - for (j=0; j=m-1; j+) xjj=xjj+

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

21、; 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; 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 9 页，共 10 页 - - - - - - - - - free(e); free(a); free(b); return(js); 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 10 页，共 10 页 - - - - - - - - -