多元信息融合EKF作业

clear all;
close all;
clc

Q=0.01;
R=0.01;
X0=1;
P0=0.01;
N=100;
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);


%模型
for k=1:N
   if k==1
   X(1)=X0+1/X0+W(k);
   else
X(k)=(X(k-1)+1/X(k-1))+W(k);
   end
end
for k=1:N
   if k==1
   Y(1)=X0^2+V(k);
   else
Y(k)=X(k)*X(k)+V(k);
   end
end
%%%f(x)=x+1/x,h(x)=x*x,f'(x)=1-1/(x*x),h'(x)=2*x;

for k=1:N
    if k==1
        F(k)=1-1/X0*X0;   %%f(x)求导
        X_pre(k)=X0+1/X0;   %%带入f(x)
        H(k)=2*X_pre(k);
        P_pre(k)=F(k)*P0*F(k)'+Q;
        Y_pre(k)=X_pre(k)*X_pre(k);
        K(k)=P_pre(k)*H(k)'*inv(H(k)*P_pre(k)*H(k)'+R);
        X_est(k)=X_pre(k)+K(k)*(Y(k)-H(k)*X_pre(k));
       %% X_est(k)=X_pre(k)+K(k)*(Y(k)-X_pre(k)*X_pre(k));
        P_est(k)=P_pre(k)-K(k)*H(k)*P_pre(k);
    else
        F(k)=1-1/(X_est(k-1)*X_est(k-1)); %%f(x)求导
        %F(k)=X_est(k-1)+1/(X_est(k-1));
        X_pre(k)=X_est(k-1)+1/X_est(k-1); %%带入f(x)
        H(k)=2*X_pre(k);
        P_pre(k)=F(k)*P0*F(k)'+Q;
        Y_pre(k)=X_pre(k)*X_pre(k);
        K(k)=P_pre(k)*H(k)'*inv(H(k)*P_pre(k)*H(k)'+R);
%         X_est(k)=X_pre(k)+K(k)*(Y(k)-H(k)*X_pre(k));
        X_est(k)=X_pre(k)+K(k)*(Y(k)-X_pre(k)*X_pre(k));
        P_est(k)=P_pre(k)-K(k)*H(k)*P_pre(k);
    end
end

%绘图分析:
figure(1)
k=1:N;
plot(k,X(k),'-o',k,X_est(k),'-x');
xlabel('时间'),ylabel('状态值');
legend ('X的状态值','X的估计值');
title('EKF算法X的状态值和估计值曲线');
for k = 1:N
    e(k)=X(k)-X_est(k);
end

figure(2)
k=1:N;
plot(k,e(k),'-');
xlabel('时间');
ylabel('误差');
title('EKF算法X的状态值与估计值的误差');

figure(3)
plot(k,P_est(k),'-');
xlabel('时间'),ylabel('误差');
title('EKF算法估计误差协方差曲线');