Liren Large Software Subsidy 10/10.iso/l/l455/3.ddi/MMLE3.DI$/MMLEGRAD.M

Text File | 1993-03-11 | 5.5 KB | 124 lines

function [llf]=mmlegrad(p,pid,p2snam,perts,gg)
%  FUNCTION USED BY MMLE.M
 %USED BY MMLESTEP & MMLEMARQ TO GET GRADIENT, HESSIAN & OTHER DATA
 %Outputs :
 %  -    llf     = Log Likelihood function. The following outputs are  GLOBAL
 %  -    grad    = Gradient of cost function wrt parameters in PID
 %  -    hes     = Hessian (second gradient) of cost function wrt params in PID
 %  -    c0      = Measurement matrix   %not called by anyone else.
 %  -    e0      = Diagonal of (Kalman gain * C)
 %  -    grade   = Gradient of E0 wrt params in PID
 %  -    rowinq  = Vector giving rows in F in which parameters occur, else [0]
 %  -    proc    = Scalar flag >0 if process noise is being used.
 %
 % Inputs :
 %  -    p       = Parameter vector (row)
 %  -    pid     = Row vector of indices of params in P to be ID'd. eg [1 3 6]
 %  -    p2snam  = Name of user defined function converting P to system.
 %                        e.g. P2SS . <Help ml_p2ss3> gives details.
 %  -    perts   = Amount by which each parameter is perturbed to estimate
 %                 gradients by finite differences. Noncritical- try .001-.0001
 %  -    gg      = Innovations covariance matrix. You get the first guess.
 %  -    uydata  - GLOBAL VARIABLE -- see MMLELIKE.M
 %---------------------------------------------------------------------------

global uydata
global grad
global grade
global hes
global e0
global rowinq
global yest
global inovt
global wersum
global rr

p2snamp=[p2snam,'(p)']; %                             ACCEPT ANY NAME FOR P2SS
[a,phi,gam,c,d,q,x0,dt,rowinq]=eval(p2snamp);c0=c; %        GET NOMINAL SYSTEM
npid=length(pid);[nm,nx]=size(c);[ndp,xx]=size(uydata); %       GET DIMENSIONS
[nx,nu]=size(gam);num=nu+nm;nnorm=ndp;
grade=ones(nx,npid); %                                      DIMENSION MATRICES
proc=sum(diag(q))>0; %       USER SETS q TO ZERO FOR NO KALMAN GAIN
%-------------------YEST,INOV,RR,LLF FOR NOMINAL P----------------------------
if proc==0,
      k=zeros(nx,nm);
else
      k=lqe(a,eye(nx),c,q*q'/dt/dt,gg)*dt; % EQN 30,MAINE & ILIFF, pp588-579
end
inovt=uydata(:,nu+1:num); % = TEMPORARY STORAGE FOR Y_MEAS
yest=dlsim(phi*(eye(nx)-k*c),... %  SIAM J. Appl. Math. Vol41, No3, Dec81
     [gam-phi*k*d,phi*k],c,[d,0*ones(nm)],uydata,x0);

inovt=inovt-yest;
rr=inovt'*inovt/nnorm; %            INNOVATIONS AND SAMPLE COVARIANCE
rtggit=chol(inv(gg))'; %    OUTPUT VECTOR WEIGHTING FOR COST FUNCTION
inovt=inovt*rtggit;yest=yest*rtggit; %             WEIGHT  YEST,INOVT
%-------------------------------------------------------------------

e0=diag(k*c); %                    FOR CONSTRAINING DIAG(E0) < EYE
wersum=sum(sum(inovt.*inovt))/nnorm/nm;%        WEIGHTED ERROR SUM
llf=ndp/2*(wersum*nm-2*log(prod(diag(rtggit))));%       FAST DET
%----------------------------NOW GET GRAD, HES, OF COST WRT PARAMS
psav=p;
phipid=[];gampid=[];ctpid=[];dtpid=[];xpid=[];

k=zeros(nx,nm);
for jj=1:npid; %        COMPUTE AND STACK KF MATRICES FOR ALL PERTURBATIONS
  p(pid(jj))=p(pid(jj))+perts(pid(jj));
  [a,phi,gam,c,d,q,x0,dt,rowinq]=eval(p2snamp); %      GET PERTURBED SYSTEM
  if proc ~ 0; %                      KALMAN FILTER IF PROCESS NOISE
%      k=lqe(a,eye(nx),c,q*q'/dt/dt,gg)*dt; %   CONT LQE FOR DISC! (SEE REF)
    [zzv,zzd]=eig([a' c'/gg*c;q*q'/dt/dt -a]);% THESE 5 LINES EQUIV TO LQE
    zzd=diag(zzd);[zzd,index]=sort(real(zzd));
    chi=zzv(1:nx,index(1:nx));
    lambda=zzv(nx+1:2*nx,index(1:nx));zzs=-real(lambda/chi);
    k=gg\c*zzs;k=k'*dt;
  end;
  phipid=[phipid;phi*(eye(nx)-k*c)];
  gampid=[gampid;[gam-phi*k*d,phi*k]];
  dtpid=[dtpid [d,0*ones(nm)]'*rtggit];
  grade(:,jj)=(diag(k*c)-e0)/perts(pid(jj)); %        GRADIENT OF DIAG(E=K*C)
%
  ctpid=[ctpid c'*rtggit]; %       STACK C' IN ROWS
  xpid=[xpid;x0]; %                STACK INITIAL CONDITIONS IN ROWS
  p=psav;
  fprintf(' ')
end;fprintf('.')

%PHI/GAM/CT/DT....PID MATRICES LOADED WITH ALL KALMAN FILTERS, XPID WITH IC's

hes=zeros(npid);
grad=zeros(npid,1);
onepid=ones(1,npid);fstn=1;
maxel=4094/2;%                  REDUCING THIS SLOWS ALGO, USES LESS MEMORY
%                                YOU CAN MAKE IT GLOBAL BEFORE CALLING MMLE

blocklen=fix(min([maxel/num,maxel/nm/npid]));%   MAX BLOCK LENGTH (MEMORY!)

fstn=1;
while fstn<=ndp, %       RUN KF AND SUM GRAD, HES IN TIME-BLOCKS (MEMORY!)
   lstn=min([ndp,fstn+blocklen-1]); %               END CURRENT DATA BLOCK
   len=lstn-fstn+1;
   yes=yest(fstn:lstn,:)';yes=yes(:); %      LONG COL OF NOMINAL KF OUTPUT
   gradz=yes*onepid;% NOMINAL ESTIMATE
   for i=1:npid           % GET GRADIENT OF INNOVATIONS WRT EACH PARAMETER
       xrows=[1+(i-1)*nx:i*nx]; %     ROWS/COLS STORING THIS KF'S MATRICES
       ycols=[1+(i-1)*nm:i*nm];
       a=phipid(xrows,:);b=gampid(xrows,:);
       x=ltitr(a,b,uydata(fstn:lstn,:),xpid(i,:)); %    PROPAGATE KF STATES
       xpid(i,:)=x(len,:)*a'+uydata(lstn,:)*b'; %          COMPUTE NEXT IC.
       x=x*ctpid(:,ycols)+uydata(fstn:lstn,:)*dtpid(:,ycols); %  KF OUTPUTS
       x=x';
       gradz(:,i)=gradz(:,i)-x(:);%  SUBTRACT COL VECTORS IN A LONG COL
    end,
    smes=(uydata(fstn:lstn,nu+1:num)*rtggit)';
    grad=grad+gradz'*(smes(:)-yes); % ACCUMULAATE GRAD. (INNOV IN PARENTHESES)
    hes=hes+gradz'*gradz; %   ACCUMULATE HES
    fstn=fstn+blocklen; %     START OF NEXT BLOCK
    fprintf(' ') %            SHOW SOME LIFE
end
grad=grad./perts(pid)';ans=diag(exp(-log(perts(pid))));
hes=ans*hes*ans; %       ACCOUNT FOR SIZE OF PERT
fprintf(':');disp(' ')
%------------------------------------------------------------end mmlegrad.m