




下載本文檔
版權(quán)說(shuō)明:本文檔由用戶提供并上傳,收益歸屬內(nèi)容提供方,若內(nèi)容存在侵權(quán),請(qǐng)進(jìn)行舉報(bào)或認(rèn)領(lǐng)
文檔簡(jiǎn)介
1、實(shí)用標(biāo)準(zhǔn)文檔卡爾曼濾波算法實(shí)現(xiàn)代碼C+攸現(xiàn)代碼如下:=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 <math.h&
2、gt;#include "cv.h"class kalmanpublic :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);kalman( int x=0, int xv=0, int y=0, int yv=0);/virtual kalman();
3、INCLUDED;#endif /!defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0.kalman.cpp#include"kalman.h"#include<stdio.h>/*testerdeprintertoutes lesvaleurs desvecteurs*/*testerdechangerles matrices du noises*/* replace state by cvkalman->state_post ? */CvRandState rng;const double
4、 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 = -1;/* create matrix data */const floatA口= 1,T,0,0,0,1,0,0,0,0,1,T,0,0,0,1;co
5、nst float H口 = 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0;constfloatP口= 0,pow(320,2),pow(320,2)/T,0,0,pow(320,2)/T,pow(320,2)/pow(T,2),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;const
6、 float 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->transition_matrix->data.fl,A,sizeof (A);memcpy( cvkalman->measurement_m
7、atrix->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,cvRealScalar(1e-5);/cvSetI
8、dentity( 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;state->data.fl3=yv;cvkalman->state_post->data.fl0=x;cvkalman->state_pos
9、t->data.fl1=xv;cvkalman->state_post->data.fl2=y;cvkalman->state_post->data.fl3=yv;0 );cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl0), cvRand( &rng, process_noise ); CvPoint2D32fkalman:get_predict( float x, float y) /* update state with current posit
10、ion */ state->data.fl0=x; state->data.fl2=y;/* predict point position */* x'k=A 敏 k+B 敏 kP'k=A 敏 k-1*AT + Q */cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl 0),0 );cvRand( &rng, measurement );/* xk=A?xk-1+B?uk+wk */cvkalmacvMatMulAdd( cvkalman->
11、transition_matrix,state, process_noise,measuren->state_post ); /* zk=H?xk+vk */ cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, ment, measurement );cvKalmanCorrect( cvkalman, measurement );floatmeasured_value_x= measurement->data.fl0;floatmeasured_value_y= measurement-&g
12、t;data.fl2;const CvMat* prediction = cvKalmanPredict( cvkalman, 0 );floatpredict_value_x= prediction->data.fl0;floatpredict_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)state->data.fl0=x;state->d
13、ata.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語(yǔ)言實(shí)現(xiàn)代碼如下#include "stdlib.h"麗 田 TI 田I1 T 田 瞋In-I LI_1 r 融I 雨 T 崩#include "
14、;rinv.c"intlman(n,m,k,f,q,r,h,y,x,p,g)intn,m,k;doublef,q口川h,y口刈,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 );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&
15、lt;=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;題 T L 述II 卜I 帕 1 T 卜 I 酶 I 丁 上 丁 郎1for (kk=0;kk<=n-1;kk+)ajj=aj
16、j+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=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
17、;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*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;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
溫馨提示
- 1. 本站所有資源如無(wú)特殊說(shuō)明,都需要本地電腦安裝OFFICE2007和PDF閱讀器。圖紙軟件為CAD,CAXA,PROE,UG,SolidWorks等.壓縮文件請(qǐng)下載最新的WinRAR軟件解壓。
- 2. 本站的文檔不包含任何第三方提供的附件圖紙等,如果需要附件,請(qǐng)聯(lián)系上傳者。文件的所有權(quán)益歸上傳用戶所有。
- 3. 本站RAR壓縮包中若帶圖紙,網(wǎng)頁(yè)內(nèi)容里面會(huì)有圖紙預(yù)覽,若沒有圖紙預(yù)覽就沒有圖紙。
- 4. 未經(jīng)權(quán)益所有人同意不得將文件中的內(nèi)容挪作商業(yè)或盈利用途。
- 5. 人人文庫(kù)網(wǎng)僅提供信息存儲(chǔ)空間,僅對(duì)用戶上傳內(nèi)容的表現(xiàn)方式做保護(hù)處理,對(duì)用戶上傳分享的文檔內(nèi)容本身不做任何修改或編輯,并不能對(duì)任何下載內(nèi)容負(fù)責(zé)。
- 6. 下載文件中如有侵權(quán)或不適當(dāng)內(nèi)容,請(qǐng)與我們聯(lián)系,我們立即糾正。
- 7. 本站不保證下載資源的準(zhǔn)確性、安全性和完整性, 同時(shí)也不承擔(dān)用戶因使用這些下載資源對(duì)自己和他人造成任何形式的傷害或損失。
最新文檔
- 餐廳餐包合同協(xié)議書
- 解除房屋賣買協(xié)議書
- 飯館雇主擔(dān)保協(xié)議書
- 項(xiàng)目理財(cái)服務(wù)協(xié)議書
- 銀行公安合作協(xié)議書
- 酒吧商家活動(dòng)協(xié)議書
- 院子地磚購(gòu)買協(xié)議書
- 食品產(chǎn)品運(yùn)輸協(xié)議書
- 足協(xié)內(nèi)部退贓協(xié)議書
- 車輛置換使用協(xié)議書
- 海外不動(dòng)產(chǎn)買賣中介合同范本
- DB44-T 2605-2025 生活垃圾焚燒發(fā)電設(shè)施能源消耗計(jì)算與限額
- 2025江蘇中考:化學(xué)必背知識(shí)點(diǎn)
- 2024-2025學(xué)年度廣東省廣州市南沙區(qū)中考英語(yǔ)一模試卷(含解析)
- 漆房外協(xié)協(xié)議書
- 2025年能源行業(yè)能源需求預(yù)測(cè)與市場(chǎng)發(fā)展趨勢(shì)2025
- 2024年“藍(lán)橋杯”科學(xué)素養(yǎng)競(jìng)賽考試題庫(kù)(含答案)
- 康復(fù)醫(yī)療復(fù)習(xí)題及參考答案
- 高標(biāo)準(zhǔn)農(nóng)田項(xiàng)目規(guī)劃設(shè)計(jì)方案
- 高血壓科普基礎(chǔ)知識(shí)培訓(xùn)-2025世界高血壓日
- 2025春季學(xué)期國(guó)開電大??啤独砉び⒄Z(yǔ)1》一平臺(tái)在線形考(綜合測(cè)試)試題及答案
評(píng)論
0/150
提交評(píng)論