版權說明：本文檔由用戶提供并上傳，收益歸屬內容提供方，若內容存在侵權，請進行舉報或認領
文檔簡介
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;
溫馨提示
- 1. 本站所有資源如無特殊說明，都需要本地電腦安裝OFFICE2007和PDF閱讀器。圖紙軟件為CAD,CAXA,PROE,UG,SolidWorks等.壓縮文件請下載最新的WinRAR軟件解壓。
- 2. 本站的文檔不包含任何第三方提供的附件圖紙等，如果需要附件，請聯系上傳者。文件的所有權益歸上傳用戶所有。
- 3. 本站RAR壓縮包中若帶圖紙，網頁內容里面會有圖紙預覽，若沒有圖紙預覽就沒有圖紙。
- 4. 未經權益所有人同意不得將文件中的內容挪作商業或盈利用途。
- 5. 人人文庫網僅提供信息存儲空間，僅對用戶上傳內容的表現方式做保護處理，對用戶上傳分享的文檔內容本身不做任何修改或編輯，并不能對任何下載內容負責。
- 6. 下載文件中如有侵權或不適當內容，請與我們聯系，我們立即糾正。
- 7. 本站不保證下載資源的準確性、安全性和完整性, 同時也不承擔用戶因使用這些下載資源對自己和他人造成任何形式的傷害或損失。
評論
0/150
提交評論