function [sys,x0,str,Ts] = mpcsfun(t,x,yrd,flag,...
   model,p,moves,limits,weights,MPCadd,filename,anticipate)

%
% MPCBLOCK S-function for SIMULINK block mpcblock.
%
% Usage: [sys,x0,str,Ts] = mpcblock(t,x,sysu,flag,...
%             model,p,moves,limits,weights,utarget,mvindex,mdindex,myindex,...
%             noisemodel,estspeed,x0,u1,ts)
%
% Masked inputs:
%   model:     prediction LTI model
%   p:         prediction horizon
%   moves:     (blocking) moves
%   limits:    cell array of bounds for u,du,y
%   weights:   cell array of weights for (u-utarget),du,(y-r)
%   MPCadd:    struct containing additional MPC parameters
%   		MPCadd.utarget:   target for the input
%   		MPCadd.mvindex:   vector of indices of manipulated variables (within input vector)
%   		MPCadd.mdindex:   vector of indices of measured disturbances (within input vector)
%   		MPCadd.myindex:   vector of indices of measured outputs (within output vector)
%   		MPCadd.noisemodel:LTI model for estimating and predicting the unmeasured disturbance
%   		MPCadd.covmat:    covariance matrices for state-estimator
%   		MPCadd.rhoeps:    weight on slack variable for soft constraints
%   		MPCadd.x0:        initial state estimate xhat(0)
%   		MPCadd.u1:        initial last input u(-1)
%   		MPCadd.ts:        MPC controller sample time (if empty, inherited from model)
%        MPCadd.delay:     Simulated computational delay
%   filename:  MAT file containing reference and measured disturbance signals
%   anticipate:flag which is either 1 (anticipate) or 0 (do not anticipate)
%
% Inputs: yrd=[measured_outputs] (r=0, v=0)
%         yrd=[measured_outputs reference] (v=0)
%         yrd=[measured_outputs reference measured_disturbance]
%
% State:  None (was: sys=[x(k|k-1);u(k-1)])
%
%   Copyright (c) 1998 by Alberto Bemporad, September 17, 1998

%   $Revision: 1.6$   $Date: 2001/05/23 $  Fixed bug for anticipation with multi-dim r/v
%   $Revision: 1.5$   $Date: 2001/03/01 $  Fixed bug for I/O delays
%   $Revision: 1.4$   $Date: 2000/09/02 $
%   $Revision: 1.25$  $Date: 2000/08/01 $
%   $Revision: 1.2 $  $Date: 1999/01/12 $
%   $Revision: 1.1 $  $Date: 1998/09/17 $


%persistent nu nv nd nutot nym nx ny A Bu Bv Bd Cm Dd Dv Dvm Du 
%persistent TYPE L Mat optimalseq degrees 
%persistent unindex lastx lastu lastt nxnoise
%persistent uwt duwt ywt umin umax dumin dumax ymin ymax
%persistent model_ utarget_ degrees_ delay_ ts_ p_ moves_ covmat_ x0_ u1_
%persistent mvindex_ mdindex_ myindex_ 
%
%% parameters passed through MPCSFUN cannot be persistent. That's why
%% I have to define model_ instead of model, etc.

% Take care of eventual multiple blocks by defining cell array _XX
% for variable XX

%persistent _nu _nv _nd _nutot _nym _nx _ny _A _Bu _Bv _Bd _Cm 
%persistent _Dd _Dv _Dvm _Du 
%persistent _TYPE _L _Mat _optimalseq _degrees 
%persistent _unindex _lastx _lastu _lastt _nxnoise
%persistent _uwt _duwt _ywt _umin _umax _dumin _dumax _ymin _ymax
%persistent _model _utarget _degrees _delay _ts _p _moves _covmat _x0 _u1
%persistent _mvindex _mdindex _myindex 
persistent GCBH MpcStruct

gc=gcbh; % Unique identifier of the simulink block

switch flag
   
case 0 % Initialization
   
   % Extract additional mask parameters from MPCadd structure
   [utarget,mvindex,mdindex,myindex,noisemodel,covmat,rhoeps,x0,u1,ts,delay]=...
      getadd(MPCadd);
   
   % Check consistency of model, sampling time, initial state, previous input
   % Possibly (re)sample the model and remove delays
   [model,ts,x0]=chkmodel(model,ts,x0,u1);
   
   % Check if delay is ok.
   delay=chkdelay(delay,ts); 
   
   nx=size(model.a,1);
   
   D=model.d;
   [ny,nutot]=size(D); % ny=total number of outputs, nutot=total number of inputs
   
   % Check correctness of indices
   [mvindex,mdindex,myindex,unindex]=chkindex(mvindex,...
      mdindex,myindex,nutot,ny);
   
   Du=D(:,mvindex);
   if norm(Du,'inf'),
      error('Matrix D has nonzero components corresponding to manipulated variables.')
   end
   
   nu=length(mvindex);  % number of manipulated vars (MV)
   nym=length(myindex); % number of measured outputs (MY)
   nv=length(mdindex);  % number of measured disturbances (MD)
   
   % eventually augment the system with unmeasured disturbance model
   [model,unindex,nxnoise,x0]=chknoisemodel(model,noisemodel,unindex,nutot,x0);
   
   %Check covmat, and eventually augment the system to include
   % step-like output disturbances
   [covmat,model,unindex,nxnoise,x0]=chkcovmat(covmat,model,unindex,myindex,nxnoise,ny,nutot,x0);
   
   nd=length(unindex);
   
   A=model.a;
   B=model.b;
   C=model.c;
   D=model.d;
   
   [ny,nutot]=size(D); % Must redefine ny,nutot, model may be changed
   nx=size(A,1);
   
   
   Bu=B(:,mvindex);
   Bv=B(:,mdindex);
   Bd=B(:,unindex);
   Dv=D(:,mdindex);
   Dd=D(:,unindex);
   Cm=C(myindex,:);
   Dvm=Dv(myindex,:);
   
   % Check correctness of prediction horizon p
   p=chkp(p);
   
   % Check correctness of input horizon/blocking moves
   moves=chkm(moves,p);
   
   % Check correctness of weights
   [uwt,duwt,ywt,rhoeps]=chkwghts(weights,p,nu,ny,rhoeps);
   
   % Check correctness of upper and lower bounds
   [umin,umax,dumin,dumax,ymin,ymax]=chklims(limits,nu,ny,p);
   
   if isfinite(dumin(1,1));
      TYPE='Constrained';   % Constrained MPC problem --> need QP
   else
      TYPE='Unconstrained'; % Unconstrained MPC problem --> no need of QP
   end
   
   % Check reference/measured disturbance signals from file
   % and resample
   if ~isempty(filename),
      rv=chkfile(filename,ts,ny,nv);
   else
      rv=[];
   end
   
   
   % Design estimator gain
   L=lgain(model,mvindex,mdindex,unindex,myindex,...
      nym,nd,nx,ny,nutot,covmat,nxnoise);
   
   % Builds optimization matrices for multiparametric QP problem
   [Mat,degrees]=buildmat(A,Bu,Bv,Bd,C,Dv,Dd,uwt,duwt,ywt,...
      umin,umax,dumin,dumax,ymin,ymax,p,moves,...
      mvindex,mdindex,unindex,TYPE,nx,ny,nutot,nu,nv,nd,rhoeps);
   
   optimalseq=zeros(degrees,1);
   
   % Checks previous input and utarget
   u1=chkx0u1(u1,nu,'u(-1)');      
   utarget=chkx0u1(utarget,nu,'u_target');      
   
   utarget=ones(p,1)*utarget'; % extends over prediction horizon
   utarget=utarget';utarget=utarget(:); %Put utarget in one column
   
   lastx=x0;
   lastu=u1;
   lastt=-1;
   
   % Now retrieves MPCblock parameters (this is needed when multiple MPC blocks are used)
   if ~isempty(GCBH),
	   mpc_i=find(GCBH==gc); % Might be already defined because user
   else                     % has stopped simulation before case 9 is executed
      mpc_i=[];
   end
   
   if isempty(mpc_i),
      mpc_i=length(GCBH);
      mpc_i=mpc_i+1;
      GCBH=[GCBH;gc];
   end   
   
   
   % Put all MPC parameters (necessary for Case 3) in a structure
   s=struct('nu',nu,'nv',nv,'nd',nd,'nutot',nutot,'nym',nym,'nx',nx,'ny',ny,...
      'A',A,'Bu',Bu,'Bv',Bv,'Bd',Bd,'Cm',Cm,'Dd',Dd,'Dv',Dv,'Dvm',Dvm,'Du',Du,...
      'TYPE',TYPE,'L',L,'Mat',Mat, 'optimalseq',optimalseq,'utarget',utarget,...
      'degrees',degrees,'delay',delay,'ts',ts,'model',model,...
      'mvindex',mvindex,'mdindex',mdindex,'unindex',unindex,'myindex',myindex,...
      'lastx',lastx,'lastu',lastu,'lastt',lastt,'nxnoise',nxnoise,'x0',x0,'covmat',covmat,...
      'p',p,'moves',moves,'uwt',uwt,'duwt',duwt,'ywt',ywt,...
      'umin',umin,'umax',umax,'dumin',dumin,'dumax',dumax,'ymin',ymin,'ymax',ymax,'u1',u1,...
      'rv',rv,'anticipate',anticipate);
   
   MpcStruct{mpc_i}=s;
   
    
   % S-Function parameters (type HELP SFUNTMPL.M for explainations)
   x0=[];
   str=[];
   %sys=[0 nx+nu nu -1 0 0 1];
   %   Ts=[ts delay];
   sizes=simsizes;
   sizes.NumContStates=0;
   sizes.NumDiscStates=0;
   sizes.NumOutputs=nu;
   sizes.NumInputs=-1;
   sizes.DirFeedthrough=1; % was 0
   sizes.NumSampleTimes=1;
   sys=simsizes(sizes);
   Ts=[ts delay];
   
   %filename='~temp_new';
   % 
   %vars=['nu nv nd nutot nym nx A Bu Bv Bd Cm Dd Dv Du TYPE L Mat'...
   %     ' optimalseq utarget degrees filename delay ts'...
   %     ' model mvindex mdindex unindex lastx lastu lastt'...
   %     ' myindex nxnoise x0 covmat'...
   %     ' p moves uwt duwt ywt umin umax dumin dumax ymin ymax u1'];
   % 
   %eval(['save ' filename ' ' vars])

case 3 % Return outputs      
   
   % Retrieve MPC parameters
   mpc_i=find(GCBH==gc);
   
   MPCS=MpcStruct{mpc_i};
   ts=MPCS.ts;
   delay=MPCS.delay;
   lastu=MPCS.lastu;
   lastt=MPCS.lastt;
   
   % Check if a sampling time has elapsed
   if abs(round((t-delay)/ts)-(t-delay)/ts)<1e-8,  
      % >>>>> INSTEAD OF 1e-8, MAYBE BETTER GET A PARAMETER FROM SIMULINK INTEGRATION PROPERTIES !!!
      tr=round((t-delay)/ts);
      if (abs(tr-(t-delay)/ts)<1e-8) & (tr>lastt), % A sampling time has elapsed
         %disp(['update ' num2str(t)]);
        
         %model=model_;
         %utarget=utarget_;
         %degrees=degrees_;
         %delay=delay_;
         %ts=ts_;
         %p=p_;
         %moves=moves_;
         %covmat=covmat_;
         %x0=x0_;
         %u1=u1_;
         %mvindex=mvindex_;
         %mdindex=mdindex_;
         %myindex=myindex_;
         
         
         % Take back from MpcStruct the values (maybe not all of them
         % are needed...)
         
         nu=MPCS.nu;
         nv=MPCS.nv;
         nd=MPCS.nd;
         nutot=MPCS.nutot;
         nym=MPCS.nym;
         nx=MPCS.nx;
         ny=MPCS.ny;
         
         A=MPCS.A;
         Bu=MPCS.Bu;
         Bv=MPCS.Bv;
         Bd=MPCS.Bd;
         Cm=MPCS.Cm;
         Dd=MPCS.Dd;
         Dv=MPCS.Dv;
         Dvm=MPCS.Dvm;
         Du=MPCS.Du;
         
         TYPE=MPCS.TYPE;
         L=MPCS.L;
         Mat=MPCS.Mat;
         optimalseq=MPCS.optimalseq;
         utarget=MPCS.utarget;
         degrees=MPCS.degrees;
         model=MPCS.model;
         
         mvindex=MPCS.mvindex;
         mdindex=MPCS.mdindex;
         unindex=MPCS.unindex;
         myindex=MPCS.myindex;
         
         lastx=MPCS.lastx;
         nxnoise=MPCS.nxnoise;
         x0=MPCS.x0;
         covmat=MPCS.covmat;
         
         p=MPCS.p;
         moves=MPCS.moves;
         uwt=MPCS.uwt;
         duwt=MPCS.duwt;
         ywt=MPCS.ywt;
         
		 umin=MPCS.umin;
         umax=MPCS.umax;
         dumin=MPCS.dumin;
         dumax=MPCS.dumax;
         ymin=MPCS.ymin;
         ymax=MPCS.ymax;
         u1=MPCS.u1;
         
         rv=MPCS.rv;
         anticipate=MPCS.anticipate;
        
          
         
         l=length(yrd);
         if l<nym,
            error('Wrong number of measured outputs');
         end
         y=yrd(1:nym);
         
         if l==nym, % MPC Regulator -- no r/v specified
            r=zeros(ny,1);
            v=zeros(nv,1);
         else
            nym2=ny+nym;
            if l<nym2,
               errmsg=sprintf('Wrong number of measured outputs (should be %d) or output references (should be %d)',nym,ny);
               error(errmsg);
            end
            r=yrd(nym+1:nym2);
            if l==nym2,
               v=zeros(nv,1);
            else
               nvnym2=nym2+nv;
               if l<nvnym2,
                  error('Wrong number of measured disturbances');
               end
               if l==nvnym2,
                  v=yrd(nym2+1:nvnym2);
               else
                  error('Wrong number of measured outputs, output references, or measured disturbances');
               end
            end
         end
         
         xk1=lastx; % This is xhat(t|t-1)
         uk1=lastu; % This u(t-1)
         
         if isempty(rv), % no signal from file
            r=r*ones(1,p); % extends over prediction horizon % Bug fixed on May 23, 2001
         else
            r=getrv(rv,1:ny,tr,tr+p-1); % Get a window of reference signal
            if ~anticipate,
               r=r(1:ny,1);    % Bug fixed on May 23, 2001
               r=r*ones(1,p);  % Bug fixed on May 23, 2001
            end
         end   
            
         r=r(:);    %Put r in one column % Bug fixed on May 23, 2001
         
         %% Future of the reference
         %ttt=t:ts:t+(p-1)*ts;
         %r=(ttt>=10);
         %r=r(:);
         
         % Same thing for measured disturbances
         if ~isempty(v),
            if isempty(rv), % no signal from file
            	v=v*ones(1,p+1); % extends over prediction horizon % Bug fixed on May 23, 2001
         	else
            	v=getrv(rv,1+ny:ny+nv,tr,tr+p);
         	   if ~anticipate,
            	v=v(1:nv,1);      % Bug fixed on May 23, 2001
               	v=v*ones(1,p+1);  % Bug fixed on May 23, 2001
            	end
         	end   
         
            v=v(:);    %Put v in one column % Bug fixed on May 23, 2001
         else
            v=zeros(p+1,0);
            v=v(:);
         end
         vk=v(1:nv); % saves current v
         
         
         %d=zeros(p+1,nd);%%%% <<<<<<<<<------------------ !!!!!!!!!!!
         %d=d(:);    
         %dk=d(1:nd);
         
         xk=xk1;
         
         % Measurement update of state observer
         yest=Cm*xk+Dvm*vk;%+Dd*dk;
         xk=xk+L*(y-yest); 
         
         
         % Solves MPC optimization problem
         isunconstr=strcmp(TYPE,'Unconstrained');
         
         if ~isunconstr,
            Mlim=Mat.Mlim;
         end
         Kv=Mat.Kv;
         if isempty(Kv),
            vKv=zeros(1,degrees);
            if ~isunconstr,
               Mvv=zeros(size(Mlim));
            end
         else
            vKv=v'*Kv;
            if ~isunconstr,
               Mvv=Mat.Mv*v;
            end
         end
         
         %Kd=Mat.Kd;
         %if isempty(Kd),
         %   dKd=zeros(1,degrees);
         %   if ~isunconstr,
         %      Mdd=zeros(size(Mlim));
         %   end
         %else
         %   dKd=d'*Kd;
         %   if ~isunconstr,
         %      Mdd=Mat.Md*d;
         %   end
         %end
         
         % Solves optimization problem
         if isunconstr,
            [zopt,epsslack,how]=mpc2([],Mat.KduINV,Mat.Kx,Mat.Ku1,Mat.Kut,Mat.Kr,[],[],...
               [],[],[],[],...%[],
               [],[],Mat.Jm,Mat.DUFree,...
               xk,uk1,utarget,r,vKv,...%dKd,
               nu,p,degrees,optimalseq,isunconstr);
            
         else
            [zopt,epsslack,how]=mpc2(Mat.MuKduINV,Mat.KduINV,Mat.Kx,Mat.Ku1,Mat.Kut,Mat.Kr,Mat.zmin,Mat.rhsc0,...
               Mat.Mlim,Mat.Mx,Mat.Mu1,Mvv,...%Mdd,
               Mat.rhsa0,Mat.TAB,Mat.Jm,Mat.DUFree,...
               xk,uk1,utarget,r,vKv,...%dKd,
               nu,p,degrees,optimalseq,isunconstr);
         end
         
         % Compute the current input (previous input + new input increment)
         uk=uk1+zopt(1:nu); % The first delta-u move is always a degree of freedom
         
         %%% BEGIN - Compute Optimal Trajectories
         if 0,
            disp(sprintf('epsslack=%5.4f',epsslack))
                % Compute optimal sequence of inputs U
            duopt=Mat.Jm*zopt;
            for i=1:nu,
               uopt(:,i)=cumsum(duopt(i:nu:p*nu))+uk1(i);
            end
            UU=zeros(p,nutot);
            UU(:,mvindex)=uopt;
            if ~isempty(v),
               UU(:,mdindex)=v(1:p,:);
            end
            UU(:,unindex)=zeros
            % Compute optimal sequence of outputs and states (for v=d=[])
            [yopt,t,xopt]=lsim(model,UU,model.Ts*(0:p-1),xk);
         end
         %%% END - Compute Optimal Trajectories
         
         % Time update of state observer
         xk=A*xk+Bu*uk+Bv*vk;%+Bd*dk;
         
         %% Rebuilds optimalseq from zopt
         %free=find(kron(Mat.DUFree(:),ones(nu,1))); % Indices of free moves
         optimalseq=zopt;%(free);
         
         
         %====>>>>>>>>>>>             <<<<<<<<<==============
         % NOT NEEDED IF lastx, lastu, lastt are STATES !!!!
         %====>>>>>>>>>>>             <<<<<<<<<==============
         % timing to check calc time, changing the iterations
         
         %====>>>>>>>>>>>             <<<<<<<<<==============
         
         MPCS.lastx=xk;
         MPCS.lastu=uk;
         MPCS.lastt=lastt+1;
         MpcStruct{mpc_i}=MPCS;
         
         u=uk;
      else
         u=lastu;
      end
   end  
   
   sys=u; % was sys=u
      
   %eval(['load ' filename ' nx nu mvindex']);
   %u=x(nx+1:nx+nu);
   %sys=u(mvindex);  
   
   %eval(['load ' filename ' nx nu']);
   %% Return last computed input (only MV components)      
   %sys=x(nx+2:nx+nu+1);
   
case 2
   % Case 2 doesn't work, because case 3 is executed before case 2 !
   % For instance, at time t=0 first is executed case 3, then case 2
   % Therefore, the input u(0) is not u(0) but u(-1). In general,
   % one would have u(t-1) instead of u(t) as an output
   
case 9 % End
   mpc_i=find(GCBH==gc);
   ikeep=[1:mpc_i-1,mpc_i+1:length(GCBH)];
   if ~isempty(ikeep), 
      MpcStruct=MpcStruct{ikeep};
   else
      MpcStruct={};
   end
   GCBH(mpc_i)=[];
end

%%%%%%%%%%%%%
% Functions %
%%%%%%%%%%%%%

%%%%%%%%%%%%
% GETADD   %
%%%%%%%%%%%%

function [utarget,mvindex,mdindex,myindex,noisemodel,covmat,...
            rhoeps,x0,u1,ts,delay]=getadd(MPCadd);
   
%   		MPCadd.utarget:   target for the input
%   		MPCadd.mvindex:   vector of indices of manipulated variables (within input vector)
%   		MPCadd.mdindex:   vector of indices of measured disturbances (within input vector)
%   		MPCadd.myindex:   vector of indices of measured outputs (within output vector)
%   		MPCadd.noisemodel:LTI model for estimating and predicting the unmeasured disturbance
%   		MPCadd.covmat:    covariance matrices for state-estimator
%   		MPCadd.rhoeps:    weight on slack variable for soft constraints
%   		MPCadd.x0:        initial state estimate xhat(0)
%   		MPCadd.u1:        initial last input u(-1)
%   		MPCadd.ts:        MPC controller sample time (if empty, inherited from model)
%        MPCadd.delay:     simulated computational delay

fields={'utarget','mvindex','mdindex','myindex','noisemodel',...
        'covmat','rhoeps','x0','u1','ts','delay'};

if ~isempty(MPCadd),
   if ~isa(MPCadd,'struct'),
      error('The additional parameters must be a structure');
   else
		s=lower(fieldnames(MPCadd)); % get field names
      for i=1:length(s),
         name=s{i};
         j=find(ismember(fields,name)); % locate name within 'fields'
         if isempty(j), % field inexistent
            error(['The field ''' name ''' in the additional parameter structure is invalid']);
         else
            aux=fields{j};
            eval([aux '=MPCadd.' name ';']); % assign variable 
         end
      end
   end
end
         
% set defaults

% default values:
%		utarget,mvindex,mdindex,myindex,noisemodel,rhoeps,x0,u1,ts=[];
%		covmat={};
%		delay=0; 

for i=1:length(fields),
   eval(['j=exist(''' fields{i} ''');']); % check if variable has been defined
   if ~j,
      switch fields{i}
      	case 'covmat'
            covmat={};
         case 'delay'
            delay=0;
         otherwise
            eval([fields{i} '=[];']);
      end
   end   
end      

%end getadd


%%%%%%%%%%%%
% CHKMODEL %
%%%%%%%%%%%%

function [newmodel,newts,newx0]=chkmodel(model,ts,x0,u1)

% check if 'model', 'ts', 'x0' are ok

if ndims(ts)>2 | ~isreal(ts) | ~isfinite(ts) | length(ts)~=1,
   error('Sample time must be a real positive number or empty.')
elseif ts<=0,
   error('Sample time cannot be less than or equal to 0');
end

if ~isa(model,'lti'),
   error('Model is not an LTI object.')
end

if ~isa(model,'ss'),
   disp('Warning: converting model to state-space.')
   model=ss(model);
   if norm(x0)>0, % note that when x0=[], then norm(x0)=0
      disp('Warning: You have specified the initial state xhat(0), but model is not SS.')
      disp('         Assuming zero initial states');
   end   
   x0=zeros(size(model.b)); % Assumes zero initial condition
else
   % Check consistency of x0
   nx=size(model.a,1);
   x0=chkx0u1(x0,nx,'xhat(0)');
end   

% Delete input/output names, to avoid conflicts when augmenting the
% plant without adding names for new inputs/outputs

set(model,'InputName',{''},'OutputName',{''});


mts=model.Ts;
if mts==0,
   if isempty(ts),
      error('Cannot build an MPC controller. No sample time specified') 
   end
   disp('Warning: converting model to discrete time');
   [model,TRmat]=c2d(model,ts);    % Note: UserData field is lost during conversion !!!
   % TRmat maps continuous initial conditions
   % into discrete initial conditions.  Specifically, if x0,u0 
   % are initial states and inputs for continuous time model, 
   % then equivalent initial conditions for discrete model 
   % are given by  xd0 = TRmat * [x0;u0],  ud0 = u0 .
   
   newts=ts;
   
   if ~norm(u1)>0,
      x0=TRmat(:,1:size(model.A,1))*x0; % u1 might be empty
   else
      x0=TRmat*[x0;u1];
   end
   %if norm(x0)>0 & norm(u1)>0 & norm(TRmat(:,nx+1:size(TRmat,2)))>0,
   %   % Usually this never happens
   %   disp('Warning: initial state information might be wrong')
   %   disp('         because of continuous->discrete conversion.')
   %end

elseif isempty(ts) | abs(ts-mts)<=1e-10,
   newts=mts; % inherit controller sample time from model
      
elseif abs(ts-mts)>1e-10, % ts different from mts
   disp('Warning: resampling model to controller''s sampling time');
   model=d2d(model,ts);
   %takes out possible imaginary parts << Not needed (Nigel, Aug 10 2000)
   %set(model,'a',real(model.a),'b',real(model.b));
   newts=ts;
   % Initial state information should still be consistent << YES (Nigel, Aug 10 2000)
   %x0=zeros(size(model.a,1));
end

% Eventually transforms I/O delays into poles in z=0

%% Solaris running 5.3 beta 2 or PC running 5.3 beta 3,4
%if strcmp('5.3.0.11306a (R11) Beta 2',version)|...
%      strcmp('5.3.0.24664 (R11) Beta 3',version)|...
%      strcmp('5.3.0.35242b (R11) Beta 4',version),
%   if any(model.InputDelay)|any(model.OutputDelay)|any(model.ioDelayMatrix),   
%      %model=d2d(model,'nodelay');
%      model=delay2z(model);
%   end
%end
% 
%% PC running 5.3 beta 1
%if strcmp('5.3.0.7138 Beta 1',version),
%   % DO NOTHING, as model.Td is nonzero only for continuous models, and
%   % conversion of delays to poles in z=0 has been done by C2D
%end


%if any(model.InputDelay)|any(model.OutputDelay)|any(model.ioDelayMatrix),   
if hasdelay(model),
    %model=d2d(model,'nodelay');
    disp('Warning: converting delays to zeros')
    model=delay2z(model);
    % Pad x0 with zeros <<<<<<<<< CHECK !!!!
    % Only works for SISO !!!
    x0=[x0(:);zeros(size(model.A,1)-length(x0),1)];
    if norm(u1)>0,
        disp('Warning: Initial state might be wrong, because u(t-1) is nonzero.')
    end   
end

% Balancing

%try % try fails if model is not stable 
%   % Use BALREAL + MODRED
%   [bmodel,Gmodel,Tx]=balreal(model);
%	x0=Tx*x0; % Compatible initial state
%
%	% Use MODRED
%	modredtol=1e-4;
%	maxG=Gmodel(1); % Largest element (Gmodel is ordered)
%	ii=find(Gmodel<maxG*modredtol);
%	if ~isempty(ii),
%  	model=modred(bmodel,ii); % Get rid of those states
%		x0=x0(1:ii(1)-1); % get rid also of those corresponding initial states
%	end
%catch
%
% >>>  BALANCE is not good !!! Because it modifies b,c matrices, and therefore
% >>>  weights, noise covariance matrices, and indices can mess up !!!
%
%   % USE BALANCE
%	[Tx,Bx]=balance(model.a);
%	model.a=Bx;
%	model.b=Tx\model.b;
%	model.c=model.c*Tx;
%	x0=Tx\x0;  % so that, for instance y(0)=Cx(0)=C*TX*inv(TX)*x0=C*TX*z0
%end

% USE MINREAL
modredtol=1e-5; % default is sqrt(eps)=1.5e-8
[model,Tx]=minreal(model,modredtol);
x0=Tx*x0; % Tx is always a square matrix, dim(Tx)=dim(original state)
x0=x0(1:size(model.a,1)); % Thanks to Nigel, 03-08-00


newmodel=model;
newx0=x0;

%%%%%%%%%%%%%%%%%
% CHKNOISEMODEL %
%%%%%%%%%%%%%%%%%

function [newmodel,newunindex,nxnoise,newx0]=chknoisemodel(...
   model,noisemodel,unindex,nutot,x0);

% check for noisemodel and eventually augment the system

if isempty(noisemodel) 
   if isempty(unindex),  
      % No disturbance model => no need to augment the system
      newmodel=model;
      newunindex=[];
      nxnoise=0;
      newx0=x0;
      return
   else
      % Default: an integrator for each unmeasured disturbance, i.e.
      %          step-like unmeasured disturbances
      nun=length(unindex);
      noisemodel=ss(zeros(nun,nun),eye(nun),eye(nun),zeros(nun,nun));
      
      %% Default: a random noise with unit covariance matrix
      %%          for each unmeasured disturbance
      %nun=length(unindex);
      %noisemodel=tf(eye(nun));
   end
end

if ~isa(noisemodel,'lti'),
   error('Unmeasured Disturbance Model is not an LTI object.')
end

if ~isa(noisemodel,'ss'),
   noisemodel=ss(noisemodel);
end

Cbar=noisemodel.c;
nxnoise=size(Cbar,2); % Number of states in NOISEMODEL, which will
% be appended below the states of MODEL

nun=length(unindex);
if nun~=size(Cbar,1),
   error(sprintf('Unmeasured disturbance model should have %d outputs',nun))
end

ts=model.Ts;
nts=noisemodel.Ts;
if nts==0,
   noisemodel=c2d(noisemodel,ts);    % Note: UserData field is lost during conversion !!!
   
elseif abs(ts-nts)>1e-10, % ts different from nts
   disp('Warning: resampling unmeasured disturbance model to controller''s sampling time');
   noisemodel=d2d(noisemodel,ts);
   %takes out possible imaginary parts
   set(noisemodel,'a',real(noisemodel.a),'b',real(noisemodel.b));
end

% Augment the system
A=model.a;
B=model.b;
D=model.d;
Bd=B(:,unindex);
Dd=D(:,unindex);
[nx,nutot]=size(B);

Bbar=noisemodel.b;
Dbar=noisemodel.d;
[nxbar,nubar]=size(Bbar); % nxbar=nxnoise

% Initialize new matrices B,D
newb=[B;zeros(nxbar,nutot)]; % Just adds zeros
newd=D;

if nun==nubar, % noisemodel is square, so simply replace d(t) with n(t)
   
   newb(:,unindex)=[Bd*Dbar;Bbar];
   newd(:,unindex)=[Dd*Dbar];
   
elseif nun>nubar,  % noisemodel is not square, must update also unindex
   
   % n(t) has dimension lower than d(t). 
   keep=unindex(1:nubar); 
   cut=unindex(nubar+1:nun);
   
   newb(:,keep)=[Bd*Dbar;Bbar];
   newb(:,cut)=[];
   newd(:,keep)=[Dd*Dbar];
   newd(:,cut)=[];
   unindex=keep;
   
else %if nun<nubar    % noisemodel is not square, must update also unindex
   
   % n(t) has dimension greater than d(t). 
   B2=[Bd*Dbar;Bbar];
   D2=[Dd*Dbar];
   
   newb(:,unindex)=B2(:,1:nun);   % d(t) is replaced by the first nun components
   % of n(t),
   newb=[newb,B2(:,nun+1:nubar)]; % the last (nubar-nun) components of n(t)
   % are appended at the end of u(t)
   
   newd(:,unindex)=D2(:,1:nun);   % idem
   newd=[newd,D2(:,nun+1:nubar)]; % idem   
   
   unindex=[unindex,nutot+(1:nubar-nun)];
end

set(model,'a',[A, Bd*Cbar;zeros(nxbar,nx), noisemodel.a],'b',newb,...
   'c',[model.c,Dd*Cbar],'d',newd);


newmodel=model;
newunindex=unindex;
newx0=[x0;zeros(nxbar,1)]; % Appends 0 to x0

% end of chkmodel

%%%%%%%%%%%%
% CHKINDEX %
%%%%%%%%%%%%


function	[newmvindex,newmdindex,newmyindex,unindex]=chkindex(mvindex,...
   mdindex,myindex,nutot,ny);

% Check if mvindex,mdindex,myindex are ok, and build unindex

mdindex=sort(mdindex(:));
if min(mdindex)<0 | max(mdindex)>nutot,
   error('Wrong indices for measured disturbances.')
end
mdindex(find(diff(mdindex)==0))=[]; % takes out duplicates (same as UNIQUE.M)

% Set default for mvindex: if empty, then all inputs that are not in mdindex
% are controls
if isempty(mvindex),
   mvindex=setdiff(1:nutot,mdindex);
end

mvindex=sort(mvindex(:));
if min(mvindex)<0 | max(mvindex)>nutot,
   error('Wrong indices for manipulated variables.')
end
mvindex(find(diff(mvindex)==0))=[]; % takes out duplicates (same as UNIQUE.M)


if ~isempty(intersect(mvindex,mdindex)),
   error('Manipulated variables and measured disturbances have common indices.')
end

% Set default for myindex: if empty, then all outputs are measured
if isempty(myindex),
   myindex=1:ny;
end

myindex=sort(myindex(:));
if min(myindex)<0 | max(myindex)>ny,
   error('Wrong indices for measured outputs.')
end
myindex(find(diff(myindex)==0))=[]; % takes out duplicates (same as UNIQUE.M)

unindex=setdiff(1:nutot,[mvindex' mdindex']);

newmvindex=mvindex';
newmdindex=mdindex';
newmyindex=myindex';

% end of chkindex

%%%%%%%%%%%%
% CHKP     %
%%%%%%%%%%%%

function newp=chkp(p)

% checks if p is ok.

if ~isa(p,'double'),
   error('P must be a real greater or equal 1.')
end
if isempty(p),
   disp('Warning: no value for P supplied. Trying P=10.')
   newp=10;
   return
elseif ~isfinite(p), %p=Inf
   %%%<<<<<<<<<<<<<<<<<<<----------------!!!!!!!!!!!!!!!!!!!!
   error('Infinite prediction horizon not implemented yet.')
else
   [nrow,nb]=size(p);
   if nrow*nb~=1 | any(p<1), 
      error('P must be a real greater or equal 1.')
   end
end
newp=p;

% end chkp

%%%%%%%%%%%%
% CHKM     %
%%%%%%%%%%%%

function newmoves=chkm(moves,p)

% checks if moves is ok.

if ~isa(moves,'double'),
   error('M must be a scalar or an array');
elseif isempty(moves)
   newmoves=ones(1,p);
   nb=p;
else
   [nrow,nb]=size(moves);
   if nrow>nb, 
      moves=moves';
      [nrow,nb]=size(moves);
   end
   if nrow ~= 1 | nb < 1
      error('M vector is wrong size');
   end
   if any(moves < 1)
      error('M contains an element that is < 1');
   end
   
   if nb == 1
      
      %  This section interprets "moves" as a number of moves, each
      %  of one sampling period duration.
      
      if moves > p
         disp('Warning: M > P.  Truncated.')
         disp(' ')      
         
         moves=p;
      elseif moves <= 0
         disp('Warning: M <= 0.  Set M = 1.')
         disp(' ')
         moves=1;
      end
      
   else
      
      % This section interprets "moves" as a vector of blocking factors.
      summoves=sum(moves);
      if summoves > p
         disp('Warning: sum(M) > P. Moves will be truncated at P.')
         disp(' ')
         nb=find(cumsum(moves) > p);
         nb=nb(1);
         moves=moves(1,1:nb-1);
         moves(nb)=p-sum(moves);
         if nb==1,    % If the vector has only one component,
            % than number of moves=1
            moves=1;
         end
      elseif summoves < p
         nb=nb+1;
         moves(nb)=p-summoves;
         %disp('Warning: sum(M) < P.  Will extend to P.')
         %disp(' ')
      end
   end
   newmoves=moves;   
end

% end chkm

%%%%%%%%%%%%
% CHKWGHTS %
%%%%%%%%%%%%

function [uwt,duwt,ywt,rhoeps]=chkwghts(weights,p,nu,ny,rhoeps);

weights{4}=[]; % Augment weights so that has length >3

if isempty(weights{1}),
   weights{1}=zeros(1,nu); % Default for uwt. See also chkwghts.m
end   

if isempty(weights{2}),
   weights{2}=0.1*ones(1,nu); % Default for duwt. See also chkwghts.m
end   

if isempty(weights{3}),
   weights{3}=ones(1,ny); % Default for ywt
end

uwt=weights{1};
duwt=weights{2};
ywt=weights{3};

[nrow,ncol]=size(uwt); 
if ncol ~= nu | nrow <= 0    
   error('UWeight is wrong size'); 
end 

if any(any(uwt < 0))    
   error('One or more elements of UWeight are negative'); 
end 
[nrow,ncol]=size(duwt); 
if ncol ~= nu | nrow <= 0    
   error('DUWeight is wrong size'); 
end 
if any(any(duwt < 0))    
   error('One or more elements of DUWeight are negative'); 
end 
[nrow,ncol]=size(ywt); 
if ncol ~= ny | nrow <= 0    
   error('YWeight is wrong size'); 
end 
if any(any(ywt < 0))    
   error('One or more elements of YWeight are negative'); 
end 
if isempty(rhoeps),
   % Set default value for rhoeps
	rhoeps=1e5*max([uwt(:);duwt(:);ywt(:)]); % Weight on slack variable
else 	                                      % for soft output constraints
   if ~isa(rhoeps,'double'),
      error('RhoEps must be a scalar');
   else
      if length(rhoeps)>1,
         warning(sprintf('RhoEps has length %d, taking RhoEps=RhoEps(1)=%f', ...
                  length(rhoeps), rhoeps(1)));
         rhoeps=rhoeps(1); % Just in case rhoeps were a vector
      end         
   end   
end

%end chkwght

%%%%%%%%%%%%
% CHKLIMS  %
%%%%%%%%%%%%

function [umin,umax,dumin,dumax,ymin,ymax]=chklims(limits,nu,ny,p);

limits{7}={}; % To have length(limits)>=6

if isempty(limits{1}),
   limits{1}=-Inf*ones(1,nu); % Default for umin
end
if isempty(limits{2}),
   limits{2}=Inf*ones(1,nu); % Default for umax
end
if isempty(limits{5}),
   limits{5}=-Inf*ones(1,ny); % Default for ymin
end
if isempty(limits{6}),
   limits{6}=Inf*ones(1,ny); % Default for ymax
end

umin=limits{1};
umax=limits{2};
dumin=limits{3};
dumax=limits{4};
ymin=limits{5};
ymax=limits{6};

[errmsg,umin,umax]=onlims('U',umin,umax,nu,p);
error(errmsg);

[errmsg,ymin,ymax]=onlims('Y',ymin,ymax,ny,p);
error(errmsg);

% Check if the problem has any bounded constraint on u,du,y or not.
% If it has, default lim for du is 1e6, otherwise Inf

% finite values in dumin, dumax which occur after p steps
% are not considered as an indicator of constrained MPC

dumin2=dumin;
if size(dumin,1)>p,
   dumin2=dumin(1:p,:);
end
dumax2=dumax;
if size(dumax,1)>p,
   dumax2=dumax(1:p,:);
end

if any(isfinite(umin(:))) | any(isfinite(umax(:))) | ...
      any(isfinite(dumin2(:))) | any(isfinite(dumax2(:))) | ...
      any(isfinite(ymin(:))) | any(isfinite(ymax(:))),
   big=1e6;
else
   big=Inf;
end

if isempty(dumin),
   dumin=-big*ones(1,nu); % Default for dumin
end
if isempty(dumax),
   dumax=big*ones(1,nu); % Default for dumax
end

[errmsg,dumin,dumax]=onlims('DU',dumin,dumax,nu,p);
error(errmsg);

% When using the DANTZGMP routine for the QP problem, we must have all
% lower bounds on delta u finite.  A bound that is finite but large can cause
% numerical problems. The following loop checks for this.

ifound=find(dumin<-1e6 & isfinite(big));
if ~isempty(ifound) & ~isempty(find(isfinite(dumin(ifound)))),
   disp('Warning: One or more constraints on DELTAU were < -1e-6.')
   disp('         Modified to prevent numerical problems in QP.')
   disp(' ')      
end
dumin(ifound)=-1e6;

% NOT NEEDED: Upper bound on Delta u can be arbitrary large
%
%ifound=find(dumax>1e6 & isfinite(big));
%dumax(ifound)=1e6;
%if ~isempty(ifound),
%   disp('Warning: One or more constraints on DELTAU were > 1e-6.')
%   disp('         Modified to prevent numerical problems in QP.')
%   disp(' ')      
%end

%end chklims

%%%%%%%%%%%%
% ONLIMS   %
%%%%%%%%%%%%

function [errmsg,newamin,newamax]=onlims(a,amin,amax,Na,p)
% Determine validity of upper and lower bound matrices

errmsg='';
newamin=amin; % Otherwise if an error occurs amin is not defined
newamax=amax; % Otherwise if an error occurs amin is not defined

[nrow,ncol]=size(amin);
if ncol ~= Na, 
   errmsg=[a 'Min matrix is wrong size. Should have ' ...
         int2str(Na) ' columns, not ' int2str(ncol)];
   return
end

if nrow>p         % Has user specified more bounds than necessary?
   disp(['Warning:  too many rows in ' a 'Min matrix. Extra rows deleted.'])
   disp(' ')
   newamin=amin(1:p,:);
elseif nrow==1,
   newamin=amin;
else
   % If fewer rows than needed, must copy last one, unless nrow=1.
   newamin=[amin;kron(ones(p-nrow,1),amin(nrow,:))];    
end

[nrow,ncol]=size(amax);
if ncol ~= Na
   errmsg=[a 'Max matrix is wrong size. Should have ' ...
         int2str(Na) ' columns, not ' int2str(ncol)];
   return
end

if nrow>p         % Has user specified more bounds than necessary?
   disp(['Warning:  too many rows in ' a 'Max matrix. Extra rows deleted.'])
   disp(' ')
   newamax=amax(1:p,:);
elseif nrow==1,
   newamax=amax;
else
   % If fewer rows than needed, must copy last one, unless nrow=1.
   newamax=[amax;kron(ones(p-nrow,1),amax(nrow,:))];    
end

if any(any(newamax<=newamin))
   errmsg=['A lower bound on ' a ' was not less than its upper bound'];
   return
end   

%end onlims

%%%%%%%%%%%%
% BUILDMAT %
%%%%%%%%%%%%

function [Mat,degrees]=buildmat(A,Bu,Bv,Bd,C,Dv,Dd,uwt,duwt,ywt,umin,umax,dumin,dumax,ymin,ymax,p,m,...
   mvi,mdi,uni,TYPE,nx,ny,nutot,nu,nv,nd,rhoeps)

% Builds optimization matrices (constrained) / gain controllers (unconstrained)

[KduINV,Kx,Kr,Ku1,Kv,Kd,Kut,Jm,DUFree,TAB,MuKduINV,rhsa0,rhsc0,...
      Mlim,Mx,Mu1,Mv,Md,zmin,... 
      degrees]=mpc1(...
   A,Bu,Bv,Bd,C,Dv,Dd,uwt,duwt,ywt,umin,umax,dumin,dumax,ymin,ymax,p,m,...
   mvi,mdi,uni,TYPE,nx,ny,nutot,nu,nv,nd,rhoeps);


if strcmp(TYPE,'Unconstrained'),
   Mat=struct('KduINV',KduINV,'Kx',Kx,'Kr',Kr,...
      'Ku1',Ku1,'Kv',Kv,'Kd',Kd,'Kut',Kut,'Jm',Jm,'DUFree',DUFree);
else
   Mat=struct('KduINV',KduINV,'Kx',Kx,'Kr',Kr,'Ku1',Ku1,'Kv',Kv,'Kd',Kd,'Kut',Kut,...
      'Jm',Jm,'DUFree',DUFree,'TAB',TAB,'MuKduINV',MuKduINV,'rhsa0',rhsa0,'rhsc0',rhsc0,...
      'Mlim',Mlim,'Mx',Mx,'Mu1',Mu1,'Mv',Mv,'Md',Md,'zmin',zmin);
end

%end buildmat

%%%%%%%%%%%%
% CHKX0U1  %
%%%%%%%%%%%%

function [newx]=chkx0u1(x,nx,name)

% checks if x0, u1, or utarget is ok.

if ~isa(x,'double'),
   error([name ' must a real valued vector.']);
end
if isempty(x),
   newx=zeros(nx,1);
   return
end

x=x(:);
l=length(x);
if l~=nx,
   error(sprintf('%s must be a vector of length %d.',name,nx));
end
newx=x;

%end chkx0u1

%%%%%%%%%%%%
% CHKDELAY %
%%%%%%%%%%%%
function newdelay=chkdelay(delay,ts)

% checks if delay is ok.
msg='delay must satisfy 0 <=delay < Ts';

if ~isa(delay,'double'),
   error(msg);
end
if isempty(delay),
   newdelay=0;
   return
end

[l1,l2]=size(delay);
if l1*l2~=1,
   error(msg);
end

if delay<0 | delay >=ts,
   error(msg);
end

newdelay=delay;

%end chkdelay

%%%%%%%%%%%%%
% CHKCOVMAT %
%%%%%%%%%%%%%
function [newcovmat,newmodel,newunindex,newnxnoise,newx0]=...
   chkcovmat(covmat,model,unindex,myindex,nxnoise,ny,nutot,x0)

% Set defaults

nym=length(myindex);
nd=length(unindex);
A=model.a;
nx=size(A,1);

if nd==0,
   def1=1;
   def2=1;
else
   def1=.001;
   def2=.1;
end

% Check if covmat is correct

if ~isa(covmat,'cell'),
   error('COVMAT must be a cell array')
end

if isempty(covmat),
   
   % By default, step-like disturbances are added on all outputs,
   % the output white noise is set to 0, all other covariance
   % matrices are set to identity
   
   %S1=def1*eye(nx);
   %S2=def2*eye(nym);
   
   % Add nym unmeasured disturbances, one for each output
   %
   % x(k+1) = Ax(k)+Bu(k)
   % z(k+1) = z(k)+Ts*d(k)
   %   y(k) = Cx(k)+Du(k)+z(k)
   
   B=model.b;
   C=model.c;
   D=model.d;
   Ts=model.ts;
   
   %A=[A,zeros(nx,nym);zeros(nym,nx),eye(nym)]; % Pure integrator
   A=[A,zeros(nx,nym);zeros(nym,nx),.999*eye(nym)]; % To prevent numerical
   %                                                 % problems in KALMAN.M
   
   B=[B,zeros(nx,nym);zeros(nym,nutot),Ts*eye(nym)];
   aux=eye(ny);
   uyindex=setdiff(1:ny,myindex); % gets unmeasured outputs
   aux(uyindex,uyindex)=aux(uyindex,uyindex)*0;
   aux(:,uyindex)=[]; 
   C=[C,aux];
   D=[D,zeros(ny,nym)];
   
   set(model,'A',A,'B',B,'C',C,'D',D);
   
   newmodel=model;
   newunindex=[unindex(:);nutot+(1:nym)']';
   newnxnoise=nxnoise+nym; % There are new nym states for unmeasured disturbance model
   newx0=[x0;zeros(nym,1)];
   
   %S1=def1*eye(nx-nxnoise);
   S1=def1*eye(nx+nym);
   S2=def2*eye(nym);
   
else
   % Don't change model,unindex,x0
   newmodel=model;
   newunindex=unindex;
   newnxnoise=nxnoise;
   newx0=x0;
   
   if length(covmat)~=2,
      error('COVMAT must have two elements or be an empty cell array')
   else
      %nx=nx-nxnoise;
      S1=covmat{1};
      [n,m]=size(S1);
      if n*m==0,
         S1=def1*eye(nx);
      elseif n*m==1,
         S1=S1*eye(nx); % scalar number transformed in diagonal matrix
      elseif ~(n==nx & m==nx),
         error(sprintf('COVMAT{1} must be %d-by-%d or scalar',nx,nx))
      end
      
      
      S2=covmat{2};
      [n,m]=size(S2);
      if n*m==0,
         S2=def2*eye(nym);
      elseif n*m==1,
         S2=S2*eye(nym); % scalar number transformed in diagonal matrix
      elseif ~(n==nym & m==nym),
         error(sprintf('COVMAT{2} must be %d-by-%d or scalar',nym,nym))
      end
   end
end
newcovmat.S1=S1;
newcovmat.S2=S2; % must save a struct otherwise MpcStruct becomes 2-dimensional !

%%%%%%%%%%%%
% CHKFILE  %
%%%%%%%%%%%%

function z=chkfile(filename,ts,ny,nv);

if ~exist(filename,'file'),
   error('Reference/Measured Disturbance file not found');
end

s=load(filename);
names=fieldnames(s);
trv=getfield(s,names{1}); % Gets the value of the first variable

if ~isa(trv,'double'),
   error('The Reference/Measured Disturbance signals are not in the form [time;r;v]');
end

% Checks if all signals have been specified, and eventually put zeros
% where missing

[n,m]=size(trv);
if n<1,
   warning(['No signal specified in ' filename ' --- Assuming 0']);
   trv=[0,ts;zeros(ny,2);zeros(nv,2)];   
else
	if n<nv+ny+1,
      addv=zeros(0,m);
      if nv>0,
			warning(['Not enough measured disturbance signals specified in ' filename ' --- Assuming 0']);
         addv=zeros(min(nv,nv+ny+1-n),m); 
      end
      addr=zeros(0,m);
      if n<ny+1,
		   warning(['Not enough reference signals specified in ' filename ' --- Assuming 0']);
   		addr=zeros(ny+1-n,m);
      end
	   trv=[trv;addr;addv];
   end
end  


% Resample

time=trv(1,:)/ts;
zorig=trv(2:end,:);

z=[];
lastt=0;
i=1;
t2=time(1);
t1=t2;
z2=zorig(:,1);

% Fill t up to time(1) (included)
while lastt<=time(1),
   z=[z,z2];
   lastt=lastt+1;
end

lent=length(time);

while i<=lent-1 & lastt<time(lent),
   while t2<lastt,
      t1=t2;
      z1=z2;
      t2=time(i+1);
      i=i+1; % takes next sample
   end   
   z2=zorig(:,i);
   z1=zorig(:,i-1);
   while lastt<=t2,
	   znew=z2+(z1-z2)/(t1-t2)*(lastt-t2); % Linear interpolation
   	z=[z,znew];
      lastt=lastt+1;
   end
end  

if lastt-1<time(lent),
   z1=zorig(:,lent-1);
   z2=zorig(:,lent);
   t1=time(lent-1);
   t2=time(lent);
	znew=z2+(z1-z2)/(t1-t2)*(lastt-t2); % Linear interpolation
	z=[z,znew];
end   
   
%t=0:lastt;
   
%figure(1);
%plot(time,zorig,time,zorig,'o',t,z,'*')
   
   
%end chkfile

%%%%%%%%%%%%
% GETRV    %
%%%%%%%%%%%%

function z=getrv(rv,indices,t1,t2);
%Gets trv(indices,t1+1:t2+1) if trv has enough columns, otherwise
%repeats the last column

len=size(rv,2);
if t1+1>len,
   z=rv(indices,len)*ones(1,t2-t1+1); % repeats the last one
elseif t2+1>size(rv,2),
   z=[rv(indices,t1+1:len),rv(indices,len)*ones(1,t2+1-len)];
else
   z=rv(indices,t1+1:t2+1);
end
   
   

%end getrv

%%%%%%%%%%%%
% LGAIN    %
%%%%%%%%%%%%

function L=lgain(model,mvindex,mdindex,unindex,myindex,nym,nd,nx,ny,nutot,covmat,nxnoise);

% Approach #1
% -----------
% assume a fake input disturbance on all states

%Nym=length(stored.MYIndex);
%Nx=stored.Nx;
%Ny=stored.Ny;
%set(model,'b',[model.b,eye(Nx)],'d',[model.d,zeros(Ny,Nx)]);
%[dummy,L]=kalman(model,eye(Nx),eye(Nym));  

% Approach #2
% -----------
% just considers the unmeasured disturbance (doesn't work if nd=0)

%[dummy,L]=kalman(model,eye(nd),.1*eye(nym),[],myindex,[mvindex,mdindex]);
%%[dummy,L]=kalman(model,eye(nd),.1*eye(nym),[],myindex,[mvindex,mdindex,linindex]);

% Approach #3
% -----------
% More robust. Considers both

% Approach #4
% -----------
% Wild!
%
%[dummy,L]=kalman(model,eye(nutot),100*eye(nym),[],myindex);

%if isempty(estspeed),
%   estspeed=.3;
%else
%	estspeed=min(max(estspeed,0),1); % truncate within 0,1
%end   
%
%speed=10^(-(1-estspeed)*14+4);

%% Add extra noise: xi(t) on all state components, z(t) on all measured outputs
%
%set(model,'b',[model.b,eye(nx)],'d',[model.d,zeros(ny,nx)]);
%
%[dummy,L]=kalman(model,diag([ones(1,nd),speed*ones(1,nx)]),.1*eye(nym),...
%   [],... %or --- instead of [] --- zeros(nd+nx,nym),...
%   myindex,[mvindex,mdindex]);

nxmodel=nx-nxnoise;

% Extends model to form Kalman filter. 

%if 1, % KALMAN.M
%	% Add extra noise: xi(t) on the state components of the model
%	% z(t) on all measured outputs
%   
%   set(model,'b',[model.b,[eye(nxmodel);zeros(nxnoise,nxmodel)]],...
%      'd',[model.d,zeros(ny,nxmodel)]);
%   
%   [dummy,L]=kalman(model,[eye(nd),zeros(nd,nxmodel);zeros(nxmodel,nd),covmat{1}],...
%      covmat{2},[],... %or --- instead of [] --- zeros(nd+nxmodel,nym),...
%      myindex,[mvindex,mdindex]);
%else 
%   % Use DAREITER.M instead. However, this set E{wz}=0, therefore
%   % direct feedthrough from white noise to output is not allowed,
%   % i.e. model.d * noisemodel.d is considered 0.
%   
%   b=model.b;
%  gamw=[b(:,unindex),[eye(nxmodel);zeros(nxnoise,nxmodel)]];
%   q=[eye(nd),zeros(nd,nxmodel);zeros(nxmodel,nd),covmat{1}];
%   r=covmat{2};
%   c=model.c;
%   c=c(myindex,:);
%   
%   % From DLQE2.M 
%   m=dareiter(model.a',c',r,gamw*q*gamw');
%   k=(r+c*m*c')\(m*c'); % Was: k=m*c'*inv(r+c*m*c');
%   p=m-k*c*m;
%   
%   L=k;
%end
   
% Add extra noise: xi(t) on the all state components (model+noisemodel),
% z(t) on all measured outputs
   
set(model,'b',[model.b,eye(nx)],'d',[model.d,zeros(ny,nx)]);

% Use KALMAN.M
warn_status=warning; % Turns off possible warnings in KALMAN.M
warning off
[dummy,L]=kalman(model,[eye(nd),zeros(nd,nx);zeros(nx,nd),covmat.S1],...
   covmat.S2,[],... %or --- instead of [] --- zeros(nd+nxmodel,nym),...
   myindex,[mvindex,mdindex]);
warning(warn_status)

%% Use DLQE2, DAREITER. However, this set E{wz}=0, therefore
%% direct feedthrough from white noise to output is not allowed,
%% i.e. model.d * noisemodel.d is considered 0.
%
%b=model.b;
%gamw=[b(:,unindex),[eye(nxmodel);zeros(nxnoise,nxmodel)]];
%q=[eye(nd),zeros(nd,nx);zeros(nx,nd),covmat.S1];
%r=covmat.S2;
%c=model.c;
%c=c(myindex,:);
%
%% From DLQE2.M 
%m=dareiter(model.a',c',r,gamw*q*gamw');
%k=((r+c*m'*c')\(c*m'))'; % Was: k=m*c'*inv(r+c*m*c');
%p=m-k*c*m;
%   
%L=k;

%end lgain


