clc  %清除命令區(qū)的歷史信息
clear all %清除工作區(qū)的所有變量
 
a=[0   1;
   0  -1];
b=[0 2]'; 
c=[1 0];  
d=0;%構(gòu)造連續(xù)系統(tǒng)矩陣

[ap,bp,cp,dp]=augstate(a,b,c,d);%把系統(tǒng)狀態(tài)添加到輸出向量中
h=0.05;%求取的時間間隔為0.05s
t=[0:h:10];%構(gòu)造時間向量
[xorg,yorg]=step(ap,bp,cp,dp,1,t);%求取沒有噪聲時的系統(tǒng)在0~10s的輸出

u=zeros(length(t),2);%這句話可要，可不要
%u一共有兩列
u(:,1)=0.1*rand(length(t),1);%產(chǎn)生0~0.1之間的均勻分布數(shù)據(jù)
u(:,2)=1.4142*rand(length(t),1);%產(chǎn)生0~1.4142之間的均勻分布數(shù)據(jù)
m=mean(u(:,1));%求取u1的均值
%這兒u的第一列包含的信息由兩部分，一個是本來的階躍輸入信號，式子中的1；
%兩外一部分是加到輸入上的隨機噪聲u1，即u(:,1)-m；
%能把噪聲直接加到輸入信號上，是因為例子中c矩陣跟H矩陣相同
u(:,1)=u(:,1)-m+1;
m=mean(u(:,2));
u(:,2)=u(:,2)-m;%產(chǎn)生噪聲數(shù)據(jù)u2

ap=a;
bp=[b [0 0]'];
cp=c;
dp=[d 1];%把噪聲u2也當成系統(tǒng)那個的一個輸入，構(gòu)造系統(tǒng)矩陣
[G,H]=c2d(ap,bp,h);%求取系統(tǒng)加入增廣噪聲輸入后的系統(tǒng)離散化矩陣

K=[0 0]';%K陣的初始值
xold=[0 0]';%系統(tǒng)的狀態(tài)初始值
pold=diag([10000 10000]);%系統(tǒng)狀態(tài)協(xié)方差的初始值
xhatold=[u(1,2),0 ]'%系統(tǒng)狀態(tài)輸入值
Q=0.01;%噪聲u1協(xié)方差
R=2;%噪聲u2協(xié)方差
gamma=[0 1]';
[pp,gamma]=c2d(a,gamma,h);%例子中g(shù)amma與H相同

y=zeros(length(t),5);%存儲系統(tǒng)輸出和狀態(tài)的數(shù)值

for jj=1:length(t)
    uplant=u(jj,:);
    xnew=G*xold+H*uplant';%加入噪聲后的k+1時刻的系統(tǒng)狀態(tài)值
    y(jj,1)=(cp*xnew+dp*uplant');%加入噪聲后的k+1時刻的系統(tǒng)輸出值
    y(jj,2:3)=xnew';%記錄系統(tǒng)狀態(tài)值
    
    %下面是（8.58）式
    pstar=G*pold*G'+gamma*Q*gamma';%預(yù)報的協(xié)方差
    K=pstar*cp'*inv(cp*pstar*cp'+R);
    xhatnew=(eye(2)-K*cp)*(G*xhatold+H*uplant')+K*y(jj,1);%估計的狀態(tài)輸出
    pnew=(eye(2)-K*cp*pstar);%估計的系統(tǒng)狀態(tài)協(xié)方差
    
    y(jj,4:5)=xhatnew;
    xold=xnew;
    xhatold=xhatnew;
    pold=pnew;
end

axes('pos',[0.1,0.56,0.35,.32]);
plot(t,[xorg y(:,:)]);
axes('pos',[0.52,0.56,0.35,.32]);
plot(t,[xorg(:,2) y(:,[1 2 4])]);
legend('真實x1','系統(tǒng)實際輸出','狀態(tài)x1的實際值','狀態(tài)x1的估計值');
title('系統(tǒng)狀態(tài)x1的對比');
axes('pos',[0.1,0.1,0.35,.32]);
plot(t,[xorg(:,3) y(:,[3 5])]);
legend('真實x2','狀態(tài)x2的實際值','狀態(tài)x2的估計值');
title('系統(tǒng)狀態(tài)x2的對比');
axes('pos',[0.52,0.1,0.35,.32]);plot(t,[xorg(:,[2 3])]);title('真實系統(tǒng)狀態(tài)');