% Model Predictive Control Example

% Define the system matrices.
A=[2.4936 -1.2130 0.4176
   2       0      0
   0       1      0];
B=[0.25 0 0]';
C=[0.0684 0.1288 0.0313];
D=0;

Np=10;        % Prediction and control horizon
lambda=1;     % Trade-off parameter
x0=[1 1 1]';  % Initial state

% Construct the prediction matrices.
At=[];
for i=1:Np,
  At=[At;A^i];
end;

temp=[];
Bt=[];
for i=1:Np,
  Bt=[Bt        zeros(size(Bt,1),size(B,2))
      A^(i-1)*B temp];
  temp=[A^(i-1)*B temp];
end;

Ct=[];
for i=1:Np,
  Ct=[Ct zeros(size(Ct,1),size(C,2))
      zeros(size(C,1),size(Ct,2)) C];
end;

% Define the lower and upper bounds, and the initial value.
lb=-0.25*ones(Np,1);
ub=0.25*ones(Np,1);
u0=zeros(Np,1);

% Simulation of the closed-loop system
u=[];
x=x0;     % Initial state
y=C*x0;   % Initial output
xk=x0;    % Start of the horizon
H=2*((Ct*Bt)'*(Ct*Bt)+lambda*eye(Np,Np)); % Note that H is a constant
                                       % matrix that does not depend 
                                       % on k.


options=optimoptions('quadprog','Algorithm','active-set', ...
                                'Display','off');
                                   % Select the active-set algorithm
                                   % and disable display

for k=1:50,
  [ut,fval,exitflag]=quadprog(H,2*xk'*At'*Ct'*Ct*Bt,[],[],[],[],lb,ub,u0,options);
  if ( exitflag < 0 )
    disp('Failure');
    return;
  end;
  u(k)=ut(1);                   % Implement only the 1st control
                                % sample.
  x(:,k+1)=A*x(:,k)+B*u(k);     % Compute the state.
  y(:,k+1)=C*x(:,k+1);          % Compute the output.
  xk=x(:,k+1);                  % Shift the horizon.
  u0=[ut(2:Np);ut(Np)];         % Use the shifted solution as the
                                % initial solution for the next
                                % iteration.
end;
