%%%%%  To find prediction equations/matrices given a fixed feedback loop%%%%%%%%%%  Feedback loop is of the form  u = -Knew[x;d] + Pr*r  + c%%%%%%%%%%  z=[x;d]    are  the estimates of x and the disturbance d%%%%%%%%%%  Model of the form    x = Ax+Bu    y = Cx + Du + d  (Assumes D=0)%%%%%%%%%%  nc is the number of extra d.o.f. c for constraint handling%%%%%%%%%%  PREDICTIONS ARE%%%%%  x =  Pc1*c + Pz1*z + Pr1*r%%%%%  u =  Pc2*c + Pz2*z + Pr2*r%%%%%  y =  Pc3*c + Pz3*z + Pr3*r%%%%%  e =  r - y is predicted tracking error%%%%%   %%%%%  [Pc1,Pc2,Pc3,Pz1,Pz2,Pz3,Pr1,Pr2,Pr3] = ssmpc_predclp(A,B,C,D,Knew,Pr,nc);%%%%%%%  %% Author: J.A. Rossiter  (email: J.A.Rossiter@shef.ac.uk)function [Pc1,Pc2,Pc3,Pz1,Pz2,Pz3,Pr1,Pr2,Pr3] = ssmpc_predclp(A,B,C,D,Knew,Pr,nc);nu = size(B,2);nx = size(A,1);ny = size(C,1);K = Knew(:,1:nx);   K2 = Knew(:,nx+1:nx+ny);Phi = A-B*K;Px1 = Phi; Px3 = C*Phi;P = Phi; P2 = eye(nx);Pc1 = B;Pd1 = -B*K2;Pr1 = B*Pr;Pc2 = eye(nu);Pd2 = -K2;Pr2 = Pr;Pc3 = C*B;Pd3 = C*B*K2;Pr3 = C*B*Pr;Px2 = -K;for i=1:50;  Pc3 = [Pc3;C*P*B];  Pc2 = [Pc2;-K*P2*B];    vec = (i-1)*nu+1:i*nu;   Pd2 = [Pd2;Pd2(vec,:)+K*P2*B*K2];  Pr2 = [Pr2;Pr2(vec,:)-K*P2*B*Pr];    vec = (i-1)*ny+1:i*ny;  Pd3 = [Pd3;Pd3(vec,:)+C*P*B*K2];  Pr3 = [Pr3;Pr3(vec,:)+C*P*B*Pr];      Px2 = [Px2;-K*P];  Pc1 = [Pc1;P*B];   vec = (i-1)*nx+1:i*nx;  Pd1 = [Pd1;Pd1(vec,:)-P*B*K2];  Pr1 = [Pr1;Pr1(vec,:)+P*B*Pr];    P = P*Phi;    Px1 = [Px1;P];  Px3 = [Px3;C*P];  P2 = P2*Phi;  endPz1 = [Px1,Pd1];Pz2 = [Px2,Pd2];Pz3 = [Px3,Pd3];nP = size(Pc1,1);nP2 = size(Pc2,1);nP3 = size(Pc3,1);for i=2:nc;  vec = (i-1)*nu+1:i*nu;  vec2 = (i-1)*nx+1:nP;  Pc1(vec2,vec) = Pc1(vec2-(i-1)*nx,1:nu);    vec2 = (i-1)*nu+1:nP2;  Pc2(vec2,vec) = Pc2(vec2-(i-1)*nu,1:nu);    vec2 = (i-1)*ny+1:nP3;  Pc3(vec2,vec) = Pc3(vec2-(i-1)*ny,1:nu);  end