# MHE with cEKF : 4 cylindrical tanks

### Giovani Tonel (view profile)

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

%___________________________________________________________________________________
```