function [yobs,u]=MPC11(FSRp,FSRm,simstep,rttype,L,P,M,alf,weighty,weightu,const_u,sp,step_r,dist,step_d)
% ------------------------------------------------------
% ***  Model Predictive Control for SISO System  ***
% Coded by Manabu KANO, Kyoto Univ.,   Nov. 01, 1995
% 			last updated : Feb. 10, 2001 
%
% USAGE :
% [yobs,u]=MPC11(FSRp,FSRm,simstep,rttype,L,P,M,alf,.....
%  weighty,weightu,const_u,sp,step_r,dist,step_d);
%
% DESCRITION :
% This function executes SISO MPC.
%
% --- Input ---
% FSRp		: step response coef. of process
% FSRm		: step response coef. of model
%   < option >
% simstep	: simulation step
% rttype	: type of reference trajectory
%		  If rttype=0, reference trajectory is not used.
% L,P,M,alf	: control parameters
% weighty	: output weight
% weightu	: input weight
% const_u = [u_max u_min du_max du_min]
%		  du(t) = u(t)-u(t-1)
% sp, step_r	: setpoint, step
% dist, step_d	: disturbance, step 
%
% --- Output --- 
% yobs		: controlled output
% u		: input
% 
% --- Example ---
% FSRp = [0; 0; 0; step(4,[12 1],1:81)];
% FSRm = [0; 0; 0; step(3,[10 1],1:81)];
% simstep = 100;
% rttype = 2; L = 4; P = 10; M = 2; alf = 0.5;
% weighty = 1;
% weightu = 0;
% const_u = [inf -inf inf -inf];
% sp   = 10; step_r = 10; 
% dist = 10; step_d = 50;
% [yobs,u]=MPC11(FSRp,FSRm,simstep,rttype,L,P,M,alf,.....
%  weighty,weightu,const_u,sp,step_r,dist,step_d);
% ------------------------------------------------------


if nargin < 15 | isempty(step_d),  step_d = 0; end 
if nargin < 14 | isempty(dist),    dist = 0; end
if nargin < 13 | isempty(step_r),  step_r = 10; end 
if nargin < 12 | isempty(sp),      sp = 1; end
if nargin < 11 | isempty(const_u), const_u = [inf -inf inf -inf]; end
if nargin < 10 | isempty(weightu), weightu = 0; end
if nargin <  9 | isempty(weighty), weighty = 1; end
if nargin <  8 | isempty(alf),     alf = 0.5; end
if nargin <  7 | isempty(M),       M = 1; end
if nargin <  6 | isempty(P),       P = 1; end
if nargin <  5 | isempty(L),       L = length(find(FSRm==0))+1; end
if nargin <  4 | isempty(rttype),  rttype = 1; end
if nargin <  3 | isempty(simstep), simstep = length(FSRm); end

%----- Step Response Coefficients -----
[row,col] = size(FSRp);
if row>col, FSRp=FSRp'; end	
[row,col] = size(FSRm);
if row>col, FSRm=FSRm'; end
Pdim   = length(FSRp);  
Mdim   = length(FSRm); 
Maxdim = max(Mdim,Pdim);

%----- Display -----
fprintf(' \n')
fprintf(' ************************************************* \n')
fprintf('     Model Predictive Control Simulation	\n')
fprintf(' \n')
fprintf('    <Setting> \n')
fprintf('     Reference Trajectory Type	: %g \n'	,rttype)
fprintf('     Control Parameters : L=%g, P=%g, M=%g \n'	,L,P,M)
fprintf('                          alpha=%g \n'		,alf)
fprintf('                          weighty=%g \n'	,weighty)
fprintf('                          weightu=%g \n'	,weightu)
fprintf('     Setpoint=%g \n'				,sp) 
fprintf('     Simulation Steps=%g \n'			,simstep) 
fprintf(' \n')
fprintf(' ************************************************* \n')

weighty = weighty*eye(P);
weightu = weightu*eye(M,M);


% /////////////////////////////////////////////////////////////
%                 MATRIX CALCULATION SECTION
% /////////////////////////////////////////////////////////////

%----- Initializing Matrices -----
Dpao = zeros(1,Pdim);
Dmaf = zeros(L+P-1,M);
Dmao = zeros(L+P-1,Mdim);

%----- Dynamic Matrix -----
FSRp(1,Pdim+1) = FSRp(1,Pdim);
Dpao(1,:) = FSRp(1,2:Pdim+1)-FSRp(1,1:Pdim);
FSRp1 = FSRp(1);

%----- Past Input Matrix -----
FSRm(1,Mdim+1:Mdim+L+P-1) = ones(1,L+P-1)*FSRm(1,Mdim);
for i=1:L+P-1;
  Dmao(i,:) = FSRm(1,i+1:i+Mdim)-FSRm(1,1:Mdim);
end
Aomat = Dmao(L:L+P-1,1:Mdim);
if rttype==2 & L~=1;
  Ao2 = Dmao(L-1,1:Mdim);
end

%----- Future Input Matrix -----
for i=L:L+P-1;
  for j=1:M;
    if i-j+1>0
      if i-j+1>=Mdim
        Dmaf(i,j) = FSRm(1,Mdim);
      else 
        Dmaf(i,j) = FSRm(1,i-j+1);
      end
    end
  end
end
if rttype==2 & L~=1;
    Dmaf(L:L+P-1,1) = Dmaf(L:L+P-1,1)-alf.^((L:L+P-1)'-L+1)*FSRm(1,L-1);
end
Afmat = Dmaf(L:L+P-1,1:M);

%----- Gain Matrix -----
Gain = Afmat'*weighty*Afmat+weightu;

clear Dmao Dmaf FSRp FSRm


% /////////////////////////////////////////////////////////////
%                     SIMULATION SECTION
% /////////////////////////////////////////////////////////////

%----- Initialization -----
yobs = 0;
y  = zeros(simstep,1);
du = zeros(simstep,1);
u  = zeros(simstep,1);
uf = zeros(M,1);
uo = zeros(Maxdim,1);
yr = zeros(P,1);

% ================== SIMULATION LOOP STARTS ===================
for time=1:simstep; 

%----- Disturbance and Setpoint Change -----
if time<step_d
  yobs(time,1) = y(time,1);
else
  yobs(time,1) = y(time,1)+dist;
end
if time<step_r
  ysetpoint(time,1) = 0;
else
  ysetpoint(time,1) = sp;
end

%----- Reference Trajectory -----
alfP = alf.^(1:P)';
if rttype==0
  yr(1:P,1) = ones(P,1)*ysetpoint(time,1);
elseif rttype==1 | L==1
  yr(1:P,1) = alfP*yobs(time,1)+(1-alfP)*ysetpoint(time,1);
elseif rttype==2
  yr(1:P,1) = alfP*(yobs(time,1)+Ao2*uo(1:Mdim,1))+(1-alfP)*ysetpoint(time,1);
end

%----- Calculating Input -----
yf = ones(P,1)*yobs(time,1)+Aomat*uo(1:Mdim,1);
er = yr-yf;
uf = Gain\Afmat'*weighty*er;
du(time,1) = uf(1);

%----- Input Constraints -----
if du(time,1)>const_u(3), du(time,1) = const_u(3); end
if du(time,1)<const_u(4), du(time,1) = const_u(4); end

if time==1
  u(time,1) = du(time,1);
else
  u(time,1) = u(time-1,1)+du(time,1);
end
if u(time,1)>const_u(1)
  if time==1
    du(time,1) = const_u(1);
    u(time,1)  = const_u(1);
  else
    du(time,1) = const_u(1)-u(time-1,1);
    u(time,1)  = const_u(1);
  end
end
if u(time,1)<const_u(2)
  if time==1
    du(time,1) = const_u(2);
    u(time,1)  = const_u(2);
  else
    du(time,1) = const_u(2)-u(time-1,1);
    u(time,1)  = const_u(2);
  end
end

%----- Calculating Process Output -----
y(time+1,1) = y(time,1)+FSRp1*du(time,1)+Dpao*uo(1:Pdim,1);

%----- Input and Output Arrangement -----
for i=Maxdim:-1:2;
  uo(i,1) = uo(i-1,1);
end
uo(1,1) = du(time,1);

end
% =================== SIMULATION LOOP ENDS ====================

% /////////////////////////////////////////////////////////////
%                     EVALUATION SECTION
% /////////////////////////////////////////////////////////////

clf
subplot(2,1,1)
plot(1:simstep,yobs,'b*',1:simstep,ysetpoint,'r.')
xlabel('time-steps'),ylabel('output'),grid
title('Simulation of Model Predictive Control')

subplot(2,1,2)
plot(u,'b*')
xlabel('time-steps'),ylabel('input'),grid

