function ml_fem(test_case, target_log)

% Maximum Likelihood parameter estimation:
% Filter Error Method accounting for process and measurement noise.
% Applicable to general nonlinear systems (Section 5.5)
% Relaxation algorithm:
%     Step 1: Explicit estimation of R (covariance matrix of residuals)
%     Step 2: Gauss-Newton method for optimization of cost function w.r.t. parameters
%
% Chapter 5: Filter Error Method,
% "Flight Vehicle System Identification - A Time Domain Methodology"
% Author: Ravindra V. Jategaonkar
% Published by AIAA, Reston, VA 20191, USA

if (nargin < 1), test_case = 304; end
if (nargin < 2), target_log = 0; end

clear Cmat F FAlt G KGPert Kgain Ndata Nk NparID NparSys Nparam Nu Nx Ny R RI RIAlt RInew Rnew SDyError Uinp Y Z currentcost delPar dt gradKC i1 iError iFcmp iFup iPar iSD iter iterFupR khalf niter_max obser_eq parFlag par_del par_std par_std_rel par_step paralt param param_old params pcov prevcost relerror state_eq t tolR uptF x0 xt stables output_file;
close all;

global stables;
stables = [20, 0, 0, 0];

% Model definition; functions for state derivatives and outputs
if (test_case == 4),        % Longitudinal motion (Cl, CD, Cm), test aircraft HFB-320
    [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata,...
                   t, Z, Uinp, param, parFlag, x0, iSD, SDyError] = mDefCase04(test_case);
elseif (test_case == 104),   % Longitudinal motion (X, Z, M),
    [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata,...
                   t, Z, Uinp, param, parFlag, x0, iSD, SDyError] = mDefCase04a(test_case);
elseif (test_case == 204),   % Longitudinal motion (X, Z, M) without dt,
    [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata,...
                   t, Z, Uinp, param, parFlag, x0, iSD, SDyError] = mDefCase04b(test_case);
elseif (test_case == 205),   % Longitudinal motion (X, Z, M) without dt, Mante Carlo
    [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata,...
                   t, Z, Uinp, param, parFlag, x0, iSD, SDyError] = mDefCase04bm(test_case);
elseif (test_case == 304),   % Longitudinal motion (X, Z, M) without dt, 6 states
    [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata,...
                   t, Z, Uinp, param, parFlag, x0, iSD, SDyError, output_file] = mDefCase04c(test_case, target_log);
elseif (test_case == 305),   % Longitudinal motion (X, Z, M) without dt, 6 states, Mante Carlo
    [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata,...
                   t, Z, Uinp, param, parFlag, x0, iSD, SDyError, output_file] = mDefCase04cm(test_case, target_log);
elseif (test_case == 306),   % Lateral motion (Y, L, N) 6 observable variables
    [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata,...
                   t, Z, Uinp, param, parFlag, x0, iSD, SDyError, output_file] = mDefCase04d(test_case, target_log);
else
    disp('Error Termination:');
    disp('Wrong specification of test_case.');
    return;
end 

if exist('output_file'),
  disp(['output_file => ', output_file]);
end

% Verify model definition
iError = mDefCHK_fem(test_case, Nx, Ny, Nu, NparSys, Nparam, NparID, ...
                                            param, parFlag, x0, iSD, SDyError);
if iError ~= 0, return, end        % Error termination 
                                    
%----------------------------------------------------------------------------------------
% parameter perturbation size for gradient computation by numerical approximation
%par_step  = 1e-6;
par_step  = 1e-5;
% Convergence limit in terms of relative change in thc cost function
tolR      = 1e-4;
%tolR      = 1e-3;
% max number of iterations
niter_max = 15;

% Iteration count from which F-estimate will be corrected using latest R-estimate
iterFupR = 3;

%----------------------------------------------------------------------------------------
% Initialize iteration counter, counter for halving step (maximum of ten)
iter     = 0;
khalf    = 0;
prevcost = 0;
iFup     = 0;
iFcmp    = 0;

% Store parameter values for plotting purposes
params   = [param];
par_std  = zeros(NparID,1);

disp('Parameter estimation applying Maximum Likelihood (Filter Error) Method:');

%----------------------------------------------------------------------------------------
% Initial measurement noise covariance matrix R: (see section 5.6)
if (iSD == 0),                                       % Default values with K = 0
    Kgain = zeros(Nx,Ny);
    [currentcost, R, RI, Y] = costfun_fem (state_eq, obser_eq, Ndata, Ny, Nu, Nx,...
                                           dt, t, x0, Uinp, Z, param, Kgain);
    disp(['Cost function: det(R) = ' num2str(currentcost)]);
    RI
else
    % Specified as standard deviations of output errors
    disp ('Standard deviations of the output errors:');
    RI = diag(1./SDyError.^2);
    RI
end

%----------------------------------------------------------------------------------------
% Iteration Loop (Jump back address)
while iter <= niter_max;
    
    khalf2 = 0;
    while iFcmp <= 0;
        % Compute steady-state Kalman Gain matrix
        xt = x0;
        [Kgain, Cmat] = gainmat(state_eq, obser_eq, Nx, Ny, Nu, Nparam,...
                                          dt, t, param, parFlag, xt, Uinp, RI);    
        % simulation for the current parameter values
        % estimate covariance matrix R; determinant and inverse of R
        xt = x0;
        [currentcost, R, RInew, Y] = costfun_fem (state_eq, obser_eq, Ndata, Ny, Nu,... 
                                          Nx, dt, t, xt, Uinp, Z, param, Kgain);

        if  iFup == 0,
            % Print out the iteration count, parameter values and cost function
            disp(' ');
            disp(['iteration = ', num2str(iter)]);
            %disp('param'); param
            disp(['cost function: det(R) = ' num2str(currentcost)]);
            iFcmp = 1;
        else
            disp('Correction of F (with the new estimated value of R)');
            %disp('param'); param
            disp(['cost function: det(R) = ' num2str(currentcost)]);
        
            if  currentcost > prevcost,
                disp('Local divergence after F-compensation');
                khalf2 = khalf2 + 1;
                if  khalf2 < 3,              % Allow 2 step of halving
                    param(Nparam-Nx+1:Nparam) = (param(Nparam-Nx+1:Nparam)+FAlt)/2;
                elseif khalf2 == 3,          % At 3rd halving restore previous values
                    param(Nparam-Nx+1:Nparam) = FAlt;
                    RI = RIAlt;
                else                        % After 3rd halving, discontinue F-correction 
                    iFcmp = 1;              % and proceed to the next full step.
                end
            else
                iFcmp = 1;                  % Set iFcmp > 0 after halving is done
            end
        end
        
    end   % end of while iFcmp <= 0
    

    % store parameters and standard deviations only if successful step: convergence plot 
    if  (iter > 0 & iFup == 0) | (iter > 0 & iFcmp == 1),
        params  = [params, param];
        pcov    = inv(F);
        par_std = [par_std, sqrt(diag(pcov))];    
    end

    %------------------------------------------------------------------------------------
    % Check convergence: relative change in the cost function
    relerror = abs((currentcost-prevcost)/currentcost);
    %disp(['relative error = ', num2str(relerror)]);

    if (relerror<tolR) & (iter > 0)  & (iFcmp ==0),
        
        disp('Iteration concluded: relative change in det(R) < tolR')
        break;
        
    else

        if  iter == niter_max,
            disp(' ');
            disp('Maximum number of iterations reached.');
            break;
            
        elseif (currentcost>prevcost) & (iter > 0),
        
            % Maximum of 10 times halving of the parameters
            if  khalf == 10,
                disp('Intermediate divergence:!')
                disp('Error termination:')
                error('No further improvement after 10 times halving of parameters.')
                %break;
            else
                disp('Intermediate divergence: halving of parameter step')
                param = (param+param_old)/2;
                khalf = khalf + 1;
            end; 
            
        else

            %----------------------------------------------------------------------------
            xt = x0;
            par_del = par_step*param;               % parameter perturbations for gradients
            par_del(find(par_del==0)) = par_step;   % if zero, set to par_step

            % compute the perturbed gain matrices for gradient computations
            [KGPert, gradKC] = gainmatPert(state_eq, obser_eq, Nx, Ny, Nu, Nparam,...
                                           dt, t, param, par_del, parFlag, xt,...
                                           Uinp, RI, Kgain, Cmat);

            % compute the matrix of second gradients F and gradient G
            [F,G]  = gradFG(Ny, Nparam, NparID, Ndata, Nu, Nx, dt, Z, Y, RI,...
                            param, par_del, parFlag, state_eq, Uinp, obser_eq,...
                            t, xt, Kgain, KGPert);
            %F
            
            % compute parameter update using Cholesky factorization: 
            uptF   = chol(F);                    % Upper traiangular F = uptF'*uptF 
            delPar = -uptF \ (uptF'\G);

            % Check for the inequality contraints KC <= 1
            Nk = NparSys + Nx;                   % Number of parameters affecting K
            [delPar] = kcle1(Nx, Ny, Nparam, NparID, Nk, delPar, Cmat, Kgain, gradKC, F);
              
            %----------------------------------------------------------------------------
            % update parameter vector param
            param_old = param;
            iPar = 0;
            for i1=1:Nparam,
                if  parFlag(i1) > 0,
                    iPar = iPar + 1;
                    param(i1) = param(i1) + delPar(iPar);
                end
            end
            
            prevcost = currentcost;
            iter  = iter + 1;
            iFcmp = 0;

            if  iter >= iterFupR,                % F compensation using new R

                khalf = 1;                       % Initialize counter
                if  khalf ~= 0,                  % Jump back address

                    xt = x0;
                    [Kgain, Cmat] = gainmat(state_eq, obser_eq, Nx, Ny, Nu, Nparam,...
                                            dt, t, param, parFlag, xt, Uinp, RI);
                                          
                    [currentcost, Rnew, RInew, Y] = ...
                            costfun_fem (state_eq, obser_eq, Ndata, Ny, Nu, Nx, dt,...
                                         t, xt, Uinp, Z, param, Kgain);
                        
                    disp(' ');
                    disp(['iteration = ', num2str(iter)]);
                    %disp('param'); param
                    disp(['cost function: det(R) = ' num2str(currentcost)]);
                                
                    relerror = abs((currentcost-prevcost)/currentcost);
                    if  relerror < tolR,
                        % On eps-convergence after the full iteration, no need to carry out
                        % F-compensation, because at the true minimum it would not (or rather
                        % should not) lead to any changes in det(R). Hence, terminate.
                        disp(' ')
                        disp('Iteration concluded: relative change in det(R) < tolR')
                        params  = [params, param];
                        pcov    = inv(F);        % save param and std.Dev for conver-plot
                        par_std = [par_std, sqrt(diag(pcov))];    
                        break
                    elseif (currentcost > prevcost),
                        
                        if  khalf == 10,         % Maximum 10 times halving of parameters
                            disp('Error termination:')
                            disp('No further improvement after 10 times halving of parameters.')
                            break;
                        else
                            disp('Intermediate divergence: halving of parameter step')
                            param = (param+param_old)/2;
                            khalf = khalf + 1;
                        end
                    else
                        khalf = 0;
                    end
                end

                prevcost = currentcost;
                
                paralt = param;
                FAlt   = param(Nparam-Nx+1:Nparam);
                RIAlt  = RI;
                
                % Compensate F-estimates using new RI
                param = fcompn(Nx, Nparam, FAlt, RInew, RI, Cmat, param, parFlag);
                RI    = RInew;
                iFup  = 1;
                iFcmp = 0;
                khalf = 0; 
            end              % end of: iter >= iterFupR, F compensation using new R

        end;                 % end of: if iter == niter_max,
        
    end;                     % end of: if (relerror<tolR) & (iter > 0) & (iFcmp ==0),
  
end;                         % end of: while iter <= niter_max;


%----------------------------------------------------------------------------------------
% Printout of final parameter estimates, standard deviations and correlation coefficients
if  iter > 0,
    par_std_rel = par_accuracy(iter, Nparam, param, par_std, pcov, parFlag, NparID);
end


%----------------------------------------------------------------------------------------
% plots of measured and estimated time histories of outputs and inputs
% convergence plots of parameter estimates
if (test_case == 4),
  [t]=plots_TC04_oem_hfb(t, Z, Y, Uinp, params, par_std, iter);
elseif (test_case == 104),
  [t]=plots_TC04a_oem_hfb(t, Z, Y, Uinp, params, par_std, iter);
elseif (test_case == 204 | test_case == 205),
  [t]=plots_TC04b_oem_hfb(t, Z, Y, Uinp, params, par_std, iter);
elseif (test_case == 304 | test_case == 305),
  [t]=plots_TC04c_oem_hfb(t, Z, Y, Uinp, params, par_std, iter);
elseif (test_case == 306),
  [t]=plots_nlinear_lat(t, Z, Y, Uinp, params, par_std, iter);
end

if exist('output_file'),
  % param, par_std(iPar,iter+1)
  
  param_est = [];
  for ip=1:Nparam,
    if  parFlag(ip) > 0,
      param_est = [param_est; param(ip)];
    end
  end
  
  csvwrite(output_file, [param_est par_std(:,iter+1)]);
end