%Lowell Brown
%2012-05-10
%Model Predictive Control Study

%Example worked from
%Source: Liuping Wang -> Model Predictive Control System Design and
%Implementation Using Matlab.
%Ch. 1 Matlab Tutorials 

%% Tutorial 1.2 Page 013
% The input parameters to function are the discrete state-space model (Ap, Bp, Cp),
% the prediction horizon Np and the control horizon Nc

%Test: [Phi_Phi, Phi_F, PhiR, A_e, B_e, C_e] = mpcgain(0.8, 0.1, 1, 4, 10)


function [Phi_Phi, Phi_F, Phi_R, A_e, B_e, C_e] = mpcgain(Ap, Bp, Cp, Nc, Np)


%determine the numbers of states, inputs and outputs
[m1, n1] = size(Cp);
[n1, n_in] = size(Bp);
%augment the model with an integrator
A_e = eye(n1+m1,n1+m1);
A_e(1:n1,1:n1) = Ap;
A_e(n1+1:n1+m1,1:n1) = Cp*Ap;
B_e(1:n1,:) = Bp;
B_e(n1+1:n1+m1,:) = Cp*Bp;
C_e = zeros(m1,n1+m1);
C_e(:,n1+1:n1+m1) = eye(m1,m1);

n = n1 + m1;
h(1,:) = C_e;
F(1,:) = C_e*A_e;

for kk = 2:Np
    h(kk,:) = h(kk-1,:)*A_e;
    F(kk,:) = F(kk-1,:)*A_e;
end

v = h*B_e;
Phi = zeros(Np,Nc); %declare the dimensions of Phi
Phi(:,1) = v;       %first column of Phi

for ii = 2:Nc
    Phi(:,ii) = [zeros(ii-1,1);v(1:Np-ii+1,1)];
end

BarRs = ones(Np,1);
Phi_Phi = Phi'*Phi;
Phi_F = Phi'*F;
Phi_R = Phi'*BarRs;

