%     PROCEDURE FILE  TO PERFORM MAXIMUM LIKELIHOOD IDENTIFICATION
%             ------- RUN ml_demo1 for a demonstration. -------
%  OUTPUTS
%  -  pfin   = Final parameter vector.
%  -  gg     = Final value of innovations covariance used or estimated.
%  - results = Matrix summarizing results, columns are pidf, pfin(pidf), then
%        pref    = pref(pidf). pref can be user-supplied or defaults to p0.
%      cramer(i) = Cramer-Rao bound = min rms parameter error
%  twofcramer(i) = 2 * Cramer-Rao bound assuming residual spectral
%                  density is constant at value estimated from
%                  filtering residuals through a Rhz filter. Rhz as below.
%        insens  = insensitivity = Cramer-Rao if only this param being ID'd
%        gdop(i) = Geometric Dilution Of Precision = Cramer(i)/Insens(i)
%  - his         = Parameter and convergence history. Cols store
%                  [p,nits,max(abs(grad)),max(abs(p-pold)),nllf,wersum]
%  - hes1,2      = Previous hes matrices. Gives info if hes = NaN when bombs.
%                  Diagonals only stored to save space. Change to whole
%                  mat. if desired. Often zero diag component => user error.
%  INPUTS
%  - uydata  = [Inputs(1:ndp,1:#inputs),Outputs(1:ndp,1:#outputs)]. GLOBAL
%  - p2snam  = Name of user written function converting p to state space model
%  - p0      = parameter vector p is reset to p0 at start of MMLE
%  - pref    = Reference values of parameters subtracted from pfin in
%               third column of RESULTS matrix. If undefined or not of
%               same size as p0, pref is reset to p0.
%  - rms0    = Standard deviation of a-priori parameter knowledge (NB, > 0)
%               Create it ONLY if you want to weight estimates towards pref.
%  - useghes : Fortran routine GHES.EXE is used if useghes exists
% linesearch : Linesearch along Newton direction occurs if linesearch exists.
%  usermods  : Create this macro and it will be executed before step 0 printout.
%   diagrr   : Estimated innovations cov. is made diagonal if diagrr exists
%  - pidq    = Indices of parameters to be identified in quadratic phase
%  - pidm    =      ditto for Marquard stage
%  - pidf    =      ditto for constrained Newton and gg estimation phases.
%  - pert    = If scalar, parameters perturbed by it to compute sensitivities
%               until gg estimation whereafter p(pidf(i)) is perturbed by
%               insens(pidf(i))*pert. Suggest pert=.001 . If vector sized p0,
%               perturbation is fixed at pert(pidf(i))
%  - gg0     = Innovations covariance gg is reset to this when mmle starts
%  - opt     = Option vector:  Defaults to [0 5 5 5 .02 .05 .001 1]
%                        Elements are, in order:
%               qstep :  Max no of quadratic steps using pidq
%               mstep :           Marquard ...           pidm
%               nstep :           constrained Newton ... pidf   f for final!
%               gstep :           gg determination   ... pidf
%               marqf : Value of marq to cause early termination
%                       - marq starts at imag(marqf), else marq0 = 1
%               fconv : wersum change < fconv triggers gg determination
%               gconv :     ...        < gconv triggers Cramer-Rao calculation
%               rhz   : Cutoff Hz of filter used to determine innovation (rr)
%                       spectral density for filtered C-R bound calculation.
%  MODEL
%        See the help file for ml_p2ss1.m and its internal comments
%        Note that process noise absolutely zeros disables the Kalman gain
%--------------------------------------------------------------------------
%  DEMO FILES : Run ml_demo1,   ml_demo2,   ml_demo3.   See their comments.
'==================================== MMLE ===================================';
disp(' ');disp(ans);stime=clock;

%=================================================================
%        CHECK INPUT OPTION DATA AND DEFAULT WHERE POSSIBLE
%=================================================================

p=p0;%  ==> p0 IS UNDEFINED
p0(pidq);%  ==> INDICES OF pidq MUST EXIST IN p0
p0(pidm);%  ==> INDICES OF pidm MUST EXIST IN p0
p0(pidf);%  ==> INDICES OF pidf MUST EXIST IN p0
perts=pert;if prod(size(pert))==1, perts=pert*exp(0*p0);end %1 DEFAULT PERTS
if exist('diagrr')==1,diag_rr=1;else diag_rr=0;end%1a DEFAULT FULL rr

'linesearch=0;';if exist('linesearch')==0,eval(ans);end%2 DEFAULT NO LINESEARCH

'wapriori=0*p0;';if exist('rms0')==0,eval(ans);else rms0=rms0,%DEFAULT WAPRIORI
   wapriori=exp(-2*log(rms0));end%3 ==> MAKE ALL ELEMENTS OF rms0 >0
   wapriori+p0;% ==> rms0 AND p0 INCOMPATIBLE

'ans=pref;';if exist('pref')==1,eval(ans);else pref=p0;end%4 DEFAULT PREF
if max(size(pref)-size(p0))~=0,pref=p0;end

'opt=[0 5 5 5 .02 .05 .001 1];';if exist('opt')==0,eval(ans);end%5 DEFAULT opt
   [opt+ones(1,8)];% ==> OPT MUST BE    1 * 8

%=================================================================
%                  CREATE MACROS FOR LATER USE
%          These can be used interactively if desired
%          Examine later code to see how they are used
%=================================================================
if exist('useghes')==1, %
  dograd='eval(domodl),load fromghes ,eval(dounwind)';
  domodl='hes2=hes1;hes1=diag(hes);llf=mmlemods(p,pid,p2snam,perts,gg);!ghes';

  ans=['npid=length(pid);grad=gradhes(1:npid)+',...
     '(p(pid)-pref(pid))''.*wapriori(pid)'';',... %ADD A-PRIORI GRAD COMPONENT
     'hes=zeros(npid);pos=[0 cumsum(npid:-1:1)]+npid;',...
     'for col=1:npid,hes(col:npid,col)=gradhes(pos(col)+1:pos(col+1));'];
  dounwind=[ans,  'end;hes=hes+hes''-diag(diag(hes)',...
     '-wapriori(pid)'');',...%                      ADD A-PRIORI HES COMPONENT
     'if rcond(hes)<1e-10,rcond_hes=rcond(hes),',...
     'disp(''hes near singular. Possibly add eye(hes)/1e9''),keyboard,end,'];
else
    ['hes2=hes1;hes1=diag(hes);llf=mmlegrad(p,pid,p2snam,perts,gg);',...
     'npid=length(pid);grad=grad+',...
     '(p(pid)-pref(pid))''.*wapriori(pid)'';']; % ADD A-PRIORI GRAD COMP.
dograd=[ans,'hes=hes+diag(wapriori(pid)'');',... %ADD A-PRIORI HES COMPONENT
     'if rcond(hes)<1e-10,rcond_hes=rcond(hes),',...
     'disp(''hes near singular. Possibly add eye(hes)/1e9''),keyboard,end,'];
end

dostep=['[p,pstep1,ggn,nllf,ggest]=',...
       'mmlestep(p,pid,p2snam,gg,ggest,fconv,linesearch);'];

domarqstep='[p,nllf,marq]=mmlemarq(p,pid,p2snam,gg,marq,nllf);';

dolike='llf0=nllf;[nllf,wersum,rr,dt,k]=mmlelike(p,p2snam,gg);';

doprt=['row2str=[nits sum(p(pidf)) ggscal*[sum(sum(gg)) sum(sum(rr))],',...
      'max(abs(grad)),max(abs(p-pold)),nllf,wersum];',...
      'pold=p;ans=[];for i=1:length(row2str),',...
      'ans1=sprintf(''%f'',row2str(i));ans=[ans,ans1(1:7),''   ''];end;'];
      doprt=[doprt 'disp(ans(1:79));',...
      'if nits>0,his(:,nits+1)=[p nits row2str(5:8)]'';end' ];

doquad=['nits=nits+1;disp([lyne,''---------quadratic (pidq)'']);pid=pidq;',...
    'eval(dograd);eval(dostep);p=pstep1;disp(tytl);eval(doprt);eval(dispq)'];

domarq=['nits=nits+1;disp([lyne,''---------Marquardt (pidm)'']);'...
       'eval(dograd);'...
       'eval(domarqstep);disp('' '');disp(tytl);eval(doprt);eval(dispm)'];

donn=['nits=nits+1;disp([lyne,''constrained Newton (pidf)'']),eval(dograd),',...
      'eval(dostep);p(qparam)=max(p(qparam),',...%  FORCES  Qd >Qd_old/100
      'pold(qparam)/100);disp(tytl),eval(doprt);eval(dispn)'];

dogg=['nits=nits+1;disp([lyne,''--gg determination (pidf)'']),gg=ggn;',...
     'eval(dograd);eval(dostep);p(qparam)=max(p(qparam),',...% Qd > Qd_old/100
     'pold(qparam)/100);disp(tytl);eval(doprt);eval(dispg)'];

dop2ss='[a,phi,gam,c,d,q,x0,dt,rowinq,b]=eval([p2snam,''(p)'']);';

dopcr='ihes=inv(hes);pcr=ihes/sqrt(diag(diag(ihes)))';%PARAMS AT CR BOUND PTS

doshes='diag(exp(-.5*log(diag(hes))));shes=ans*hes*ans;';% UNIT DIAG HES
%=================================================================
%           CHECK DIMENSIONS OF MATRICES FROM P2NAM(P)
%=================================================================

eval(dop2ss);%  USE FROM KEYBOARD TO GET SYSTEM MATRICES
[1,dt,dt'];%  ==>  dt MUST BE DEFINED AS A SCALAR
[a,a',phi,gam,q,x0'];%  ==>  STATE DIMENSIONS INCOMPATIBLE. x0 MUST BE A ROW
[gam;d];%  ==>  INPUT DIMENSIONS INCOMPATIBLE
[c,d,gg0];%  ==>  OUTPUT DIMENSIONS INCOMPATIBLE
[p-rowinq,1];%  ==>  ROWINQ AND P MUST BE SAME SIZE ROW VECTORS
[uydata(1,:) ;[gam c']];%  ==>  COLS OF uydata <> TOTAL INPUTS AND OUTPUTS

%=================================================================
%   INITIALIZE.  DEFINE SOME GLOBALS TO AVOID COPYING TO FUNCTIONS
%=================================================================
if exist('useghes')==1,uydat=uydata';uydat=uydat(:);
   save uydat uydat;clear uydat;end
[ndp,num]=size(uydata);qparam=find(rowinq>0);
yest=zeros(ndp,size(c).*[1 0]);
[tmp1,tmp2]=size(yest);
inovt=zeros(tmp1,tmp2);
hes=zeros(max(size(pidf)));
wersum=0;grad=0;rr=1;e0=1;grade=1;

his=[p0 0 0 0 0 0]'*ones(1,sum(opt(1:4)'));hes1=0;
qstep=opt(1);mstep=opt(2);nstep=opt(3);gstep=opt(4);marq0=imag(opt(5));
marqf=real(opt(5));fconv=opt(6);gconv=opt(7);rhz=opt(8);gg=gg0;ggn=gg;
pold=p;

' STEP    PIDF_nsum  GG_osum   RR_nsum   MAXGRAD   MAX|dP|      LLF     WERSUM';
tytl=ans;lyne='---------=---------=---------=---------=---------=--';

global uydata
global yest
global inovt
global hes
global wersum
global grad
global wapriori
global pref
global rr
global e0
global grade
global rowinq
global diag_rr

%=================================================================
%      DEFINE PARAMETERS TO BE DISPLAYED AT END OF EACH STEP
%=================================================================

dispq='ppid=p(pid)';
dispm='ppid=p(pid)';
dispn='ppid=p(pid)';
dispg='ppid=p(pid)';

if exist('usermods')==1, eval(usermods);end%      USERMODS
%=================================================================
%         COMPUTE AND DISPLAY LLF FOR INITIAL PARAMETERS
%=================================================================
nits=0;grad=8888888;nllf=[];marq=0; %  INITIALIZE
ggscal=1;if sum(sum(gg))<.01,ggscal=1e6;
  disp('SUM(GG) and SUM(RR) will be multiplied by 1E6 before display'),end%6

'----------';if linesearch==0; [setstr(45*ones(1,63)),'mmle : initial'];
   else;[setstr(45*ones(1,52)),'mmle linesearch : initial'];end;disp(ans);

disp(tytl);eval(dolike);yest=[];
eval(doprt); %           DISPLAY LLF
llf0=nllf;pstep1=[];

%=================================================================
%             QUADRATIC PHASE ( PURE NEWTON ALGORITHM )
%=================================================================

if qstep > .1
   pid=pidq;ggest=0;
     while nits < (qstep-.01),eval(doquad);end%8
end%9

%=================================================================
%                          MARQUARD PHASE
%=================================================================

if mstep>0.1;pid=pidm;marq=marq0; %            MARQUARD ITERATIONS
   if marq==0, marq=1;end; %10 DEFAULT CHOICE
   if marq <= marqf, disp('marqf lower than marq0');end;%11
   fnits=nits+mstep-.01;
   while nits<fnits,
       if marq > (marqf*1.0001)
         eval(domarq)
       else fnits=nits;
       end%12
   end%13
end%14

%=================================================================
%              CONSTRAINED NEWTON MINIMIZATION PHASE
%  Process noise is prevented from getting larger than possible
%      given the specified measurement noise covariance.
%=================================================================

wersum0=1e10;ggest=gstep>.5; %                          INITIALIZE
if nstep>0.1, %                       CONSTRAINED NEWTON ALGORITHM
   pid=pidf;
   fnits=nits+nstep-.01;
   while nits<fnits, %                      UP TO FSTEP ITERATIONS
       if (ggest==2)|(abs(wersum-wersum0)<fconv*wersum0),
           fnits=0; %                     STOP IF WERSUM CONVERGED
   disp('============ Delta_wersum/wersum < opt(6) ==> Nconvergence')
       else wersum0=wersum;eval(donn)
       end%15
   end%16
end%17

%=================================================================
%                     gg ESTIMATION PHASE
%=================================================================

if gstep>0.5, % CONSTRAINED NEWTON WITH GG ESTIM. AND F CORRECTION
   pid=pidf;wersum0=1e10;ggest=2;
   fnits=nits+gstep-.01;
%-------------CHECK FOR UNSTABLE KF AFTER BIG CHANGE IN GG
   gg=ggn;flag=1;
       while flag==1;eval(dolike);ei=abs(eig(phi-phi*k*c));flag=max(ei)>1;
         if flag==1;AbsEigKalman=ei',disp('Reduce process noise till < 1')
   keyboard,end,end%18

wersum=-1e10;
   while nits<fnits, %                      UP TO GSTEP ITERATIONS
       if abs(wersum0-wersum)<gconv*wersum0, fnits=0;
       else wersum0=wersum;
         if prod(size(pert))==1,
         perts(pid)=exp(-.5*log(diag(hes))')*pert;end;%19   PERTS=INSENS*PERT
       eval(dogg)
       end%20
   end%21
end%22
pfin=p; %                            RETURN FINAL PARAMETER VECTOR
%-----------------------------------------------------------------

if (gstep<=.5) | (fnits>0) ==1,%  |= OR
   disp('========== GG convergence incomplete.  Cramer-Rao doubtful')
else
   disp('=========== Delta_wersum/wersum < opt(7) ==> GGconvergence')
end%23
linesearch=0;

%=================================================================
%           FILTERED AND CONVENTIONAL CRAMER-RAO BOUNDS
%=================================================================

lyne='-------------------------------------------------';
disp([lyne,'------Filtered-Cramer-Rao'])
cramer=sqrt(diag(inv(hes)))';

% -------------------------------------------- FILTERED C-R BOUNDS
['2fcramer = 2 * Cramer-Rao Bounds for residual psd to ',...
num2str(rhz),' hz'];
disp(ans)
[nx,nm]=size(k);[ndp,num]=size(uydata);um=[1:num-nm];
ym=[num-nm+1:num];%    um,ym = LOCATIONS OF INPUT/OUTPUT IN uydata

eval(dolike);inovt=uydata(:,ym) - yest; %           GET INOVATIONS
e2at=exp(-2*pi*rhz*dt);
ans=dlsim(eye(nm)*e2at,eye(nm)*(1-e2at)/rhz,...        FILTER THEM
           eye(nm),0*ones(nm),inovt);
jfil=.5*sum(sum(ans/gg.*ans)); %                     FILTERED COST

% Now scale cramer with filtered resid pwr/unfiltered pwr(ie nm*ndp)
% times nyquist freq/residual filter freq.  see NASA TP1563 p16.

twofcramer=2*cramer*sqrt(jfil/(nm*ndp*rhz*2*dt)); %FILTERED BOUNDS

%------------------------------------------------ HESSIAN ANALYSIS
disp([lyne,'------------------RESULTS'])
'     pid      p(pid)     pref     cramer   2fcramer   insens     gdop';
format short;disp(ans);
insens=exp(-.5*log(diag(hes)))';%                  INSENSITIVITIES
gdop=cramer./insens;%              Geometric Dilution Of Precision
results=[pid;p(pid);pref(pid);cramer;twofcramer;insens;gdop]'
clear gdop;clear cramer;clear twofcramer;

%=================================================================
%               DOCUMENT OPT, DATE AND CPU TIME
%=================================================================

disp([lyne, '-----------------opt(1:7)'])
opt1to7=opt(1:7)
disp([lyne, '-----------------cpu time'])
secs=etime(clock,stime);mins=fix(secs/60);secs=secs-60*mins;
disp(['cpu time =  ',num2str(mins),' minutes,  ',num2str(secs),' seconds'])
ans=[];ans2=fix(clock);for i=1:6,ans1=num2str(ans2(i));
ans=[ans,ans1,'  '];end;disp(['    date = ',ans])
eval(dop2ss)% GET FINAL SYSTEM MATRICES
clear inovt;
disp([lyne, '-----------------end mmle']);
%====================================================== end mmle.m

