卡尔曼滤波程序.doc

最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。现设线性时变系统的离散状态防城和观测方程为：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时刻观测噪声则卡尔曼滤波的算法流程为：1. 预估计X(k)= F(k,k-1)X(k-1)2. 计算预估计协方差矩阵C(k)=F(k,k-1)C(k)F(k,k-1)+T(k,k-1)Q(k)T(k,k-1)Q(k) = U(k)U(k)3. 计算卡尔曼增益矩阵K(k) = C(k)H(k)H(k)C(k)H(k)+R(k)(-1)R(k) = N(k)N(k)4. 更新估计X(k)=X(k)+K(k)Y(k)-H(k)X(k)5. 计算更新后估计协防差矩阵C(k) = I-K(k)H(k)C(k)I-K(k)H(k)+K(k)R(k)K(k)6. X(k+1) = X(k)C(k+1) = C(k)重复以上步骤 其c语言实现代码如下：#includestdlib.h#includerinv.cintlman(n,m,k,f,q,r,h,y,x,p,g)intn,m,k;doublef,q,r,h,y,x,p,g;inti,j,kk,ii,l,jj,js;double*e,*a,*b;e=malloc(m*m*sizeof(double);l=m;if(ln)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=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;for(kk=0;kk=n-1;kk+)ajj=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+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;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(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-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=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*fj*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;kk1000#pragmaonce#endif/_MSC_VER1000#include#includecv.hclasskalmanpublic:voidinit_kalman(intx,intxv,inty,intyv);CvKalman*cvkalman;CvMat*state;CvMat*process_noise;CvMat*measurement;constCvMat*prediction;CvPoint2D32fget_predict(floatx,floaty);kalman(intx=0,intxv=0,inty=0,intyv=0);/virtualkalman();#endif/!defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_)=kalman.cpp=#includekalman.h#include/*testerdeprintertouteslesvaleursdesvecteurs*/*testerdechangerlesmatricesdunoises*/*replacestatebycvkalman-state_post?*/CvRandStaterng;constdoubleT=0.1;kalman:kalman(intx,intxv,inty,intyv)cvkalman=cvCreateKalman(4,4,0);state=cvCreateMat(4,1,CV_32FC1);process_noise=cvCreateMat(4,1,CV_32FC1);measurement=cvCreateMat(4,1,CV_32FC1);intcode=-1;/*creatematrixdata*/constfloatA=1,T,0,0,0,1,0,0,0,0,1,T,0,0,0,1;constfloatH=1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0;constfloatP=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);constfloatQ=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;constfloatR=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_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(cvkalman-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);/*chooseinitialstate*/state-data.fl0=x;state-data.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;cvRandSetRange(&rng,0,sqrt(cvkalman-process_noise_cov-data.fl0),0);cvRand(&rng,process_noise);CvPoint2D32fkalman:get_predict(floatx,floaty)/*updatestatewithcurrentposition*/state-data.fl0=x;state-data.fl2=y;/*predictpointposition*/*xk=A鈥k+B鈥kPk=A鈥k-1*AT+Q*/cvRandSetRange(&rng,0,sqrt(cvkalman-measurement_noise_cov-data.fl0),0);cvRand(&rng,measurement);/*xk=A?xk-1+B?uk+wk*/cvMatMulAdd(cvkalman-transition_matrix,state,process_noise,cvkalman-state_post);/*zk=H?xk+vk*/cvMatMulAdd(cvkalman-measurement_matrix,cvkalman-state_post,measurement,measurement);/*adjustKalmanfilterstate*/*Kk=Pk鈥T鈥?H鈥k鈥T+R)-1xk=xk+Kk鈥?zk-H鈥k)Pk=(I-Kk鈥)鈥k*/cvKalmanCorrect(cvkalman,measurement);floatmeasured_value_x=measurement-data.fl0;floatmeasured_value_y=measurement-data.fl2;constCvMat*prediction=cvKalmanPredict(cvkalman,0);floatpredict_value_x=prediction-data.fl0;floatpredict_value_y=prediction-data.fl2;return(cvPoint2D32f(predict_value_x,predict_value_y);voidkalman:init_kalman(intx,intxv,inty,intyv)state-data.fl0=x;state-data.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;其中专业理论知识内容包括：保安理论知识、消防业务知识、职业道德、法律常识、保安礼仪、救护知识。作技能训练内容包括：岗位操作指引、勤务技能、消防技能、军事技能。二培训的及要求培训目的安全生产目标责任书为了进一步落实安全生产责任制，做到“责、权、利”相结合，根据我公司2015年度安全生产目标的内容，现与财务部签订如下安全生产目标：一、目标值：1、全年人身死亡事故为零，重伤事故为零，轻伤人数为零。2、现金安全保管，不发生盗窃事故。3、每月足额提取安全生产费用，保障安全生产投入资金的到位。4、安全培训合格率为100%。二、本单位安全工作上必须做到以下内容： 1、对本单位的安全生产负直接领导责任，必须模范遵守公司的各项安全管理制度，不发布与公司安全管理制度相抵触的指令，严格履行本人的安全职责，确保安全责任制在本单位全面落实，并全力支持安全工作。 2、保证公司各项安全管理制度和管理办法在本单位内全面实施，并自觉接受公司安全部门的监督和管理。 3、在确保安全的前提下组织生产，始终把安全工作放在首位，当“安全与交货期、质量”发生矛盾时，坚持安全第一的原则。 4、参加生产碰头会时，首先汇报本单位的安全生产情况和安全问题落实情况；在安排本单位生产任务时，必须安排安全工作内容，并写入记录。 5、在公司及政府的安全检查中杜绝各类违章现象。 6、组织本部门积极参加安全检查，做到有检查、有整改，记录全。 7、以身作则，不违章指挥、不违章操作。对发现的各类违章现象负有查禁的责任，同时要予以查处。 8、虚心接受员工提出的问题，杜绝不接受或盲目指挥；9、发生事故，应立即报告主管领导，按照“四不放过”的原则召开事故分析会，提出整改措施和对责任者的处理意见，并填写事故登记表，严禁隐瞒不报或降低对责任者的处罚标准。 10、必须按规定对单位员工进行培训和新员工上岗教育；11、严格执行公司安全生产十六项禁令，保证本单位所有人员不违章作业。 三、 安全奖惩： 1、对于全年实现安全目标的按照公司生产现场管理规定和工作说明书进行考核奖励；对于未实现安全目标的按照公司规定进行处罚。 2、每月接受主管领导指派人员对安全生产责任状的落