clear
clc
h=1; %???????¤
N=60;
M=1;
P=10;
Tao=3;
for i=1:Tao/h
    Temp(i)=0;
end 
 
u(1)=1;
 
y(1)=0;
t(1)=1;
k=1;
for i=0:h:N
    k=k+1;
    t(k)=t(k-1)+h;
    u(k)=1;
    [y(k),Temp]=Gp(u(k-1),h,Temp);
end
figure
a=y(2:61);
plot(a)
 
for i=1:M
A(i:P,i)=a(1:P-i+1)';
end
for i=1:P
    k=0;
    for j=i:N-2
    A0(i,j)=a(N-k)-a(N-k-1);
    k=k+1;
    end
end
for i=1:P
    A0(i,N-1)=a(i+1);
end
for i=1:N-1
    U(i)=0;
end
for i=1:P
H(i)=1;
end
Q=eye(P);
% Q(1,1)=0;
% Q(2,2)=0;
% Q(3,3)=0;
R=eye(M)*0;
e(1)=0;
Y(1)=0;
k=1;
YY(1)=0;
for i=1:Tao/h
    Temp(i)=0;
end 
for i=1:50
 w(i)=1;%sin(i/10);
    for j=1:P
        %Yr(j)=alfa^j*Y(i)+(1-alfa^j)*w;
         Yr(j)=w(i);
    end 
du=inv(A'*Q*A+R)*A'*Q*(Yr'-A0*U'-H'*e(i));
Um(i)=U(N-1)+du(1);
[Y(i+1),Temp]=Gm(Um(i),h,Temp);
Yyc=A*du+A0*U';
Yyc1=A(1)*du+A0(1,:)*U';
e(i+1)=Y(i+1)-Yyc(1);
YY(i+1)=Yyc(1);
 for j=1:N-2
 U(j)=U(j+1);    
 end
 U(N-1)=Um(i);
 
end
figure
subplot(2,1,1)
plot(Um)
title('輸入')
subplot(2,1,2)
plot(Y)
title('輸出')