kalmanstatic.m

简单的卡尔曼滤波器仿真代码

%%%%函数1静态模型
function[X_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1)
   T=1;
   alpha=0.8;            %  加权衰减因子
   Phi=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
   H=[1,0,0,0;0,0,1,0];
   I=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];
   R=[10000,0;0,10000];  %  观测噪声方差阵
   X_estimate(k-1,:)=X_est;
    u(k-1)=u1;
    X_predict(k,:)=(Phi*X_estimate(k-1,:)')';
    P_predict=Phi*P_estimate*(Phi)';
    K=P_predict*(H)'*inv(H*P_predict*(H)'+R);
    X_estimate(k,:)=(X_predict(k,:)'+K*(z1-H*X_predict(k,:)'))';
    P_estimate=(I-K*H)*P_predict;
    X_est=X_estimate(k,:);
    X_pre=X_predict(k,:);

    v(:,k)=z1-H*(X_predict(k,:))';     %  新信息
    S=H*P_predict*H'+R;                %  新信息的方差阵
    delta(k)=v(:,k)'*inv(S)*v(:,k); 
    u(k)=alpha*u(k-1)+delta(k);
    u1=u(k);