u=zeros(4,1);
y=zeros(3,1);
e=zeros(3,1);
kp=0.045;
ki=0.1;
kd=0.3;
k1=kp+ki+kd;
k2=-kp-2*kd;
k3=kd;
kpid=[k1,k2,k3];
a=[0.368,0,0];
b=[0,0,0,0.6321];
yk=0;
yt=0;
uk=0;
uk1=0;
t=0;
r=1;
ek=0;
for k=1:30
    ek=r-yk;     %提前一拍獲得誤差值
    e=[ek;e(1:2)];
    uk1=kpid*e;
    uk=uk+uk1;
    u=[uk;u(1:3)];
    yk=a*y+b*u;
    y=[yk;y(1:2)]; 
    yt=[yt,yk];
    t=[t,k*0.5];
end
figure
plot(t,yt)
grid on