卡爾曼濾波算法

1、卡爾曼濾波算法實(shí)現(xiàn)代碼C+實(shí)現(xiàn)代碼如下：=kalma n.h=/ kalman.h: interface for the kalman class./ /#if !defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED#defi ne AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_#if _MSC_VER > 1000#pragma once#en dif / _MSC_VER > 1000#in clude <math.h&g

2、t;#i nclude "cv.h"class kalma npublic :void init_kalman(int x, int xv, int y, int yv);CvKalma n* cvkalma n;CvMat* state;CvMat* process_ no ise;CvMat* measureme nt;const CvMat* prediction;CvPoint2D32f get_predict(float x, float y);kalman( int x=0, int xv=O, int y=0, int yv=O);/virtual kalma

3、 n();；#en dif / !defi ned(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2CO_INCLUDED_)=kalma n. cpp=#i nclude "kalma n.h"#i nclude <stdio.h>/* tester de prin ter toutes les valeurs des vecteurs*/* tester de cha nger les matrices du no ises */* replace state by cvkalma n->state_pos

4、t ? */CvRa ndState rng;const double T = 0.1;kalman:kalman( int x, int xv, int y, int yv)cvkalma n = cvCreateKalma n( 4, 4, 0 );state = cvCreateMat( 4, 1, CV_32FC1 );process_ noise = cvCreateMat( 4, 1, CV_32FC1 );measureme nt = cvCreateMat( 4, 1, CV_32FC1 );int code = -1;/* create matrix data */con s

5、t float A = 1, T , 0, 0,0,1, 0, 0,0, 0,1, T ,0, 0, 0, 1；con st float H = 1, 0, 0, 0,0, 0, 0, 0,0, 0, 1, 0,0, 0, 0, 0；con st float P = 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);const float Q = pow (T

6、 ,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;con st float R = 1, 0, 0, 0,0, 0, 0, 0,0, 0,1,0,0, 0, 0, 0；cvRa ndl nit( &rng, 0, 1, -1, CV_RAND_UNI );cvZero( measureme nt );cvRa ndSetRa nge( &rng, 0, 0.1, 0 );rng.disttype = CV_RAND_NORMAL;c

7、vRa nd( &rng, state );memcpy( cvkalman->transition_matrix->data.fl, A,sizeof (A);memcpy( cvkalman->measurement_matrix->data.fl, H,sizeof (H);memcpy( cvkalma n->process_ no ise_cov->data.fl, Q,sizeof (Q);memcpy( cvkalman->error_cov_post->data.fl, P,sizeof (P);memcpy( cvkal

8、man->measurement_noise_cov->data.fl, R,sizeof (R);/cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5);/cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1);/cvSetIde ntity( cvkalma n-> measureme nt_no ise_cov, cvRealScalar(1e-1);/* choose in itial state */state->dat

9、a.fl0=x;state->data.fl1=xv;state->data.fl2=y;state->data.fl3=yv;cvkalma n->state_post->data.fl0=x;cvkalma n->state_post->data.fl1=xv;cvkalma n->state_post->data.fl2=y;cvkalma n->state_post->data.fl3=yv;cvRa ndSetRa nge( &rng, 0, sqrt(cvkalma n->process_ no ise

10、_cov->data.fl0), 0 );cvRa nd( &rng, process_ no ise );CvPoint2D32f kalman:get_predict(float x, float y)/* update state with curre nt positi on */state->data.fl0=x;state->data.fl2=y;/* predict point positi on */* x'k=A 欽 k+B 欽 kP'k=A 欽 k-1*AT + Q */cvRandSetRange( &rng, 0, sq

11、rt(cvkalman->measurement_noise_cov->data.fl0), 0 )；cvRa nd( &rng, measureme nt );/* xk=A?xk-1+B?uk+wk */cvMatMulAdd( cvkalma n->tra nsitio n_ matrix, state, process_ no ise, cvkalma n-> state_post );/* zk=H?xk+vk */cvMatMulAdd( cvkalma n->measureme nt_matrix, cvkalma n->state_p

12、ost, meas ureme nt, measureme nt );cvKalma nCorrect( cvkalma n, measureme nt );float measured_value_x = measurement->data.flO;float measured_value_y = measurement->data.fl2;const CvMat* prediction = cvKalmanPredict( cvkalman, 0 );float predict_value_x = prediction->data.fl0;float predict_va

13、lue_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->data.fl1=xv;state->data.fl2=y;state->data.fl3=yv;cvkalma n->state_post->data.fl0=x;cvkalma n->state_post->data.f

14、l1=xv;cvkalma n->state_post->data.fl2=y;cvkalma n->state_post->data.fl3=yv;c語(yǔ)言實(shí)現(xiàn)代碼如下#i nclude "stdlib.h"#i nclude "rin v.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;UU int i,j,kk,ii,l,jj,js;I double *e,*a,*b;I e=malloc(m*m* sizeof(double);I l=m;

15、I if (l< n) l=n;Ia=malloc(l*l* sizeof(double);Ib=malloc(l*l* sizeof(double);I for (i=0; i<=n-1; i+)I for (j=0; j<=n-1; j+) ii=i*l+j; aii=0.0;I for (kk=0; kk<=n-1; kk+)Iaii=aii+pi* n+kk*f n+kk;for (i=0; i<=n-1; i+)for (j=0; jv=n-1; j+)電由 ii=i*n+j; pii=qii;for (kk=0; kk<=n-1; kk+)pii

16、=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;1for (kk=0; kk<=n-1; kk+)1ajj=ajj+pi* n+kk*hj* n+kk;for (i=0; i<=m-1; i+)1for (j=0; j<=m-1; j+)誨LT jj=i*m+j； ejj=rjj;for (kk=0; kk<=n-1; kk+)ejj=ejj+hi* n+kk*akk*l+j;bT1js=r

17、 in v(e,m);if (js=0)T free(e); free(a); free(b); return (js); for (i=0; i<=n-1; i+)Lfor (j=0; j<=m-1; j+)軸 jj=i*m+j; gjj=0.0;IJfor (kk=0; kk<=m-1; kk+)T卜gjj=gjj+ai*l+kk*e j*m+kk;for (i=0; i<=n-1; i+) jj=(ii-1)*n+i; xjj=0.0;1Tfor (j=0; j<=n-1; j+) xjj=xjj+fi* n+j*x(ii-2)* n+j;卜T11,. 1f

18、or (i=0; i<=m-1; i+)軸TLLJ1 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;上Tfor (i=0; i<=n-1; i+)帥 jj=(ii-1)* n+i;for (j=0; j<=m-1; j+)xjj=xjj+gi*m+j*bl;if (ii<k) 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-g i *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