卡尔曼滤波MATLAB代码(共2页).docx

《卡尔曼滤波MATLAB代码(共2页).docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波MATLAB代码(共2页).docx（2页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、精选优质文档-倾情为你奉上 xt=Ftxt-1+Gtvt vtN0,Qt yt=Htxt+wt wtN0,Rt 代码：初始设定%系数矩阵设定F=1,0.1;-1,0.9;G=0,0.1;H=1,0; %系统噪音和观测噪音的设定Q=1;R=1;%数据个数N=300;%噪音生成v=randn(N,1)*sqrtm(Q);w=randn(N,1)*sqrtm(R);%真值和观测数据的生成x=zeros(N,2);y=zeros(N,1);y(1)=H*x(1,:)+w(1);for k=2:N x(k,:)=F*x(k-1,:)+G*v(k-1); y(k)=H*x(k,:)+w(k);end%推测

2、值的保存内存生成xest=zeros(N,2);%初始推测值设定V=1,0;0,1;xest(1,:)=0,0;卡尔曼滤波主函数function xest_a,V_a,K = KF( F,G,H,Q,R,y,xest,V )% KF( F,G,H,Q,R,y,xest,V ) % x(t)=Fx(t-1)+Gv(t)% y(t)=Hx(t)+w(t)% y: 观测数据% xest: 状态的初始推测值% xest_a: 状态的推定値%行向量变列向量xest=xest(:);%预测阶段xest_f=F*xest; %状态的预测值V_f=F*V*F+G*Q*G; K=V_f*H/(H*V_f*H+R);%推测阶段xest_a=xest_f+K*(y-H*xest_f); %状态的推测值V_a=V_f-K*H*V_f; end时刻2开始调用主函数for k=2:Nxest(k,:),V,K=KF(F,G,H,Q,R,y(k),xest(k-1,:),V);end输出图plotfigure(1),clfplot(1:N,x(:,2),black,1:N,xest(:,2),b-)xlabel(时刻t),ylabel(状态向量x的第二行),legend(真值,状态的推测值)有疑问群里找我By 刹那恍惚专心-专注-专业