home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 3.ddi / MMLE3.DI$ / MMLEMODS.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  3.3 KB  |  83 lines
  1. function [llf]=mmlemods(p,pid,p2snam,perts,gg)
  2. %                         USED BY MMLE.M
  3.  % CREATES MODELS FOR COMPILED FORTRAN GHES.EXE ROUTINE TO CALC THE
  4.  % GRADIENT AND HESSIAN OF THE COST FUNCTION. MODELS STORED IN TOGHES.MAT.
  5.  % GHES.EXE READS TOGHES.MAT FOR THE SYSTEM MATRICES, UYDAT.MAT FOR THE
  6.  % TIME HISTORIES, AND WRITES FROMGHES.MAT WHICH CONTAINS GRAD AND HES.
  7.  
  8. %define global variable
  9.  
  10. global uydata
  11. global inovt
  12. global wersum
  13. global grad
  14. global rr
  15. global e0
  16. global grade
  17. global rowinq
  18. global diag_rr
  19.  
  20.  
  21. dnorm=0;% USED TO CHECK IF D MATRICES ALL ZERO, TO SAVE COMPUTATIONS IN GHES
  22.  
  23. p2snamp=[p2snam,'(p)']; %                             ACCEPT ANY NAME FOR P2SS
  24. [a,phi,gam,c,d,q,x0,dt,rowinq]=eval(p2snamp);c0=c; %        GET NOMINAL SYSTEM
  25. npid=length(pid);[nm,nx]=size(c);[ndp,xx]=size(uydata); %       GET DIMENSIONS
  26. [nx,nu]=size(gam);num=nu+nm;nnorm=ndp;
  27. grade=ones(nx,npid); %                                      DIMENSION MATRICES
  28. proc=sum(diag(q))>0; %  USER SETS q TO ZERO FOR NO KALMAN GAIN
  29.  %------------------YEST,INOV,RR,LLF FOR NOMINAL P----------------------------
  30. if proc==0,
  31.       k=zeros(nx,nm);
  32. else
  33.       k=lqe(a,eye(nx),c,q*q'/dt/dt,gg)*dt; % EQN 30,MAINE & ILIFF, pp588-579
  34. end%                                   SIAM J. Appl. Math. Vol41, No3, Dec81
  35.  %
  36. phinom=phi*(eye(nx)-k*c);gamnom=[gam-phi*k*d,phi*k];
  37.      dnom=[-d,eye(nm)];
  38. dnorm=dnorm+norm(d,1);
  39. inovt=dlsim(phinom,gamnom,-c,dnom,uydata,x0);
  40.  %
  41.  %
  42. rr=inovt'*inovt/nnorm; %            INNOVATIONS AND SAMPLE COVARIANCE
  43. rtggi=chol(inv(gg)); %    OUTPUT VECTOR WEIGHTING FOR COST FUNCTION
  44. inovt=inovt*rtggi';%yest=yest*rtggi'; %             WEIGHT  YEST,INOVT
  45.  %-------------------------------------------------------------------
  46. e0=diag(k*c); %                    FOR CONSTRAINING DIAG(E0) < EYE
  47. wersum=sum(sum(inovt.*inovt))/nnorm/nm;%        WEIGHTED ERROR SUM
  48. llf=ndp/2*(wersum*nm-2*log(prod(diag(rtggi))));%
  49.  %----------------------------NOW GET GRAD, HES, OF COST WRT PARAMS
  50. psav=p;
  51. s=[phinom,gamnom;rtggi*[c,d],zeros(nm)];xpid=x0';  % STACK NOMINAL PLANT
  52.  
  53. k=zeros(nx,nm);
  54. for jj=1:npid; %        COMPUTE AND STACK KF MATRICES FOR ALL PERTURBATIONS
  55.   p(pid(jj))=p(pid(jj))+perts(pid(jj));
  56.   [a,phi,gam,c,d,q,x0,dt,rowinq]=eval(p2snamp); %      GET PERTURBED SYSTEM
  57.   if proc ~ 0; %                      KALMAN FILTER IF PROCESS NOISE
  58.  %      k=lqe(a,eye(nx),c,q*q'/dt/dt,gg)*dt; %   CONT LQE FOR DISC! (SEE REF)
  59.     [zzv,zzd]=eig([a' c'/gg*c;q*q'/dt/dt -a]);% THESE 5 LINES EQUIV TO LQE
  60.     zzd=diag(zzd);[zzd,index]=sort(real(zzd));
  61.     chi=zzv(1:nx,index(1:nx));
  62.     lambda=zzv(nx+1:2*nx,index(1:nx));zzs=-real(lambda/chi);
  63.     k=gg\c*zzs;k=k'*dt;
  64.   end;
  65.   dnorm=dnorm+norm(d,1); %
  66.   s=[s;[phi*(eye(nx)-k*c),[gam-phi*k*d,phi*k];rtggi*[c,d],zeros(nm)]];
  67.   grade(:,jj)=(diag(k*c)-e0)/perts(pid(jj)); %        GRADIENT OF DIAG(E=K*C)
  68.  %
  69.   xpid=[xpid x0']; %                STACK INITIAL CONDITIONS IN ROWS
  70.   p=psav;
  71.   fprintf(' ')
  72. end;
  73. fprintf('.')
  74.  %------------------------------------------interface to ghes.exe
  75. s=s';
  76. size([phi gamnom]);abcol=ans(2);cdcol=nx+nu*(dnorm>0);
  77. rtggit=rtggi';
  78. toghes=[npid,nx,nm,abcol,cdcol,num,-7,-8,-9,-10,perts(pid),xpid(:)',...
  79. rtggit(:)',s(:)', zeros(1,50)];
  80. save toghes toghes
  81. %----------------------------------------- end
  82.  
  83.