卡尔曼滤波程序

1、最佳线性滤波理论起源于 40 年代美国科学家 Wiener 和前苏联科学家K。畀Mor等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波 的最大缺点是必须用到无限过去的数据， 不适用于实时处理。为了克服这一缺点， 60 年代Kalman 把状态空间模型引入滤波理论， 并导出了一套递推估计算法， 后 人称之为卡尔曼滤波理论。 卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法， 其基本思想是： 采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计， 求出现时刻的 估计值。它适合于实时处理和计算机运算。现设线性时变系统的离散状态防城和

2、观测方程为：X(k) = F(k,k-1) X(k-1)+T(k,k-1) U(k-1)Y(k) = H(k) X(k)+N(k)其中X(k)和 Y(k)分别是 k 时刻的状态矢量和观测矢量F(k, k- 1 )为状态转移矩阵U(k)为 k 时刻动态噪声T( k,k-1)为系统控制矩阵H(k)为 k 时刻观测矩阵N(k)为 k 时刻观测噪声则卡尔曼滤波的算法流程为：预估计 X(k)A= F(k,k-1) X(k-1)1.12.计算预估计协方差矩阵C(kF=F(k,k-1)xC(k)xF(k,k-1)+T(k,k-1)xQ(k)xT(k,k-1)Q(k) = U(k)xU(k)3.计算卡尔曼增益

3、矩阵K(k) = C(k)AxH(k)xH(k)xC(k)AxH(k)+R(k)A(-1)R(k) = N(k)xN(k)4.更新估计X(k)=X(kF+K(k)xY(k)-H(k)xX(k)A5.计算更新后估计协防差矩阵C(k) = I-K(k)xH(k)xC(k)AxI-K(k)xH(k)+K(k)xR(k)xK(k)6.X(k+1) = X(k)C(k+1) = C(k)重复以上步骤 其 c 语言实现代码如下:#i nclude stdlib.h#in elude rin v.cint lman(n ,m,k,f,q,r,h,y,x,p,g) int n,m,k;double f,q,r

4、,h,y,x,p,g;LJU int i,j,kk,ii,l,jj,js;I double *e,*a,*b;e=malloc(m*m* sizeof(double); l=m;if (ln)匸 n;a=malloc(l*l* sizeof(double); b=malloc(l*l* sizeof(double);for (i=0; i=n-1; i+)for (j=0; jv=n-1; j+) ii=i*l+j; aii=0.0;for (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

5、v=n-1; j+)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+)ITT軸Tajj=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;for (kk=0; kk=n-1; kk+) ejj=ejj+hi* n+

6、kk*akk*l+j;js=rin v(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*e j*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

7、=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+g i *m+j*bl;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-g i *m+kk*hkk*n+j;if (i=j) ajj=1.0+ajj;for (i=0; i=n-1

8、; i+)for (j=0; j=n-1; j+) jj=i*l+j; bjj=0.0;for (kk=0; kk=n-1; kk+)艸一I I F F 一殛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*f 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; kk 1000#pragma once#e

9、n dif / _MSC_VER 1000#in clude #i nclude cv.h class kalmanpublic :void init_kalman(int x, int xv, int y, int yv);CvKalma n* cvkalma n;CvMat* state;CvMat* process, no ise;T-CvMat* measureme nt;const CvMat* prediction;CvPoint2D32f get_predict(float x, float y);kalman( int x=0, int xv=0, int y=0, int y

10、v=0);/virtual kalma n();#en dif / !defi ned(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2CO_INCLUDED_)=kalma n. cpp=#i nclude kalma n.h#i nclude /* tester de prin ter toutes les valeurs des vecteurs*/* tester de cha nger les matrices du no ises */* replace state by cvkalma n-state_post ? */CvRa nd

11、State 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 st float A = 1,

12、 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 ,3)/3, pow (T,2

13、)/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 );cvRandSetRange( &rng, 0, 0.1, 0 ); rng.disttype = CV_RAND_NORMAL;cvRa nd( &rng

14、, state );memcpy( cvkalman-transition_matrix-data.fl. A,memcpy( cvkalman-measurement_matrix-data.fl, H,memcpy( cvkalma n-process_ no ise_cov-data.fl, Q,memcpy( cvkalman-error_cov_post-data.fl, P,memcpy( cvkalman-measurement_noise_cov-data.fl, R,sizeof (A);sizeof (H);sizeof (Q);sizeof (P);sizeof (R);

15、/cvSetldentity( cvkalman-process_noise_cov, cvRealScalar(1e-5);/cvSetldentity( cvkalman-error_cov_post, cvRealScalar(1);/cvSetIde ntity( cvkalma n- measureme nt_no ise_cov, cvRealScalar(1e-1);/* choose in itial state */state-data.flO=x;state-data.fl1=xv;state-data.fl2=y;state-data.fl3=yv;cvkalma n-s

16、tate_post-data.flO=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_cov-data.flO),0 );cvRa nd( &rng, process_ no ise );CvPo in t2D32f kalman:get_predict(float x, float y)/* update sta

17、te with curre nt positi on */state-data.flO=x;state-data.fl2=y;/* predict point positi on */* xk=A 欽 k+B 欽 kPk=A 欽 k-1*AT + Q */cvRandSetRange( &rng, 0, sqrt(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

18、_ matrix, state, process_ no ise, cvkalman-、目标值:state_post );/* zk=H?xk+vk */cvMatMulAdd( cvkalma n-measureme nt_matrix, cvkalma n-state_post, measureme nt, measureme nt );/* adjust Kalma n filter state */* Kk=Pk 欽 T 欽？H 欽k 欽 T+R)-1xk=xk+Kk 欽?zk-H 欽 k)Pk=(l-Kk欽)欽k */cvKalma nCorrect( cvkalma n, meas

19、ureme 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_value_y = prediction-data.fl2;return (cvPoint2D32f(predict_value_x,predict_value_y

20、);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.fl1=xv;cvkalma n-state_post-data.fl2=y;cvkalma n-state_post-data.fl3=yv;其中专业理论知识内容包括：保安理论知识、消防业务知识职业道德、法律常识、保安礼仪、救护知识。作技能训练内容包括：岗位操作指引、勤务技能、消防技能、军事技能。二培训的及要求培训目的安全生产目标责任书为了进一步落实安全生产责任制，做到“责、权、利”相结合，根据我公司2015 年度安全生产目标的内容，现与财务部 签订如下安全生产目标:2、每月接受主管领导指派人员对安全生产责任状的落1、全年人身死亡事故为零，重伤事故