% Maximum Likelihood parameter estimation: % Output Error Method accounting for measurement noise. % Relaxation algorithm: % Step 1: Explicit estimation of R (covariance matrix of residuals) % Step 2: Gauss-Newton (or Levenberg-Marquardt) method for optimization of % cost function w.r.t. parameters % % Chapter 4: Output Error Method % "Flight Vehicle System Identification - A Time Domain Methodology" % Author: Ravindra V. Jategaonkar % Published by AIAA, Reston, VA 20191, USA clear all; close all; %--------------------------------------------------------------------------------------- % Select the test case to be analyzed: test_case = 1; % Lateral-directional motion, n=2, m=5, p=3, ATTAS % test_case = 2; % Lateral motion, simulated data with turbulence % test_case = 3; % Lateral-directional motion, n=2, m=2, p=3, ATTAS % test_case = 4; % Longitudinal motion (Cl, CD, Cm), test aircraft HFB-320 % test_case = 6; % Unstable aircraft, short period, combined LS/OEM % test_case = 7; % Unstable aircraft, short period, Eqn Decoupling % test_case = 8; % Unstable aircraft, short period, OEM % test_case = 9; % Unstable aircraft, short period, Stabilized OEM % test_case = 10; % Unstable aircraft, short period, EigValTransform % test_case = 11; % Short period motion, n=2, m=2, p=1, ATTAS % test_case = 22; % Flight path reconstruction, ATTAS, NZI=3, % longitudinal and lateral-directional motion % test_case = 23; % Aero-model by Regression, ATTAS, NZI=3, % longitudinal and lateral-directional motion % test_case = 24; % same as test_case=23, but nonlinear in CD % test_case = 27; % ATTAS Regression NL -- Quasi-steady stall % longitudinal mode only %--------------------------------------------------------------------------------------- % Choose iOptim for optimization method: =1: Gauss-Newton; =2: Levenberg-Marquardt iOptim = 1; % iOptim = 2; %--------------------------------------------------------------------------------------- % Choose numerical integration method: integMethod = 'ruku4'; % ruku2, ruku3, ruku4 (2nd, 3rd or 4th order Rumge-Kutta) %--------------------------------------------------------------------------------------- % Model definition; functions for state derivatives and outputs if (test_case == 1), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase01(test_case); elseif (test_case == 2), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase02(test_case); elseif (test_case == 3), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase03(test_case); elseif (test_case == 4), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase04(test_case); elseif (test_case == 6), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase06(test_case); elseif (test_case == 7), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase07(test_case); elseif (test_case == 8), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase08(test_case); elseif (test_case == 9), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase09(test_case); elseif (test_case == 10), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase10(test_case); elseif (test_case == 11), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase11(test_case); elseif (test_case == 22), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase22(test_case); elseif (test_case == 23), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase23(test_case); elseif (test_case == 24), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase24(test_case); elseif (test_case == 27), [state_eq, obser_eq, Nx, Ny, Nu, NparSys, Nparam, NparID, dt, Ndata, Nzi, izhf, ... t, Z, Uinp, param, parFlag, x0, iArtifStab, StabMat] = mDefCase27(test_case); else disp('Error Termination:'); disp('Wrong specification of test_case.'); break; end % Verify model definition iError = mDefCHK_oem(test_case, Nx, Ny, Nu, NparSys, Nparam, NparID, Nzi, ... param, parFlag, x0, iArtifStab, StabMat); if iError ~= 0, break, end % Error termination %---------------------------------------------------------------------------------------- % parameter perturbation size for gradient computation by numerical approximation par_step = 1e-6; %par_step = 1e-3; % Convergence limit in terms of relative change in the cost function tolR = 1e-4; % max number of iterations niter_max = 50; % niter_max = 10; %---------------------------------------------------------------------------------------- % Initialize iteration counter, counter for halving steps (maximum of ten) iter = 0; khalf = 0; prevCost = 0; % Store parameter values for plotting purposes params = param; par_std = zeros(NparID,1); % options for ode solver (default) options = odeset; %---------------------------------------------------------------------------------------- disp('Maximum Likelihood Parameter estimation: Output Error Method:'); if (iOptim == 1), disp('Optimization method: Gauss-Newton method:'); elseif (iOptim == 2), disp('Optimization method: Levenberg-Marquardt method:'); lamda = 0.001; else disp('Error Termination:'); disp('Wrong specification of the optimization method.'); break; end if (strcmp(integMethod,'ruku2')), disp('Numerical integration method: Runge-Kutta 2nd order'); elseif (strcmp(integMethod,'ruku3')), disp('Numerical integration method: Runge-Kutta 3rd order'); elseif (strcmp(integMethod,'ruku4')), disp('Numerical integration method: Runge-Kutta 4th order'); else disp('Error Termination:'); disp('Wrong specification of the integration method.'); break; end %---------------------------------------------------------------------------------- % Iteration Loop (Jump back address) while iter <= niter_max; % perform simulation for the current parameter values [currentCost, R, RI, Y] = costfun_oem (integMethod, state_eq, obser_eq, Ndata, Ny, Nu,... Nzi, Nx, dt, t, x0, Uinp, Z, izhf, param,... iArtifStab, StabMat); % Print out the iteration count (, parameter values) and cost function disp(['iteration = ', num2str(iter)]); %disp('param'); param disp(['cost function: det(R) = ' num2str(det(R))]); % Relative change in the cost function relerror = abs((currentCost-prevCost)/currentCost); %disp(['relative error = ', num2str(relerror)]); % store parameters and standard deviations only if successful step: convergence plot % if (currentCost < prevCost), if ( (currentCost < prevCost) | ((relerror 0)) ), params = [params, param]; pcov = inv(F); % parameter error covariance matrix, Eq. (4.87) par_std = [par_std, sqrt(diag(pcov))]; % standard deviations, Eq. (4.88) end % Check convergence: relative change in the cost function if (relerror < tolR) & (iter > 0), disp('convergence criterion is satisfied'); break; else if (iter == niter_max), if (currentCost < prevCost | niter_max == 0), disp('Maximum number of iterations reached.'); break; else % to allow halving at itmax before terminating disp('Intermediate divergence: halving of parameter step') param = (param+param_old)/2; khalf = khalf + 1; end; elseif (currentCost > prevCost) & (iter > 0), % Maximum of 10 times halving of the parameters if (khalf == 10), disp('Intermediate divergence!') disp('Error termination:') disp('No improvement after 10 times halving of parameter step.') break; else disp('Intermediate divergence: halving of parameter step') param = (param+param_old)/2; khalf = khalf + 1; end; else % compute the matrix of second gradients F and gradient G, Eq. (4.30) par_del = par_step*param; % parameter perturbations for gradients par_del(find(par_del==0)) = par_step; % if zero, set to par_step [F,G] = gradFG(Ny, Nparam, NparID, Ndata, Nu, Nx, Nzi, izhf, ... dt, Z, Y, RI, param, parFlag, par_del, state_eq, Uinp, ... obser_eq, t, x0, iArtifStab, StabMat, integMethod); % compute parameter update step: either GN or LM if iOptim == 1, % Gauss-Newton %delPar = -inv(F) * G; % using inverse of amtrix %delPar = - F \ G; % Using matrix division operator: % Using Cholesky factorization: uptF = chol(F); % Upper triangular F = uptF'*uptF delPar = -uptF \ (uptF'\G); % Eq. (4.29) elseif iOptim == 2, % Levenberg-Marquardt, Section 4.13 [delPar, lamda] = ... LMmethod(integMethod, state_eq, obser_eq, Ndata, Ny, Nu, Nzi, Nx,... Nparam, NparID, lamda, param, parFlag, dt, t, x0,... Uinp, Z, izhf, F, G, tolR, currentCost,... iArtifStab, StabMat); end % 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); % Eq. (4.29) end end prevCost = currentCost; iter = iter + 1; khalf = 0; end; end; end; %---------------------------------------------------------------------------------------- % 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 plot of parameter estimates if (test_case == 1), [t]=plots_TC01_oem_lat(t, Z, Y, Uinp, params, par_std, iter); elseif (test_case == 2), [t]=plots_TC02_oem_SimNoise(t, Z, Y, Uinp, params, par_std, iter); elseif (test_case == 3), [t]=plots_TC03_oem_lat_pr(t, Z, Y, Uinp, params, par_std, iter); elseif (test_case == 4), [t]=plots_TC04_oem_hfb(t, Z, Y, Uinp, params, par_std, iter); elseif (test_case <= 6 | test_case <= 10), [t]=plots_TC08_oem_uAC(t, Z, Y, Uinp, params, par_std, iter); elseif (test_case == 11), [t]=plots_TC11_oem_sp(t, Z, Y, Uinp, params, par_std, iter); elseif (test_case == 22), [t]=plots_TC22_oem_fpr(t, Z, Y, Uinp, params, par_std, iter); elseif (test_case == 23 | test_case ==24), [t]=plots_TC23_attas_regLonLat(t, Z, Y, Uinp, params, par_std, iter); elseif (test_case == 27), [t]=plots_TC27_oem_regQStall(t, Z, Y, Uinp, params, par_std, iter); end