卡爾曼濾波算法(C--C++兩種實(shí)現(xià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