clc
clear
num=[0 1 -0.3 -0.06 0.008];
den=[1 -0.5 -0.49 0.125 0.06];%差分方程
a=[0.5 0.49 -0.25 -0.48;1 0 0 0;0 0.5 0 0;0 0 0.25 0];
b=[1 0 0 0]';
c=[1 -0.3 -0.12 0.064];
d=0
ito=obsv(a,c);
to=inv(ito);
ap=ito*a*to;
bp=ito*b;
cp=c*to;
dp=0;
%[ap,bp,cp,dp]=augstate(a,b,c,d);

t=1:1000;
%xx=1+t*0;%%%%%%輸入信號
xx=sin(t/25);%%%%%%輸入信號
[xorg,yorg]=dlsim(a,b,c,d,xx,[0 0 0 0]);%
% DLSIM  Simulation of discrete-time linear systems.
 %   DLSIM(A,B,C,D,U)  plots the time response of the discrete system:
 %        x[n+1] = Ax[n] + Bu[n]
  %      y[n]   = Cx[n] + Du[n]
 %
  %  to input sequence U.
dp=[0 0 0 1];

u=zeros(length(t),4);
u(:,1)=0.1*rand(length(t),1);
u(:,4)=1.4142*rand(length(t),1);
m=mean(u(:,1));
u(:,1)=u(:,1)-m+1;
m=mean(u(:,2));
u(:,2)=u(:,2)-m;

%[G,H]=c2d(ap,bp,h)
G=ap;
H=[bp [0 0 0 0]' [0 0 0 0]' [0 0 0 0]'];
K=[0 0 0 0]';
xold=[0 0 0 0]';
pold=diag([10000 10000 10000 10000]);
xhatold=[0 0 0 0]';
Q=0.01;
R=2;
B=bp;
gamma=[0 0 0 1]';
%[pp,gamma]=c2d(ap,gamma,h);
y=zeros(length(t),9);
for jj=1:length(t)
    yplant=yorg(jj,:);
    uplant=u(jj,:);
    xnew=G*xold+H*yplant'+H*uplant';
    y(jj,1)=(cp*xnew+dp*uplant')';
   y(jj,2:5)=xnew';
    pstar=G*pold*G'+gamma*Q*gamma';
    K=pstar*cp'*inv(cp*pstar*cp'+R);
    xhatnew=(eye(4)-K*cp)*xnew+K*y(jj,1);
    pnew=(eye(4)-K*cp)*pstar;
    y(jj,6:9)=xhatnew;
    xold=xnew;
    xhatold=xhatnew;
    pold=pnew;
end;
%%%%%紅色為原始狀態輸出；綠色為量測方程輸出；黑色為狀態方程輸出；藍色為卡爾曼濾波輸出
figure(1)
plot(t,yorg(:,1),'r',t, y(:,1),'g',t,y(:,2),'k',t,y(:,6),'b');
figure(2)
plot(t,yorg(:,2),'r',t,y(:,3),'k',t,y(:,7),'b');
figure(3)
plot(t,yorg(:,3),'r',t,y(:,4),'k',t,y(:,8),'b');
figure(4)
plot(t,yorg(:,4),'r',t,y(:,5),'k',t,y(:,9),'b');