image thumbnail

MHE with cEKF : 4 cylindrical tanks

by

 

15 May 2007 (Updated )

Moving Horizon Estimator (MHE) and cEKF applied to 4 cylindrical tanks

CEKF(t,x,u,flag, parcekf, parpro)
function [sys,x0,str,ts] = CEKF(t,x,u,flag, parcekf, parpro)
%
% ##################         cEKF S-FUNCTION              ##################
%
% ############     CONSTRAINED EXTEND KALMAN FILTER (CEKF) #################
%
% ##################        4 CYLINDRICAL TANKS           ################## 
%___________________________________________________________________________
%___________________________________________________________________________
%
%             Giovani Tonel (giotonel@enq.ufrgs.br) - April 2007
%___________________________________________________________________________
%___________________________________________________________________________


%___________________________________________________________________________
%

% S-function name
namesf		= 2;

%  Convert field to variable
par_pro;
%___________________________________________________________________________


%___________________________________________________________________________________
%
%                                Beginning the S-function
%___________________________________________________________________________________

switch flag,
   

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
case 0,
   
    sys = [0,      					% number of continuous states
           ns+ns^2,      		% number of discrete states
           ns,      				% number of outputs
           nms+num,      		% number of inputs
           0,      					% reserved must be zero
           1,      					% direct feedthrough flag
           1];     					% number of sample times
        
    x0  = [x0(:); P0(:)];  	% initialize the initial conditions
    str = [];      					% str is always an empty matrix
    ts  = [dt 0];   				% sample time: [period, offset]
    
    
    
  %%%%%%%%%%%%%%%
  % Derivatives %
  %%%%%%%%%%%%%%%
case 1,
   
   sys=[];
   % If you put the diferential equations here, the matlab/simulink solve them,
   % using the default solver (ode45).  
   
   
   
  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
case 2,
   xp			= x(1:ns,1);												% xp = Predicte x on tk|k-1
   Pp			=	reshape(x((ns+1):(ns+ns^2)),ns,ns);		% Pp = Predicte P on tk|k-1
   
   %__________________________________________________________________________________
   %
   %                             INPUT PROCESS MEASUREMENT
   %
   %
   % 			Filter inputs:									Filter outputs:
   %
   % 		u(1) <-- F1	(feed flow)	;				% y(1) <-- h1 (filtered)	;
   % 		u(2) <-- F2	(feed flow)	;				% y(2) <-- h2 (filtered)	;
   % 		u(3) <-- X1	(fr. flow)	;				% y(3) <-- h3 (estimated)	;
   % 		u(4) <-- X2	(fr. flow)	;				% y(4) <-- h4 (estimated)	;
   % 		u(5) <-- h1 (measured)	;					
   % 		u(6) <-- h2 (measured)	;
   
   ipm;
   
   %__________________________________________________________________________________  
     
     
     % #################################################################################
     % #########################                ########################################
     % #########################   CORRECTION   ########################################
     % #########################                ########################################
     % #################################################################################         
     
     % disp(' ')
     % disp('... correcting the x(k|k) state ') 
     % disp(' ')
     
          
     CEKF_c     
               
     % disp(' ')
     % disp('Finished corretion phase!!!')
     % disp(' ')
     % disp(' ')
     
         
     % #################################################################################
     % #########################                ########################################
     % #########################   PREDICTION   ########################################
     % #########################                ########################################
     % #################################################################################
     %      
     
     

     % disp(' ')
     % disp('... predicting the x(k+1|k) state') 
     % disp(' ')
     
     %__________________________________________________________________________________
     %
        
     CEKF_p
     %__________________________________________________________________________________
          
          
     % disp(' ')
     % disp('Finished predition fase!!!')
     
     % disp('cEKF: goes to next step ==>> ')
     disp(' ')
     disp(' ') 
     
     if t==Tsim,
        disp('***************************************************************************')
        disp('**************    Finished simulation on CEKF !!!!    *********************');
        disp('***************************************************************************')
     else
     end
     
         
sys=[x_k1(:);	P_k1(:)];
     
     
  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3,
     sys=x(1:ns,1);

  %%%%%%%%%%%%%%%%%%%%%%%
  % GetTimeOfNextVarHit %
  %%%%%%%%%%%%%%%%%%%%%%%
  case 4,
    sys=[] % mdlGetTimeOfNextVarHit(t,x,u);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,
    sys=[]; % do nothing

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    error(['Unhandled flag = ',num2str(flag)]);

end


%___________________________________________________________________________________

Contact us