clear;
x0=0;
p0=0;
fai=0.98;
Q=0.01;
gama=1;
H=0.98;
R=0.01;
n=100;
%產生狀態和測量
for k=1:n;    
    w(k)=sqrt(Q)*randn(1);     %#ok<AGROW>
    v(k)=sqrt(R)*randn(1);     %#ok<AGROW>
    if k==1;       
     
        
        x(k)=fai*x0+gama*w(k);        
        z(k)=H*x(k)+v(k);    
        
        x_pre(k)=fai*x0;        
        p_pre(k)=fai*p0*fai'+gama*Q*gama';        
        z_pre(k)=H*x_pre(k);        
        K(k)=p_pre(k)*H'*inv(H*p_pre(k)*H'+R);        
        x_est(k)=x_pre(k)+K(k)*(z(k)-z_pre(k));        
        p_est(k)=(1-K(k)*H)*p_pre(k);  
    else x(k)=fai*x(k-1)+gama*w(k);        
        z(k)=H*x(k)+v(k);    
        
        x_pre(k)=fai*x_est(k-1);        
        p_pre(k)=fai*p_est(k-1)*fai'+gama*Q*gama';        
        z_pre(k)=H*x_pre(k);        
        K(k)=p_pre(k)*H'*inv(H*p_pre(k)*H'+R);        
        x_est(k)=x_pre(k)+K(k)*(z(k)-z_pre(k));        
        p_est(k)=(1-K(k)*H)*p_pre(k); 
    end
end 
%kalman濾波算法
% for k=1:n;    
%     if k==1;        
%         x_pre(k)=fai*x0;        
%         p_pre(k)=fai*p0*fai'+gama*Q*gama';        
%         z_pre(k)=H*x_pre(k);        
%         K(k)=p_pre(k)*H'*inv(H*p_pre(k)*H'+R);        
%         x_est(k)=x_pre(k)+K(k)*(z(k)-z_pre(k));        
%         p_est(k)=(1-K(k)*H)*p_pre(k);    
%     else        
%         x_pre(k)=fai*x_est(k-1);        
%         p_pre(k)=fai*p_est(k-1)*fai'+gama*Q*gama';        
%         z_pre(k)=H*x_pre(k);        
%         K(k)=p_pre(k)*H'*inv(H*p_pre(k)*H'+R);        
%         x_est(k)=x_pre(k)+K(k)*(z(k)-z_pre(k));        
%         p_est(k)=(1-K(k)*H)*p_pre(k);    
%     end
% end 
% figure
% hold on
% plot(x,'r')
% plot(z,'b:')
% plot(x_est,'k-.')
% xlabel('時刻k')
% ylabel('幅值')
% title('狀態值、測量值和估計值曲線圖')
% legend('狀態值','測量值','估計值')%%圖例
% hold off
%繪制曲線(狀態值和測量值)
figure
hold on
plot(x,'r')
plot(z,'b')
xlabel('時刻k')
ylabel('狀態值和測量值')
title('狀態值和測量值曲線圖')
legend('狀態值','測量值')%%圖例
hold off
%繪制曲線(狀態值和估計值)
figure
hold on
plot(x,'r')
plot(x_est,'b')
xlabel('時刻k')
ylabel('狀態值和估計值')
title('狀態值和估計值曲線圖')
legend('狀態值','估計值')
hold off
%繪制曲線(估計誤差)
figure
plot(x-x_est,'r')
xlabel('時刻k')
ylabel('估計誤差')
title('估計誤差曲線圖')
figure
%繪制曲線(估計誤差協方差)
plot(p_est,'r')
xlabel('時刻k')
ylabel('估計誤差協方差')
title('估計誤差協方差曲線圖')