广义预测控制程序

clearn=4;m=3;t0=0.5;a=0.4;p=0.98;A=[1;-1.5;0.7];B=[1;1.5];A1=[2.5,-2.2,0.7];B1=[1.5];T=50;na=length(A)-1;nb=length(B)-1;A2=zeros(na+1,1);A2(1,1)=1;B2=zeros(nb+1,1);Q0=zeros(na+nb+1,1);forl=1:1:na+nb+1if(lna+1)Q0(l,1)=A(l+1,1);endif(lna)Q0(l,1)=B(l-na,1);endendQ1=zeros(na+nb+1,1);Q2=zeros(na+nb+1,1);P1=(1e+5)*eye(na+nb+1);P2=(1e+5)*eye(na+nb+1);uu=zeros(n,1);yy=zeros(n,1);y1=zeros(na+1,1);u1=zeros(nb,1);G=zeros(n,m);u=zeros(m,1);t=zeros(T,1);t(1,1)=1;t(2,1)=2;uuu=zeros(T,1);yyy=zeros(T,1);uuu(1,1)=0;uuu(2,1)=0;yyy(1,1)=0;yyy(2,1)=0;yyr=zeros(T,1);yyr(1,1)=1.5;yyr(2,1)=1.5;yd=zeros(n,1);dy=zeros(na,1);ddu=zeros(nb+1,1);forij=3:Tt(ij,1)=ij;fors=1:1:naif(ij-s-10)dy(s,1)=-(yyy(ij-s,1)-yyy(ij-s-1,1));endendfors1=1:1:nb+1if(ij-t-10)ddu(s1,1)=uuu(ij-s1,1)-uuu(ij-s1-1,1);endend%DDU=dduX=[dy;ddu];ddy=X'*Q0;ss=ddy-X'*Q1;Q2=Q1+(P1*X*ss)/(p+(X'*P1*X));P2=(P1-(P1*X*X'*P1)/(p+(X'*P1*X)))/p;Q1=Q2;%q2=Q2P1=P2;forf=2:1:na+1A2(f,1)=Q1(f-1,1);endforf1=1:1:nb+1B2(f1,1)=Q1(f1+na,1);endif(nb==1)B1=B2(2,1);endif(nb1)fork=1:1:nbB1(1,k)=B2(k+1,1);endend%b1=B1form1=1:1:naA1(1,m1)=A2(m1,1)-A2(m1+1,1);endA1(1,na+1)=A2(na+1,1);%a1=A1F1=zeros(1,na+1);F2=zeros(1,na+1);H1=zeros(1,nb);H2=zeros(1,nb);e=1;E=e;g=E(1,1)*B(1,1);G1=g;fori=1:na+1F1(1,i)=A1(1,i);endif(nb==1)H1(1,1)=B1(1,1);endif(nb1)forj=1:nbH1(1,j)=B1(1,j);endendF=F1;H=H1;forkh=2:ne=F1(1,1);E=[E;e];g=e*B(1,1)+H1(1,1);G1=[G1;g];fork=1:naF2(1,k)=F1(1,k+1)+A1(1,k)*e;endF2(1,na+1)=A1(1,na+1)*e;F1=F2;F=[F;F2];if(nb==1)H2(1,1)=B1(1,1)*e;endif(nb==2)H2(1,1)=H1(1,2)+B1(1,1)*e;endif(nb2)forh=1:nb-1H2(1,h)=H1(1,h+1)+B1(1,h)*e;endH2(1,nb)=B1(1,nb)*e;endH=[H;H2];H1=H2;endfors2=1:mforl1=n-s2+1:-1:1il=l1+s2-1;G(il,s2)=G1(l1,1);endendyr=1.5;a9=0;fori=1:1a9=a9+rand;%noisesenda8=0.04*a9;yyr(ij,1)=yr+a8;y=1.5*yy(n,1)-0.7*yy(n-1,1)+uu(n,1)+1.5*uu(n-1,1);yyy(ij,1)=y;forp1=na+1:-1:2y1(p1)=y1(p1-1);endy1(1,1)=y;forv=1:1:n-1yy(v,1)=yy(v+1,1);endyy(n,1)=y;if(nb==1)u1(1,1)=uuu(ij-1,1)-uuu(ij-2,1);endif(nb==2)u1(1,1)=u1(2,1);u1(2,1)=uuu(ij-1,1)-uuu(ij-2,1);endif(nb2)forq=nb:-1:2u1(q,1)=u1(q-1,1);endu1(1,1)=uuu(ij-1,1)-uuu(ij-2,1);endy0=F*y1+H*u1;yd(1,1)=a*y+(1-a)*yr;fori=2:nyd(i)=a^i*yd(i-1,1)+(1-a^i)*yr;endu=inv(G'*G+t0*eye(m))*G'*(yd-y0);if(u(1,1)0.2)u(1,1)=0.2;endif(u(1,1)-0.2)u(1,1)=-0.2;endforj=1:n-1uu(j,1)=uu(j+1,1);enduu(n,1)=uuu(ij-1,1)+u(1,1);uuu(ij)=uu(n,1);ddu(2,1)=ddu(1,1);ddu(1,1)=u(1,1);end%plot(t,yyr,t,yyy,'r');subplot(2,1,1):plot(t,yyr,t,yyy,'r');xlabel('t');ylabel('yr,y');subplot(2,1,2):plot(t,uuu);xlabel('t');ylabel('u');