%MISMATCH Function to investigate effects of mismatch in MPC
%
%  Usage: [y,u,ym] = mismatch(percent,M,P,Ts,altsp,tend,elevlim,eratelim,...
%                       pitchlim,hdotlim,hlim)
%
% Uses Cessna Citation linearised model. Perturbs gain to altitude rate
% by specified percentage. 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 fixed at 0. Set-points for pitch and climb-rate fixed
% at 0.
%      Needs: function `makecita' and MPC Toolbox.
%
% Input arguments:
%     percent: magnitude of gain perturbation in percent ( <0 ok).
%         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/Ts.
%     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.
%
% Output arguments:

% J.M.Maciejowski, 2 October 1998, revised 29.12.99.
% Cambridge University Engineering Dept.
% Copyright (C) 1998, All rights reserved.

function [y,u,ym] = mismatch(percent,M,P,Ts,altsp,tend,elevlim,eratelim,...
                       pitchlim,hdotlim,hlim)

nargchk(3,11,nargin);

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

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

a = get(citation,'a'); c = get(citation,'c');
disp(['Original parameter value: ',num2str(a(4,1))]);

a(4,1) = a(4,1)*(1+percent/100); % Perturbation for plant
a(4,2) = -a(4,1);  % This preserves the structure
c(3,1) = a(4,1);   % a(4,1) = -a(4,2) = c(4,1) = -c(4,2)
c(3,2) = -a(4,1);  % which changes the gain to altitude rate.
disp(['New parameter value: ',num2str(a(4,1))]);
disp([num2str(percent),' percent perturbation']);

citation2 = citation;  % This is going to be the `real' plant
set(citation2,'a',a);  % Make the changes to a and c matrices.
set(citation2,'c',c);

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

% Form internal model in MPC Toolbox format:
imod = ss2mod(get(dcitation,'a'),get(dcitation,'b'),get(dcitation,'c'),...
              get(dcitation,'d'),Ts);

% Form `real' plant in MPC Toolbox format:
pmod = ss2mod(get(dcitation2,'a'),get(dcitation2,'b'),get(dcitation2,'c'),...
              get(dcitation2,'d'),Ts);



% 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.

% Run MPC simulation:
tic;
[y,u,ym] = scmpc(pmod,imod,ywt,uwt,M,P,tend,setpt,ulim,ylim);
            % Use modified `scmpc' which preserves results obtained 
            % before infeasibility.
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)]);