ssmpc_predclp.m

%%%%%  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: [e-mail])

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;
  
end

Pz1 = [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