卡尔曼滤波MATLAB代码

𝒙𝑡=𝑭𝑡𝒙𝑡−1+𝑮𝑡𝒗𝑡𝒗𝑡~𝑁(𝟎,𝑸𝑡)𝒚𝑡=𝑯𝑡𝒙𝑡+𝒘𝑡𝒘𝑡~𝑁(𝟎,𝑹𝑡)代码：初始设定%系数矩阵设定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);fork=2:Nx(k,:)=F*x(k-1,:)'+G'*v(k-1);y(k)=H*x(k,:)'+w(k);end%推测值的保存内存生成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开始调用主函数fork=2:N[xest(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刹那恍惚