%Lowell Brown
%2012-05-11
%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.3 Page 020
%Requires mpcgain.m -> to generate the necessay gain matrices
clear
clc
%Define the Plant (SISO system)
% The plant is the double integrator system 
Ap = [1 1;...
      0 1];
Bp = [0.5; 1];
Cp = [1 0];
Dp = 0;

%Define the prediction horizon and the control horizon 
%Note Np > Nc
Np = 20; %Prediciton horizon
Nc = 4;  %Control horizon

%Generate the necessay gain matrices
[Phi_Phi, Phi_F, Phi_R, A_e, B_e, C_e] = mpcgain(Ap, Bp, Cp, Nc, Np);
[n, n_in] = size(B_e);
xm = [0; 0];
Xf = zeros(n,1);
N_sim = 11;
r = ones(N_sim,1);
u = 0; %sets: u(k-1) = 0
y = 0;

%LOOP STEPS
%(1.) At sample time kk, the \Delta U vector is calculated using the set-point
%signal r(kk) and the state vector Xf.
%(2.) \Delta u(kk) is taken as the first element of \Delta U; 
%-> u(kk) = u(kk-1) + \Delta u(kk).
%(3.) The weight factor is selected as 0.1;
%(4.) plant state and output are simulated using the generated control
%signal u
%(5.) The state variable used in the feedback mechanism is updated as Xf.

r_w = 0.1;
r_w = 2; %small = larger control effort quicker settle time
% r_w = 50; % larger = smaller control effort slower settle time
for kk = 1:N_sim
    DeltaU = inv(Phi_Phi + r_w*eye(Nc,Nc))*(Phi_R*r(kk) - Phi_F*Xf);
    delta_u = DeltaU(1,1);
    u = u + delta_u; %control signal
    u1(kk) = u;
    y1(kk) = y;
    
    xm_old = xm;
    xm = Ap*xm + Bp*u;
    y = Cp*xm;
    Xf = [xm - xm_old; y];
end

k = 0:(N_sim-1);
figure1 = figure;
axes1 = axes('Parent',figure1,'FontSize',16);
box(axes1,'on');
grid(axes1,'on');
hold(axes1,'all');
subplot(2,1,1)
plot(k,y1, 'g', 'linewidth', 1.5)
xlabel('Sampling Instant','FontSize',16);
legend({'y_1 Output'},'FontSize',16);
grid on

subplot(2,1,2)
plot(k,u1, 'c','linewidth', 1.5)
xlabel('Sampling Instant','FontSize',16);
legend({'u_1 Control'},'FontSize',16);
grid on
    



