%% Designing Model Predictive Controller Equivalent to Infinite-Horizon LQR
% This example shows how to design an infinite-horizon model predictive
% controller by setting the weights on the terminal predicted states.

% Copyright 2009-2012 The MathWorks, Inc.

%% Define Plant Model
% The linear open-loop dynamic model is defined below.  DC gain is also
% computed.
Ts = 0.1;                                   % Sampling time
A = [0.8 Ts;0 0.9];                              
B = [0;Ts];
C = [1 0];
sysd = ss(A,B,C,0,Ts);                      % Discrete-time plant model
dcg = dcgain(sysd);                         % DC-gain of prediction model

%% Design an Infinite-Horizon LQR Controller
% Compute the Riccati matrix associated with the LQR problem with output
% weight |Qy| and input weight |Qu|:
Qy = 10;                                    % Output weight: y'*Qy*y
Qu = 0.1;                                   % Input weight:  u'*Qu*y
[K,P] = lqry(sysd,Qy,Qu);                   % LQR gain and Riccati matrix
%%
% Weight the terminal state |x'(t+p)*P*x(t+p)|, where |p| is the prediction
% horizon of the MPC controller. Compute the Cholesky factor |chol(P)| of
% the Riccati matrix |P|, so that the terminal penalty becomes:
%
% |x'(t+p)*P*x(t+p) = [chol(P)*x(t+p)]'*[chol(P)*x(t+p)] = yc'(t+p)*yc(t+p)|
%
% where the new output |yc(t+p) = chol(P)*x(t+p)| and the state |x(t)| is
% assumed to be fully measurable.
cholP = chol(P);     
%% 
% Change and augment the output vector to include the full state |x| and |yc|:
% Output = state vector x + output yc such that yc'*yc = x'*P*x
sysd.C = [eye(2);cholP];
sysd.D = zeros(4,1);

%% Convert the LQR Controller Into a Finite-Horizon MPC Controller
% Label the new additional output signal yc(t) as unmeasured:
sysd = setmpcsignals(sysd,'UO',[3 4]); % Cholesky factor is not measured
%%
% Create the controller object with sampling period, prediction and control
% horizons:
p = 3;                   % Prediction horizon (for any p>=1, unconstrained MPC = LQR)
m = p;                   % Control horizon = prediction horizon
mpcobj = mpc(sysd,Ts,p,m); % MPC object
%%
% Specify actuator saturation limits as MV constraints.
mpcobj.MV = struct('Min',-3,'Max',3);
%% 
% Define input and output weights at each step of the prediction horizon
% (terminal weights are set later). Very small weights on input increments
% are included to make the QP problem associated with the MPC controller
% positive definite:
mpcobj.Weights.OV = [sqrt(Qy) 0 0 0];       % Output weights (only on original output)
mpcobj.Weights.MV = sqrt(Qu);               % Input weight
mpcobj.Weights.MVRate = 1e-5;               % Very small weight on command input increments
%%
% Define set-points for the output and input signals:
ry = 1;                                     % Output set point
mpcobj.MV.Target = ry/dcg;                  % Set-point for manipulated variable
%%
% Impose the terminal penalty |x'(t+p)*P*x(t+p)| by specifying a unit
% weight on |yc(t+p) = chol(P)*x(t+p)|. The terminal weight on |u(t+p-1)|
% remains the same, that is |sqrt(Qu)|:
Y = struct('Weight',[0 0 1 1]);             % Weight on y(t+p)
U = struct('Weight',sqrt(Qu));              % Weight on u(t+p-1)
setterminal(mpcobj,Y,U);                    % Set terminal weight y'*y = x'*P*x
%%
% Since the measured output is the entire state, remove any additional
% output disturbance integrator inserted by the MPC controller:
setoutdist(mpcobj,'model',ss(zeros(4,1)));                
%%
% Remove the state estimator by defining the following measurement update
% equation:
%
% |x[n|n] = x[n|n-1] + I * (x[n]-x[n|n-1]) = x[n]|
%
% Because |setterminal| function resets state estimator to default value,
% we use |setEstimator| function to change state estimator after
% |setterminal| is called.
setEstimator(mpcobj,[],eye(2));             % State estimate = state measurement

%% Compare MPC and LQR Controllers
% Compute the gain of the MPC controller when constraints are inactive, and
% compare it to the LQR gain:
mpcgain = dcgain(ss(mpcobj));
fprintf('\n(unconstrained) MPC: u(k)=[%8.8g,%8.8g]*x(k)',mpcgain(1),mpcgain(2));
fprintf('\n                LQR: u(k)=[%8.8g,%8.8g]*x(k)\n\n',-K(1),-K(2));
%%
% The state feedback gains are exactly the same.

%% Simulate Using Simulink(R)
% To run this example, Simulink(R) is required.
if ~mpcchecktoolboxinstalled('simulink')
    disp('Simulink(R) is required to run this example.')
    return
end
%%
% Define a set-point for the new extended output vector containing |x| and
% |chol(P)*x|:
rx = (eye(2)-A)\B/dcg;
r = [rx;cholP*rx];                          % Set point for extended prediction model
%%
% Simulate closed-loop control of the linear plant model in Simulink.
% Controller "mpcobj" is specified in the block dialog.
mdl = 'mpc_infinite';
open_system(mdl);                           % Open Simulink(R) Model
sim(mdl);                                   % Start Simulation
%%
% The closed-loop response shows good setpoint tracking performance.

%%
bdclose('mpc_infinite');
