clear;
x0=0;
p0=0;
fai=0.98;
Q=0.01;
gama=1;
H=0.98;
R=0.01;
n=100;
%產(chǎn)生狀態(tài)和測(cè)量
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('時(shí)刻k')
% ylabel('幅值')
% title('狀態(tài)值、測(cè)量值和估計(jì)值曲線(xiàn)圖')
% legend('狀態(tài)值','測(cè)量值','估計(jì)值')%%圖例
% hold off
%繪制曲線(xiàn)(狀態(tài)值和測(cè)量值)
figure
hold on
plot(x,'r')
plot(z,'b')
xlabel('時(shí)刻k')
ylabel('狀態(tài)值和測(cè)量值')
title('狀態(tài)值和測(cè)量值曲線(xiàn)圖')
legend('狀態(tài)值','測(cè)量值')%%圖例
hold off
%繪制曲線(xiàn)(狀態(tài)值和估計(jì)值)
figure
hold on
plot(x,'r')
plot(x_est,'b')
xlabel('時(shí)刻k')
ylabel('狀態(tài)值和估計(jì)值')
title('狀態(tài)值和估計(jì)值曲線(xiàn)圖')
legend('狀態(tài)值','估計(jì)值')
hold off
%繪制曲線(xiàn)(估計(jì)誤差)
figure
plot(x-x_est,'r')
xlabel('時(shí)刻k')
ylabel('估計(jì)誤差')
title('估計(jì)誤差曲線(xiàn)圖')
figure
%繪制曲線(xiàn)(估計(jì)誤差協(xié)方差)
plot(p_est,'r')
xlabel('時(shí)刻k')
ylabel('估計(jì)誤差協(xié)方差')
title('估計(jì)誤差協(xié)方差曲線(xiàn)圖')