home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 3.ddi / MMLE3.DI$ / MMLE.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  15.4 KB  |  341 lines
  1. %     PROCEDURE FILE  TO PERFORM MAXIMUM LIKELIHOOD IDENTIFICATION
  2. %             ------- RUN ml_demo1 for a demonstration. -------
  3. %  OUTPUTS
  4. %  -  pfin   = Final parameter vector.
  5. %  -  gg     = Final value of innovations covariance used or estimated.
  6. %  - results = Matrix summarizing results, columns are pidf, pfin(pidf), then
  7. %        pref    = pref(pidf). pref can be user-supplied or defaults to p0.
  8. %      cramer(i) = Cramer-Rao bound = min rms parameter error
  9. %  twofcramer(i) = 2 * Cramer-Rao bound assuming residual spectral
  10. %                  density is constant at value estimated from
  11. %                  filtering residuals through a Rhz filter. Rhz as below.
  12. %        insens  = insensitivity = Cramer-Rao if only this param being ID'd
  13. %        gdop(i) = Geometric Dilution Of Precision = Cramer(i)/Insens(i)
  14. %  - his         = Parameter and convergence history. Cols store
  15. %                  [p,nits,max(abs(grad)),max(abs(p-pold)),nllf,wersum]
  16. %  - hes1,2      = Previous hes matrices. Gives info if hes = NaN when bombs.
  17. %                  Diagonals only stored to save space. Change to whole
  18. %                  mat. if desired. Often zero diag component => user error.
  19. %  INPUTS
  20. %  - uydata  = [Inputs(1:ndp,1:#inputs),Outputs(1:ndp,1:#outputs)]. GLOBAL
  21. %  - p2snam  = Name of user written function converting p to state space model
  22. %  - p0      = parameter vector p is reset to p0 at start of MMLE
  23. %  - pref    = Reference values of parameters subtracted from pfin in
  24. %               third column of RESULTS matrix. If undefined or not of
  25. %               same size as p0, pref is reset to p0.
  26. %  - rms0    = Standard deviation of a-priori parameter knowledge (NB, > 0)
  27. %               Create it ONLY if you want to weight estimates towards pref.
  28. %  - useghes : Fortran routine GHES.EXE is used if useghes exists
  29. % linesearch : Linesearch along Newton direction occurs if linesearch exists.
  30. %  usermods  : Create this macro and it will be executed before step 0 printout.
  31. %   diagrr   : Estimated innovations cov. is made diagonal if diagrr exists
  32. %  - pidq    = Indices of parameters to be identified in quadratic phase
  33. %  - pidm    =      ditto for Marquard stage
  34. %  - pidf    =      ditto for constrained Newton and gg estimation phases.
  35. %  - pert    = If scalar, parameters perturbed by it to compute sensitivities
  36. %               until gg estimation whereafter p(pidf(i)) is perturbed by
  37. %               insens(pidf(i))*pert. Suggest pert=.001 . If vector sized p0,
  38. %               perturbation is fixed at pert(pidf(i))
  39. %  - gg0     = Innovations covariance gg is reset to this when mmle starts
  40. %  - opt     = Option vector:  Defaults to [0 5 5 5 .02 .05 .001 1]
  41. %                        Elements are, in order:
  42. %               qstep :  Max no of quadratic steps using pidq
  43. %               mstep :           Marquard ...           pidm
  44. %               nstep :           constrained Newton ... pidf   f for final!
  45. %               gstep :           gg determination   ... pidf
  46. %               marqf : Value of marq to cause early termination
  47. %                       - marq starts at imag(marqf), else marq0 = 1
  48. %               fconv : wersum change < fconv triggers gg determination
  49. %               gconv :     ...        < gconv triggers Cramer-Rao calculation
  50. %               rhz   : Cutoff Hz of filter used to determine innovation (rr)
  51. %                       spectral density for filtered C-R bound calculation.
  52. %  MODEL
  53. %        See the help file for ml_p2ss1.m and its internal comments
  54. %        Note that process noise absolutely zeros disables the Kalman gain
  55. %--------------------------------------------------------------------------
  56. %  DEMO FILES : Run ml_demo1,   ml_demo2,   ml_demo3.   See their comments.
  57. '==================================== MMLE ===================================';
  58. disp(' ');disp(ans);stime=clock;
  59.  
  60. %=================================================================
  61. %        CHECK INPUT OPTION DATA AND DEFAULT WHERE POSSIBLE
  62. %=================================================================
  63.  
  64. p=p0;%  ==> p0 IS UNDEFINED
  65. p0(pidq);%  ==> INDICES OF pidq MUST EXIST IN p0
  66. p0(pidm);%  ==> INDICES OF pidm MUST EXIST IN p0
  67. p0(pidf);%  ==> INDICES OF pidf MUST EXIST IN p0
  68. perts=pert;if prod(size(pert))==1, perts=pert*exp(0*p0);end %1 DEFAULT PERTS
  69. if exist('diagrr')==1,diag_rr=1;else diag_rr=0;end%1a DEFAULT FULL rr
  70.  
  71. 'linesearch=0;';if exist('linesearch')==0,eval(ans);end%2 DEFAULT NO LINESEARCH
  72.  
  73. 'wapriori=0*p0;';if exist('rms0')==0,eval(ans);else rms0=rms0,%DEFAULT WAPRIORI
  74.    wapriori=exp(-2*log(rms0));end%3 ==> MAKE ALL ELEMENTS OF rms0 >0
  75.    wapriori+p0;% ==> rms0 AND p0 INCOMPATIBLE
  76.  
  77. 'ans=pref;';if exist('pref')==1,eval(ans);else pref=p0;end%4 DEFAULT PREF
  78. if max(size(pref)-size(p0))~=0,pref=p0;end
  79.  
  80. 'opt=[0 5 5 5 .02 .05 .001 1];';if exist('opt')==0,eval(ans);end%5 DEFAULT opt
  81.    [opt+ones(1,8)];% ==> OPT MUST BE    1 * 8
  82.  
  83. %=================================================================
  84. %                  CREATE MACROS FOR LATER USE
  85. %          These can be used interactively if desired
  86. %          Examine later code to see how they are used
  87. %=================================================================
  88. if exist('useghes')==1, %
  89.   dograd='eval(domodl),load fromghes ,eval(dounwind)';
  90.   domodl='hes2=hes1;hes1=diag(hes);llf=mmlemods(p,pid,p2snam,perts,gg);!ghes';
  91.  
  92.   ans=['npid=length(pid);grad=gradhes(1:npid)+',...
  93.      '(p(pid)-pref(pid))''.*wapriori(pid)'';',... %ADD A-PRIORI GRAD COMPONENT
  94.      'hes=zeros(npid);pos=[0 cumsum(npid:-1:1)]+npid;',...
  95.      'for col=1:npid,hes(col:npid,col)=gradhes(pos(col)+1:pos(col+1));'];
  96.   dounwind=[ans,  'end;hes=hes+hes''-diag(diag(hes)',...
  97.      '-wapriori(pid)'');',...%                      ADD A-PRIORI HES COMPONENT
  98.      'if rcond(hes)<1e-10,rcond_hes=rcond(hes),',...
  99.      'disp(''hes near singular. Possibly add eye(hes)/1e9''),keyboard,end,'];
  100. else
  101.     ['hes2=hes1;hes1=diag(hes);llf=mmlegrad(p,pid,p2snam,perts,gg);',...
  102.      'npid=length(pid);grad=grad+',...
  103.      '(p(pid)-pref(pid))''.*wapriori(pid)'';']; % ADD A-PRIORI GRAD COMP.
  104. dograd=[ans,'hes=hes+diag(wapriori(pid)'');',... %ADD A-PRIORI HES COMPONENT
  105.      'if rcond(hes)<1e-10,rcond_hes=rcond(hes),',...
  106.      'disp(''hes near singular. Possibly add eye(hes)/1e9''),keyboard,end,'];
  107. end
  108.  
  109. dostep=['[p,pstep1,ggn,nllf,ggest]=',...
  110.        'mmlestep(p,pid,p2snam,gg,ggest,fconv,linesearch);'];
  111.  
  112. domarqstep='[p,nllf,marq]=mmlemarq(p,pid,p2snam,gg,marq,nllf);';
  113.  
  114. dolike='llf0=nllf;[nllf,wersum,rr,dt,k]=mmlelike(p,p2snam,gg);';
  115.  
  116. doprt=['row2str=[nits sum(p(pidf)) ggscal*[sum(sum(gg)) sum(sum(rr))],',...
  117.       'max(abs(grad)),max(abs(p-pold)),nllf,wersum];',...
  118.       'pold=p;ans=[];for i=1:length(row2str),',...
  119.       'ans1=sprintf(''%f'',row2str(i));ans=[ans,ans1(1:7),''   ''];end;'];
  120.       doprt=[doprt 'disp(ans(1:79));',...
  121.       'if nits>0,his(:,nits+1)=[p nits row2str(5:8)]'';end' ];
  122.  
  123. doquad=['nits=nits+1;disp([lyne,''---------quadratic (pidq)'']);pid=pidq;',...
  124.     'eval(dograd);eval(dostep);p=pstep1;disp(tytl);eval(doprt);eval(dispq)'];
  125.  
  126. domarq=['nits=nits+1;disp([lyne,''---------Marquardt (pidm)'']);'...
  127.        'eval(dograd);'...
  128.        'eval(domarqstep);disp('' '');disp(tytl);eval(doprt);eval(dispm)'];
  129.  
  130. donn=['nits=nits+1;disp([lyne,''constrained Newton (pidf)'']),eval(dograd),',...
  131.       'eval(dostep);p(qparam)=max(p(qparam),',...%  FORCES  Qd >Qd_old/100
  132.       'pold(qparam)/100);disp(tytl),eval(doprt);eval(dispn)'];
  133.  
  134. dogg=['nits=nits+1;disp([lyne,''--gg determination (pidf)'']),gg=ggn;',...
  135.      'eval(dograd);eval(dostep);p(qparam)=max(p(qparam),',...% Qd > Qd_old/100
  136.      'pold(qparam)/100);disp(tytl);eval(doprt);eval(dispg)'];
  137.  
  138. dop2ss='[a,phi,gam,c,d,q,x0,dt,rowinq,b]=eval([p2snam,''(p)'']);';
  139.  
  140. dopcr='ihes=inv(hes);pcr=ihes/sqrt(diag(diag(ihes)))';%PARAMS AT CR BOUND PTS
  141.  
  142. doshes='diag(exp(-.5*log(diag(hes))));shes=ans*hes*ans;';% UNIT DIAG HES
  143. %=================================================================
  144. %           CHECK DIMENSIONS OF MATRICES FROM P2NAM(P)
  145. %=================================================================
  146.  
  147. eval(dop2ss);%  USE FROM KEYBOARD TO GET SYSTEM MATRICES
  148. [1,dt,dt'];%  ==>  dt MUST BE DEFINED AS A SCALAR
  149. [a,a',phi,gam,q,x0'];%  ==>  STATE DIMENSIONS INCOMPATIBLE. x0 MUST BE A ROW
  150. [gam;d];%  ==>  INPUT DIMENSIONS INCOMPATIBLE
  151. [c,d,gg0];%  ==>  OUTPUT DIMENSIONS INCOMPATIBLE
  152. [p-rowinq,1];%  ==>  ROWINQ AND P MUST BE SAME SIZE ROW VECTORS
  153. [uydata(1,:) ;[gam c']];%  ==>  COLS OF uydata <> TOTAL INPUTS AND OUTPUTS
  154.  
  155. %=================================================================
  156. %   INITIALIZE.  DEFINE SOME GLOBALS TO AVOID COPYING TO FUNCTIONS
  157. %=================================================================
  158. if exist('useghes')==1,uydat=uydata';uydat=uydat(:);
  159.    save uydat uydat;clear uydat;end
  160. [ndp,num]=size(uydata);qparam=find(rowinq>0);
  161. yest=zeros(ndp,size(c).*[1 0]);
  162. [tmp1,tmp2]=size(yest);
  163. inovt=zeros(tmp1,tmp2);
  164. hes=zeros(max(size(pidf)));
  165. wersum=0;grad=0;rr=1;e0=1;grade=1;
  166.  
  167. his=[p0 0 0 0 0 0]'*ones(1,sum(opt(1:4)'));hes1=0;
  168. qstep=opt(1);mstep=opt(2);nstep=opt(3);gstep=opt(4);marq0=imag(opt(5));
  169. marqf=real(opt(5));fconv=opt(6);gconv=opt(7);rhz=opt(8);gg=gg0;ggn=gg;
  170. pold=p;
  171.  
  172. ' STEP    PIDF_nsum  GG_osum   RR_nsum   MAXGRAD   MAX|dP|      LLF     WERSUM';
  173. tytl=ans;lyne='---------=---------=---------=---------=---------=--';
  174.  
  175. global uydata
  176. global yest
  177. global inovt
  178. global hes
  179. global wersum
  180. global grad
  181. global wapriori
  182. global pref
  183. global rr
  184. global e0
  185. global grade
  186. global rowinq
  187. global diag_rr
  188.  
  189. %=================================================================
  190. %      DEFINE PARAMETERS TO BE DISPLAYED AT END OF EACH STEP
  191. %=================================================================
  192.  
  193. dispq='ppid=p(pid)';
  194. dispm='ppid=p(pid)';
  195. dispn='ppid=p(pid)';
  196. dispg='ppid=p(pid)';
  197.  
  198. if exist('usermods')==1, eval(usermods);end%      USERMODS
  199. %=================================================================
  200. %         COMPUTE AND DISPLAY LLF FOR INITIAL PARAMETERS
  201. %=================================================================
  202. nits=0;grad=8888888;nllf=[];marq=0; %  INITIALIZE
  203. ggscal=1;if sum(sum(gg))<.01,ggscal=1e6;
  204.   disp('SUM(GG) and SUM(RR) will be multiplied by 1E6 before display'),end%6
  205.  
  206. '----------';if linesearch==0; [setstr(45*ones(1,63)),'mmle : initial'];
  207.    else;[setstr(45*ones(1,52)),'mmle linesearch : initial'];end;disp(ans);
  208.  
  209. disp(tytl);eval(dolike);yest=[];
  210. eval(doprt); %           DISPLAY LLF
  211. llf0=nllf;pstep1=[];
  212.  
  213. %=================================================================
  214. %             QUADRATIC PHASE ( PURE NEWTON ALGORITHM )
  215. %=================================================================
  216.  
  217. if qstep > .1
  218.    pid=pidq;ggest=0;
  219.      while nits < (qstep-.01),eval(doquad);end%8
  220. end%9
  221.  
  222. %=================================================================
  223. %                          MARQUARD PHASE
  224. %=================================================================
  225.  
  226. if mstep>0.1;pid=pidm;marq=marq0; %            MARQUARD ITERATIONS
  227.    if marq==0, marq=1;end; %10 DEFAULT CHOICE
  228.    if marq <= marqf, disp('marqf lower than marq0');end;%11
  229.    fnits=nits+mstep-.01;
  230.    while nits<fnits,
  231.        if marq > (marqf*1.0001)
  232.          eval(domarq)
  233.        else fnits=nits;
  234.        end%12
  235.    end%13
  236. end%14
  237.  
  238. %=================================================================
  239. %              CONSTRAINED NEWTON MINIMIZATION PHASE
  240. %  Process noise is prevented from getting larger than possible
  241. %      given the specified measurement noise covariance.
  242. %=================================================================
  243.  
  244. wersum0=1e10;ggest=gstep>.5; %                          INITIALIZE
  245. if nstep>0.1, %                       CONSTRAINED NEWTON ALGORITHM
  246.    pid=pidf;
  247.    fnits=nits+nstep-.01;
  248.    while nits<fnits, %                      UP TO FSTEP ITERATIONS
  249.        if (ggest==2)|(abs(wersum-wersum0)<fconv*wersum0),
  250.            fnits=0; %                     STOP IF WERSUM CONVERGED
  251.    disp('============ Delta_wersum/wersum < opt(6) ==> Nconvergence')
  252.        else wersum0=wersum;eval(donn)
  253.        end%15
  254.    end%16
  255. end%17
  256.  
  257. %=================================================================
  258. %                     gg ESTIMATION PHASE
  259. %=================================================================
  260.  
  261. if gstep>0.5, % CONSTRAINED NEWTON WITH GG ESTIM. AND F CORRECTION
  262.    pid=pidf;wersum0=1e10;ggest=2;
  263.    fnits=nits+gstep-.01;
  264. %-------------CHECK FOR UNSTABLE KF AFTER BIG CHANGE IN GG
  265.    gg=ggn;flag=1;
  266.        while flag==1;eval(dolike);ei=abs(eig(phi-phi*k*c));flag=max(ei)>1;
  267.          if flag==1;AbsEigKalman=ei',disp('Reduce process noise till < 1')
  268.    keyboard,end,end%18
  269.  
  270. wersum=-1e10;
  271.    while nits<fnits, %                      UP TO GSTEP ITERATIONS
  272.        if abs(wersum0-wersum)<gconv*wersum0, fnits=0;
  273.        else wersum0=wersum;
  274.          if prod(size(pert))==1,
  275.          perts(pid)=exp(-.5*log(diag(hes))')*pert;end;%19   PERTS=INSENS*PERT
  276.        eval(dogg)
  277.        end%20
  278.    end%21
  279. end%22
  280. pfin=p; %                            RETURN FINAL PARAMETER VECTOR
  281. %-----------------------------------------------------------------
  282.  
  283. if (gstep<=.5) | (fnits>0) ==1,%  |= OR
  284.    disp('========== GG convergence incomplete.  Cramer-Rao doubtful')
  285. else
  286.    disp('=========== Delta_wersum/wersum < opt(7) ==> GGconvergence')
  287. end%23
  288. linesearch=0;
  289.  
  290. %=================================================================
  291. %           FILTERED AND CONVENTIONAL CRAMER-RAO BOUNDS
  292. %=================================================================
  293.  
  294. lyne='-------------------------------------------------';
  295. disp([lyne,'------Filtered-Cramer-Rao'])
  296. cramer=sqrt(diag(inv(hes)))';
  297.  
  298. % -------------------------------------------- FILTERED C-R BOUNDS
  299. ['2fcramer = 2 * Cramer-Rao Bounds for residual psd to ',...
  300. num2str(rhz),' hz'];
  301. disp(ans)
  302. [nx,nm]=size(k);[ndp,num]=size(uydata);um=[1:num-nm];
  303. ym=[num-nm+1:num];%    um,ym = LOCATIONS OF INPUT/OUTPUT IN uydata
  304.  
  305. eval(dolike);inovt=uydata(:,ym) - yest; %           GET INOVATIONS
  306. e2at=exp(-2*pi*rhz*dt);
  307. ans=dlsim(eye(nm)*e2at,eye(nm)*(1-e2at)/rhz,...        FILTER THEM
  308.            eye(nm),0*ones(nm),inovt);
  309. jfil=.5*sum(sum(ans/gg.*ans)); %                     FILTERED COST
  310.  
  311. % Now scale cramer with filtered resid pwr/unfiltered pwr(ie nm*ndp)
  312. % times nyquist freq/residual filter freq.  see NASA TP1563 p16.
  313.  
  314. twofcramer=2*cramer*sqrt(jfil/(nm*ndp*rhz*2*dt)); %FILTERED BOUNDS
  315.  
  316. %------------------------------------------------ HESSIAN ANALYSIS
  317. disp([lyne,'------------------RESULTS'])
  318. '     pid      p(pid)     pref     cramer   2fcramer   insens     gdop';
  319. format short;disp(ans);
  320. insens=exp(-.5*log(diag(hes)))';%                  INSENSITIVITIES
  321. gdop=cramer./insens;%              Geometric Dilution Of Precision
  322. results=[pid;p(pid);pref(pid);cramer;twofcramer;insens;gdop]'
  323. clear gdop;clear cramer;clear twofcramer;
  324.  
  325. %=================================================================
  326. %               DOCUMENT OPT, DATE AND CPU TIME
  327. %=================================================================
  328.  
  329. disp([lyne, '-----------------opt(1:7)'])
  330. opt1to7=opt(1:7)
  331. disp([lyne, '-----------------cpu time'])
  332. secs=etime(clock,stime);mins=fix(secs/60);secs=secs-60*mins;
  333. disp(['cpu time =  ',num2str(mins),' minutes,  ',num2str(secs),' seconds'])
  334. ans=[];ans2=fix(clock);for i=1:6,ans1=num2str(ans2(i));
  335. ans=[ans,ans1,'  '];end;disp(['    date = ',ans])
  336. eval(dop2ss)% GET FINAL SYSTEM MATRICES
  337. clear inovt;
  338. disp([lyne, '-----------------end mmle']);
  339. %====================================================== end mmle.m
  340.  
  341.