function [p,pstep1,ggn,nllf,ggest]=...
mmlestep(p,pid,p2snam,gg,ggest,fconv,linesearch,c)
%   FUNCTION USED BY MMLE.M
 %                CONSTRAINED NEWTON WITH gg ESTIMATION
 %   -UPDATES PARAMETERS, CONSTRAINING DIAGONALS OF k*c TO BE LESS THAN 1 TO
 %      GUARANTEE A VALID KALMAN FILTER (MEASUREMENT NOISE POS-DEF).
 %  INPUTS
 %  - p      : parameter vector (row)
 %  - pid    : vector giving indices of parameters in p to be identified
 %  - p2snam : name of user function converting p to state space model
 %  - uydata : [input,output] time history. [ndp * (# inputs+outputs)] GLOBAL
 %  - gg     : innovations covariance. -- You get first guess.
 %  - ggest  : if > 0.5 causes gg to be estimated by program
 %  - fconv  : gg estimation triggers if delta-wersum < fconv
 %  - linesearch : if 1 causes linesearch along Newton direction.
 %  - grad   : grad of cost function - GLOBAL
 %  - hes    : Hessian of cost function - GLOBAL

 %  OUTPUTS
 %  - p      : new parameter vector
 %  - pstep1 : for ggest =0, gives unconstrained Newton update
 %                        1, gives constrained update before correction of q
 %                           to minimise change in k from gg estimation
 %  - ggn    : new innovations covariance
 %  - nllf   : Log - Likelihood function
 %  - wersum : Weighted error sum - converges to 1.0
 %   FUNCTIONS
 %  - calls mmlelike

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

wersumold=wersum;
psold=p(pid); %          SAVE OLD P(PID)
dxsi=-hes\grad;
psnew=psold+dxsi'; %                            STANDARD NEWTON UPDATE
     if max(abs(dxsi))>1e6, disp('Delta_p > 1e6');keyboard,end
pnew=p;pnew(pid)=psnew;
if linesearch==1,
  dpnew=0*pnew;dpnew(pid)=.3*(psnew-psold);%  BEST STEP SIZE - PARABOLA MIN.
  [ans,werhi]=mmlelike(pnew+dpnew,p2snam,gg);
  [ans,wer]=mmlelike(pnew,p2snam,gg);
  if abs(werhi-wer) > 1e-10, [ans,werlo]=mmlelike(pnew-dpnew,p2snam,gg);
      k_line=(werlo-werhi)/(werlo+werhi-2*wer)/2;pquad=pnew+dpnew*k_line;
      [ans,werquad]=mmlelike(pquad,p2snam,gg);
      if werquad<wer;pnew=pquad;k_line_wer=[.3*k_line+1 wer werquad],end
end;end
pstep1=pnew; % ----- SAVE UNCONSTRAINED NEWTON TO RETURN FOR QUADRATIC PHASE

if sum(rowinq(pid))>.5; %           CONSTRAIN IF STATE NOISE INCLUDED IN PID
predict=e0+grade*(dxsi); %                      PREDICT DIAG(KC) FOR NEW P
if max(predict)>1, %                        AAH, SOME CONSTRAINT VIOLATORS
       size(e0);wune=ones(ans(1),1);
       rows=find(predict>wune); %                      FIND VIOLATING ROWS
       bvec=wune-e0;bvec=bvec(rows);
       m=grade(rows,:); %                        SEE MAINE & ILIFF'S PAPER
       dxsi=dxsi-hes\m'*(((m/hes)*m')\(m*dxsi-bvec));
       kc_predict=[predict,e0+grade*dxsi]'
pnew(pid)=p(pid)+dxsi';
end;end
%                 CONSTRAINED UPDATE OF ALL BUT GG COMPLETED

[nllf,wersum,rr,dum1,dum2,c]=mmlelike(pnew,p2snam,gg); % GET NEW RR, C
if ggest<.5, p=pnew;return,end % RETURN IF NO GGEST CONTEMPLATED --------
ggest0=ggest;
if abs(wersumold-wersum)<fconv*wersumold, ggest=2;end % FLAG 1ST CONVERGENCE

if ggest<1.5;p=pnew;return ;end; % RETURN IF WERSUM NEVER CONVERGED -----

if exist('diagrr')==1,rr=diag(diag(rr));end % FORCE rr DIAGONAL IF DESIRED
%------------------------------------------------------------ GG ESTIMATION

% MAINE & ILIFF STAGE 2A  =   REPLACE GG WITH SAMPLE INNOVATIONS COVARIANCE.
%
pstep1=pnew; %     RETURN CONSTRAINED UPDATE IN PSTEP1 WHEN GG IS ESTIMATED
[nm,ndata]=size(uydata);
[nm,nx]=size(c);
ggold=gg; %            GK-OLD
ggn=rr; %                                NEW SAMPLE INNOVATIONS COV. => GGN
if min(diag(ggn))<1e-10,ggn=ggn+eye(ggn)/1e10;
disp('min(diag(ggn)) < 1e-10.  1e-10 added to diagonal to improve condition')
end
%
%     STAGE 2B  =   ADJUST Q (TRY TO KEEP K CONSTANT THOUGH GG HAS CHANGED).
%
gkold=diag(inv(ggold));gknew=diag(inv(ggn)); %            G(SUB K) IN EQN 48
srgkon=sqrt((gkold./gknew));
avg=exp(sum(log(srgkon))/nm);
qadjust=ones(1,nx); %                DIMENSION ROW ADJUSTMENT FACTOR VECTOR
for irow=1:nx,
    cik2=c(:,irow).*c(:,irow);
    %            CALCULATE ADJUSTMENT FACTOR FOR Q'S ROWS
    num=sum(cik2.*gkold.*srgkon);
    if abs(num)<1e-10, qadjust(irow)=avg;
    else; qadjust(irow)=num/sum(cik2.*gkold);end
end
for pnum=pid,
    if rowinq(pnum)>0, %    ADJUST PARAMETERS IN ROWS OF Q IDENTIFIED IN PID
       pnew(pnum)=pnew(pnum)*qadjust(rowinq(pnum));
    end
end
p=pnew;
%---------------------------------------------------------------end mmlestep

