版權(quán)說明:本文檔由用戶提供并上傳,收益歸屬內(nèi)容提供方,若內(nèi)容存在侵權(quán),請進行舉報或認領(lǐng)
文檔簡介
1、卡爾曼濾波算法實現(xiàn)代碼其c語言實現(xiàn)代碼如下:#includestdlib.h#includerinv.cintlman(n,m,k,f,q,r,h,y,x,p,g)intn,m,k;doublef,q,r,h,y,x,p,g;inti,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;fo
2、r(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+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;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
3、;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+)xjj=xjj+fi*n+j*x(ii-2)*n+j;for(i=0;i=m-1;i+)jj=i
4、*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(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
5、.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;kk1000#pragmaonce#endif/_MSC_VER1000#include#includecv.hclasskalmanpublic:voidinit_kalman(intx,int
6、xv,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_8BF57636F2C0_INCLUDED_)=kalman.cpp=#includekalma
7、n.h#include/*testerdeprintertouteslesvaleursdesvecteurs*/*testerdechangerlesmatricesdunoises*/*replacestatebycvkalman-state_post?*/CvRandStaterng;constdoubleT=0.1;kalman:kalman(intx,intxv,inty,intyv)cvkalman=cvCreateKalman(4,4,0);state=cvCreateMat(4,1,CV_32FC1);process_noise=cvCreateMat(4,1,CV_32FC1
8、);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,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)
9、;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(&rng,0,0.1,0);rng.disttype=CV_RAND_NORMAL;cvRand(&rng,state);memcpy(cvkalman-transitio
10、n_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-process_noise_cov,cvRealScala
11、r(1e-5);/cvSetIdentity(cvkalman-error_cov_post,cvRealScalar(1);/cvSetIdentity(cvkalman-measurement_noise_cov,cvRealScalar(1e-1);/*chooseinitialstate*/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_po
12、st-data.fl2=y;cvkalman-state_post-data.fl3=yv;cvRandSetRange(&rng,0,sqrt(cvkalman-process_noise_cov-data.fl0),0);cvRand(&rng,process_noise);CvPoint2D32fkalman:get_predict(floatx,floaty)/*updatestatewithcurrentposition*/state-data.fl0=x;state-data.fl2=y;/*predictpointposition*/*xk=A鈥k+B鈥kPk=A鈥k-1*AT+
13、Q*/cvRandSetRange(&rng,0,sqrt(cvkalman-measurement_noise_cov-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);/
14、*adjustKalmanfilterstate*/*Kk=Pk鈥T鈥?H鈥k鈥T+R)-1xk=xk+Kk鈥?zk-H鈥k)Pk=(I-Kk鈥)鈥k*/cvKalmanCorrect(cvkalman,measurement);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:init
溫馨提示
- 1. 本站所有資源如無特殊說明,都需要本地電腦安裝OFFICE2007和PDF閱讀器。圖紙軟件為CAD,CAXA,PROE,UG,SolidWorks等.壓縮文件請下載最新的WinRAR軟件解壓。
- 2. 本站的文檔不包含任何第三方提供的附件圖紙等,如果需要附件,請聯(lián)系上傳者。文件的所有權(quán)益歸上傳用戶所有。
- 3. 本站RAR壓縮包中若帶圖紙,網(wǎng)頁內(nèi)容里面會有圖紙預(yù)覽,若沒有圖紙預(yù)覽就沒有圖紙。
- 4. 未經(jīng)權(quán)益所有人同意不得將文件中的內(nèi)容挪作商業(yè)或盈利用途。
- 5. 人人文庫網(wǎng)僅提供信息存儲空間,僅對用戶上傳內(nèi)容的表現(xiàn)方式做保護處理,對用戶上傳分享的文檔內(nèi)容本身不做任何修改或編輯,并不能對任何下載內(nèi)容負責(zé)。
- 6. 下載文件中如有侵權(quán)或不適當內(nèi)容,請與我們聯(lián)系,我們立即糾正。
- 7. 本站不保證下載資源的準確性、安全性和完整性, 同時也不承擔(dān)用戶因使用這些下載資源對自己和他人造成任何形式的傷害或損失。
最新文檔
- 小學(xué)一年級英語完形填空測試
- 青年人的社會參與與責(zé)任感
- 地理坐標地圖分析模擬試卷
- 小學(xué)生物激發(fā)測試
- 初中實驗?zāi)M試卷
- 小學(xué)英語詞匯應(yīng)用模擬試卷
- 如何正確安排中學(xué)生的學(xué)習(xí)時間
- 手表的機械結(jié)構(gòu)和時間的表示
- 江門市2025屆普通高中高三10月調(diào)研測試 歷史試卷(含答案詳解)
- 超拽yy娛樂K歌頻道設(shè)計模板 撥動了我的心弦
- 電子技術(shù)基礎(chǔ)與技能授課計劃(共5頁)
- 常見食物GI(升糖指數(shù))列表
- IATF16949中文Word版
- 四年級上冊勞動教案(福建教育出版社)2022秋.doc
- 配電設(shè)備差異化運維策略及法電運維經(jīng)驗介紹(深圳局)
- HPDL360G7服務(wù)器安裝說明
- 專業(yè)技術(shù)人員考核辦法及細則
- 公路養(yǎng)護工程設(shè)計規(guī)范(2020)
- EXcel制作的日歷版自動排班表(完美版終版密碼4個a)(精編版)
- 2022高端新款個人簡歷模板(可編輯)14 (2)
評論
0/150
提交評論