home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 3.ddi / MMLE3.DI$ / MMLESTEP.M < prev   
Encoding:
Text File  |  1993-03-11  |  4.5 KB  |  111 lines
  1. function [p,pstep1,ggn,nllf,ggest]=...
  2. mmlestep(p,pid,p2snam,gg,ggest,fconv,linesearch,c)
  3. %   FUNCTION USED BY MMLE.M
  4.  %                CONSTRAINED NEWTON WITH gg ESTIMATION
  5.  %   -UPDATES PARAMETERS, CONSTRAINING DIAGONALS OF k*c TO BE LESS THAN 1 TO
  6.  %      GUARANTEE A VALID KALMAN FILTER (MEASUREMENT NOISE POS-DEF).
  7.  %  INPUTS
  8.  %  - p      : parameter vector (row)
  9.  %  - pid    : vector giving indices of parameters in p to be identified
  10.  %  - p2snam : name of user function converting p to state space model
  11.  %  - uydata : [input,output] time history. [ndp * (# inputs+outputs)] GLOBAL
  12.  %  - gg     : innovations covariance. -- You get first guess.
  13.  %  - ggest  : if > 0.5 causes gg to be estimated by program
  14.  %  - fconv  : gg estimation triggers if delta-wersum < fconv
  15.  %  - linesearch : if 1 causes linesearch along Newton direction.
  16.  %  - grad   : grad of cost function - GLOBAL
  17.  %  - hes    : Hessian of cost function - GLOBAL
  18.  
  19.  %  OUTPUTS
  20.  %  - p      : new parameter vector
  21.  %  - pstep1 : for ggest =0, gives unconstrained Newton update
  22.  %                        1, gives constrained update before correction of q
  23.  %                           to minimise change in k from gg estimation
  24.  %  - ggn    : new innovations covariance
  25.  %  - nllf   : Log - Likelihood function
  26.  %  - wersum : Weighted error sum - converges to 1.0
  27.  %   FUNCTIONS
  28.  %  - calls mmlelike
  29.  
  30. global uydata
  31. global grad
  32. global hes
  33. global wersum
  34. global rr
  35. global e0
  36. global grade
  37. global rowinq
  38.  
  39. wersumold=wersum;
  40. psold=p(pid); %          SAVE OLD P(PID)
  41. dxsi=-hes\grad;
  42. psnew=psold+dxsi'; %                            STANDARD NEWTON UPDATE
  43.      if max(abs(dxsi))>1e6, disp('Delta_p > 1e6');keyboard,end
  44. pnew=p;pnew(pid)=psnew;
  45. if linesearch==1,
  46.   dpnew=0*pnew;dpnew(pid)=.3*(psnew-psold);%  BEST STEP SIZE - PARABOLA MIN.
  47.   [ans,werhi]=mmlelike(pnew+dpnew,p2snam,gg);
  48.   [ans,wer]=mmlelike(pnew,p2snam,gg);
  49.   if abs(werhi-wer) > 1e-10, [ans,werlo]=mmlelike(pnew-dpnew,p2snam,gg);
  50.       k_line=(werlo-werhi)/(werlo+werhi-2*wer)/2;pquad=pnew+dpnew*k_line;
  51.       [ans,werquad]=mmlelike(pquad,p2snam,gg);
  52.       if werquad<wer;pnew=pquad;k_line_wer=[.3*k_line+1 wer werquad],end
  53. end;end
  54. pstep1=pnew; % ----- SAVE UNCONSTRAINED NEWTON TO RETURN FOR QUADRATIC PHASE
  55.  
  56. if sum(rowinq(pid))>.5; %           CONSTRAIN IF STATE NOISE INCLUDED IN PID
  57. predict=e0+grade*(dxsi); %                      PREDICT DIAG(KC) FOR NEW P
  58. if max(predict)>1, %                        AAH, SOME CONSTRAINT VIOLATORS
  59.        size(e0);wune=ones(ans(1),1);
  60.        rows=find(predict>wune); %                      FIND VIOLATING ROWS
  61.        bvec=wune-e0;bvec=bvec(rows);
  62.        m=grade(rows,:); %                        SEE MAINE & ILIFF'S PAPER
  63.        dxsi=dxsi-hes\m'*(((m/hes)*m')\(m*dxsi-bvec));
  64.        kc_predict=[predict,e0+grade*dxsi]'
  65. pnew(pid)=p(pid)+dxsi';
  66. end;end
  67. %                 CONSTRAINED UPDATE OF ALL BUT GG COMPLETED
  68.  
  69. [nllf,wersum,rr,dum1,dum2,c]=mmlelike(pnew,p2snam,gg); % GET NEW RR, C
  70. if ggest<.5, p=pnew;return,end % RETURN IF NO GGEST CONTEMPLATED --------
  71. ggest0=ggest;
  72. if abs(wersumold-wersum)<fconv*wersumold, ggest=2;end % FLAG 1ST CONVERGENCE
  73.  
  74. if ggest<1.5;p=pnew;return ;end; % RETURN IF WERSUM NEVER CONVERGED -----
  75.  
  76. if exist('diagrr')==1,rr=diag(diag(rr));end % FORCE rr DIAGONAL IF DESIRED
  77. %------------------------------------------------------------ GG ESTIMATION
  78.  
  79. % MAINE & ILIFF STAGE 2A  =   REPLACE GG WITH SAMPLE INNOVATIONS COVARIANCE.
  80. %
  81. pstep1=pnew; %     RETURN CONSTRAINED UPDATE IN PSTEP1 WHEN GG IS ESTIMATED
  82. [nm,ndata]=size(uydata);
  83. [nm,nx]=size(c);
  84. ggold=gg; %            GK-OLD
  85. ggn=rr; %                                NEW SAMPLE INNOVATIONS COV. => GGN
  86. if min(diag(ggn))<1e-10,ggn=ggn+eye(ggn)/1e10;
  87. disp('min(diag(ggn)) < 1e-10.  1e-10 added to diagonal to improve condition')
  88. end
  89. %
  90. %     STAGE 2B  =   ADJUST Q (TRY TO KEEP K CONSTANT THOUGH GG HAS CHANGED).
  91. %
  92. gkold=diag(inv(ggold));gknew=diag(inv(ggn)); %            G(SUB K) IN EQN 48
  93. srgkon=sqrt((gkold./gknew));
  94. avg=exp(sum(log(srgkon))/nm);
  95. qadjust=ones(1,nx); %                DIMENSION ROW ADJUSTMENT FACTOR VECTOR
  96. for irow=1:nx,
  97.     cik2=c(:,irow).*c(:,irow);
  98.     %            CALCULATE ADJUSTMENT FACTOR FOR Q'S ROWS
  99.     num=sum(cik2.*gkold.*srgkon);
  100.     if abs(num)<1e-10, qadjust(irow)=avg;
  101.     else; qadjust(irow)=num/sum(cik2.*gkold);end
  102. end
  103. for pnum=pid,
  104.     if rowinq(pnum)>0, %    ADJUST PARAMETERS IN ROWS OF Q IDENTIFIED IN PID
  105.        pnew(pnum)=pnew(pnum)*qadjust(rowinq(pnum));
  106.     end
  107. end
  108. p=pnew;
  109. %---------------------------------------------------------------end mmlestep
  110.  
  111.