《卡尔曼滤波算法》由会员分享，可在线阅读，更多相关《卡尔曼滤波算法（9页珍藏版）》请在金锄头文库上搜索。
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_8BF57636F2C0INCLUDED
2、#if _MSC_VER 1000#pragma once#endif/ _MSC_VER 1000#include #include cv.hclass 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,in
3、t y=0,int yv=0);/virtual kalman();；#endif/ !defined(AFX_KALMAN_HED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_)= = = = = = = = = = = = = = = = = = = = = = = = = = = = kalman.cpp= = = = = = = = = = = = = = =#include kalman.h#include /* tester de printer toutes les valeurs des vecteurs */* tester de c
4、hanger les matrices du noises */* 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, 4, 0 );state = cvCreateMat( 4, 1, CV_32FC1 );process_noise = cvCreateMat( 4, 1, CV_32FC1 );measurement = cvCreateMat(
5、4, 1, CV_32FC1 );int code = -1;/* create matrix data */const float A = 1, T, 0, 0,0, 1, 0, 0,0, 0, 1, T,0, 0, 0, 1const float H = 1, 0, 0, 0,0, 0, 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,0, 0, pow(240,2), pow(240,2)/T,0, 0,
6、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 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,
7、0.1, 0 );rng.disttype = CV_RAND_NORMAL;cvRand( &rng, state );memcpy( cvkalman-transition_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( 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);/cvSetIdentity( cvkalman-measurement_noise_cov, cvRealScalar(1e-1);/* choose initial state */state-data.fl0=x;state-data.fl1=xv;stat
9、e-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;cvRandSetRange( &rng, 0, sqrt(cvkalman-process_noise_cov-data.fl0), 0 );cvRand( &rng, process_noise );CvPoint2D32f kalman:get_predict(float x,
10、float y)/* update state with current position */state-data.fl0=x;state-data.fl2=y;/* predict point position */* xk=A 跋 k+B 跋 kPk=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 */cvMatMulAdd( cvkalman-tr
11、ansition_matrix, state, process_noise, cvkalman- state_post );/* zk=H?xk+vk */cvMatMulAdd( cvkalman-measurement_matrix, cvkalman-state_post, meas urement, measurementcvKalmanCorrect( cvkalman, measurement );float measured_value_x = measurement-data.fl0;float measured_value_y = measurement-data.fl2;c
12、onst CvMat* prediction = cvKalmanPredict( cvkalman, 0 );float 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)state-data.fl0=x;state-data.fl1=xv;state-data.fl2=y;s
13、tate-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语言实现代码如下:#include stdlib.h#include rinv.cint 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;日田 inti,j,kk,ii,l,jj,js;I double *e,*a,*b;e=malloc(
14、m*m*sizeof(double);l=m;I if (ln) l=n;a=malloc(l*l*sizeof(double);b=malloc(l*l*sizeof(double);I for (i=0; i=n-l; i+)I for (j=0; j=n-l; j+)ii=i*l+j; aii=0.0;I for (kk=0; kk=n-1; kk+)aii=a ii +pi*n+kk *f j *n+kk;卜I for (i=0; i=n-l; i+)I for (j=0; j=n-l; j+) ii=i*n+j; pii=qii；I for (kk=0; kk=n-l; kk+)pii=pii+fi*n+kk*akk*l+j;卜I for (ii=2; ii=k; ii+) for (i=0; i=n-1; i+)I for (j=0; j=m-l; j+)jj=i*l+j; ajj=O.O;for (kk=0; kk=n-l; kk+)ajj=ajj+pi*n+kk
