%DISTURB Function to simulate effects of disturbance in MPC
%
%  Usage: [y,u,ym,xp,xm,lagmul] = disturb(amp,dur,M,P,Ts,altsp,tend,...
%                           elevlim,eratelim,pitchlim,hdotlim,hlim,...
%                           pencoeff,normtype)
%
% This version with soft output constraints
%
% Uses Cessna Citation linearised model. 
% Applies pulse disturbance on altitude rate (climb rate) after 10 steps.
% Uniform weights on outputs used; input moves
% and output errors weighted equally strongly. Default observer used.
% Default simulation length: (altitude change)/4. 
% Minimum altitude and climb-rate unconstrained in this version (30.12.99).
% Set-points for pitch and climb-rate fixed at 0.
%      Needs: function `makecita' and MPC Toolbox.
%
% Input arguments:
%         amp: ampliude of pulse on climb rate (m/sec)
%         dur: duration of pulse on climb rate (sec)
%         M,P: control and prediction horizons, respectively (# steps).
%          Ts: sampling and update interval (s). Default: 1 s.
%       altsp: altitude set-point (m). (Initial altitude 0). Default: 20 m.
%        tend: length of simulation run (s). Default: altsp/4.
%     elevlim: symmetric elevator limit (deg). Default: 15 deg.
%    eratelim: symmetric elevator rate limit (deg/s). Default: 30 deg/s.
%    pitchlim: symmetric pitch limit (deg). Default: 20 deg.
%     hdotlim: positive climb rate limit (m/sec). Default: infinite.
%        hlim: positive altitude limit (m). Default: altsp.
%    pencoeff: penalty coefft for output constraint violations. Default:inf
%    normtype: `1-norm' or `2-norm' or `inf-norm' or `mixed-norm'. Def: inf
%
% Output arguments:
%           y: Plant outputs.          u: Plant inputs
%          ym: noise-free outputs     xp: Plant states
%          xm: Model states, in augmented MPCTB format
%      lagmul: Lagrange multipliers

% J.M.Maciejowski, 30 December 1999.
% Cambridge University Engineering Dept.
% Copyright (C) 1999, All rights reserved.

function [y,u,ym,xp,xm,lagmul] = disturb(amp,dur,M,P,Ts,altsp,tend,...
              elevlim,eratelim,pitchlim,hdotlim,hlim,pencoeff,normtype)

nargchk(3,14,nargin);

% Process input arguments:
if nargin<5 | isempty(Ts), Ts = 1; end;
if nargin<6 | isempty(altsp), altsp = 20; end;
if nargin<7 | isempty(tend), tend = altsp/4; end;
if nargin<8 | isempty(elevlim), elevlim = 15; end;   % DEG
if nargin<9 | isempty(eratelim), eratelim = 30; end; % DEG/S
if nargin<10 | isempty(pitchlim), pitchlim = 20; end; % DEG
if nargin<11 | isempty(hdotlim), hdotlim = inf; end;
if nargin<12 | isempty(hlim), hlim = altsp; end;
if altsp<0, error('Altitude set-point should be nonnegative'), end;
if nargin<13 | isempty(pencoeff), pencoeff = inf; end; % default: hard
if nargin<14 | isempty(normtype), normtype = 'inf-norm'; end; %default: inf

citation = makecita('lti'); % Makes `citation' as LTI object

a = get(citation,'a');  b = get(citation,'b');
c = get(citation,'c');  d = get(citation,'d');
% Augment B matrix for disturbance:
bw = [0;0;0;1]; % Disturbance comes in on altitude rate
dw = [0;0;1];
bb = [b,bw]; 
dd = [d,dw];
% Augment citation model:
set(citation,'b',bb,'d',dd,'InputName',{'Elevator';'Disturbance'});

dcitation = c2d(citation,Ts,'zoh');   % Form discrete-time equivalent
                                      % (no computational delay)

% Form internal model in MPC Toolbox format:
a = get(dcitation,'a');   b = get(dcitation,'b');
c = get(dcitation,'c');   d = get(dcitation,'d');
minfo = [Ts,4,1,0,1,3,0];

imod = ss2mod(a,b,c,d,minfo);

% Form `real' plant in MPC Toolbox format - same as model:
pmod = imod;

% Unit conversions:
elevlim = elevlim*pi/180;
eratelim = eratelim*Ts*pi/180; % rate limit changed into max delta.
pitchlim = pitchlim*pi/180;

% Form arguments in MPC Toolbox format:  
ywt = [1,1,1]; uwt = 1; % Uniform weights on everything.
setpt = [0,altsp,0];    % 0 set-points for pitch and climb-rate.
ulim = [-elevlim,elevlim,eratelim];           % Elevator limits.
ylim = [-pitchlim,0,0,pitchlim,hlim,hdotlim]; % Output limits.
% Form unmeasured disturbance:
wpulse = amp*ones(max(fix(dur/Ts),1),1);
w = [zeros(10,1);wpulse;0]; % 10 steps before pulse is applied.

% Soften output constraints:
if strcmp(normtype,'1-norm') | strcmp(normtype,'2-norm') | ...
   strcmp(normtype,'mixed-norm'),
  suwt = [inf,inf,inf];  % Keep input constraints hard
%  sywt = pencoeff*[1,1,1,1,1,1];  % Soft output constraints
  sywt = pencoeff*[1,0,0,1,1,1];  % min altitude & alt. rate unconstrained.
elseif strcmp(normtype,'inf-norm'),
  suwt = pencoeff;
  sywt = [];
end

% Run MPC simulation:
tic;
[y,u,ym,xp,xm,lagmul] = scmpc2(pmod,imod,ywt,uwt,M,P,tend,setpt,ulim,ylim,...
                  [],[],[],w,[],[],[],[],suwt,sywt,normtype);
            % Use modified `scmpc' which preserves results obtained 
            % before infeasibility, allows soft constraints, etc.
trun = toc;
disp('********** MPC run finished **********')
disp([' Time to run simulation = ',num2str(trun)]);
nsteps = size(y,1);
disp([' Time needed per step = ',num2str(trun/nsteps)]);

%%%%% Display results: 4 plots to a figure:
tvec = 0:Ts:(nsteps-1)*Ts; % Allows for premature termination of scmpc2.
figure

subplot(221)

plot(tvec,y(:,1)*180/pi); grid
xlabel('Time (sec)'); ylabel('Pitch (deg)'); 
title('Pitch angle and constraint')
if pitchlim < inf,
  axis([0,tend,-1.2*pitchlim*180/pi,1.2*pitchlim*180/pi])
  hold
  plot([0,tend],[1 1]*pitchlim*180/pi,'--')
  plot([0,tend],-[1 1]*pitchlim*180/pi,'--')
else
  axis([0,tend,-1.2*abs(min(y(:,1)))*180/pi,1.2*abs(max(y(:,1)))*180/pi])
end

subplot(222)
plot(tvec,y(:,2)); grid
xlabel('Time (sec)'); ylabel('Altitude (m)'); title('Altitude and set-point')
axis([0,tend,0,1.2*max(max(y(:,2),altsp))])
hold
plot([0,tend],[1 1]*altsp,'--')

subplot(223)
plot(tvec,y(:,3)); grid
xlabel('Time (sec)'); ylabel('Altitude rate (m/sec)'); 
title('Altitude rate and constraint')
if hdotlim < inf,
  axis([0,tend,-5,1.2*max(max(y(:,3),hdotlim))])
  hold
  plot([0,tend],[1 1]*hdotlim,'--')
else
  axis([0,tend,-5,1.2*max(y(:,3))])
end

subplot(224)
stairs(tvec,u*180/pi); grid
xlabel('Time (sec)'); ylabel('Elevator angle (deg)'); 
title('Elevator angle and constraint')
if elevlim < inf,
  maxu = 1.2*max(max(abs(u*180/pi)),elevlim*180/pi);
  axis([0,tend,-maxu,maxu])
  hold
  plot([0,tend],[1 1]*elevlim*180/pi,'--')
  plot([0,tend],-[1 1]*elevlim*180/pi,'--')
else
  axis([0,tend,-1.2*abs(min(u*180/pi)),1.2*abs(max(u*180/pi))])
end