ode15s_old.m

国外经典书籍MULTIVARIABLE FEEDBACK CONTROL-多变量反馈控制 的源码

    hinvGak = h * invGa(k);
    nconhk = 0;
    [L,U] = lu(Mt - hinvGak * dfdy);
    ndecomps = ndecomps + 1; 		% stats
    havrate = false;
  end
  
  % LOOP FOR ADVANCING ONE STEP.
  nofailed = true; 			% no failed attempts
  while true
    
    gotynew = false; 			% is ynew evaluated yet?
    while ~gotynew

      % Compute the constant terms in the equation for ynew.
      psi = (dif(:,K) * G(K)) * invGa(k);

      % Predict a solution at t+h.
      tnew = t + h;
      if k == 1
	pred = y + dif(:,1);
      else
	pred = y + sum(dif(:,K).').';
      end
      ynew = pred;
      
      % The difference, difkp1, between pred and the final accepted 
      % ynew is equal to the backward difference of ynew of order
      % k+1. Initialize to zero for the iteration to compute ynew.
      difkp1 = zerosneq; 
      invwt = 1 ./ (max(abs(y), abs(ynew)) + threshold);
      minnrm = 100*eps*norm(ynew .* invwt,inf);

      % Mtnew is required in the RHS function evaluation.
      if ~constantM
	Mtnew = feval(mass,tnew);
      end

      % Iterate with simplified Newton method
      tooslow = false;
      for iter = 1:maxit
        del = U \ (L \ (hinvGak*feval(ydot,tnew,ynew) -  Mtnew*(psi+difkp1)));
        newnrm = norm(del .* invwt,inf);
        difkp1 = difkp1 + del;
        ynew = pred + difkp1;
 
        if newnrm <= minnrm
          gotynew = true;
          break;
	elseif iter == 1
          if havrate
            errit = newnrm * rate / (1 - rate) ;
	    if errit <= 0.05*rtol 	% More stringent when using old rate
              gotynew = true;
              break;
            end
	  else
	    rate = 0;
	  end
	elseif newnrm > 0.9*oldnrm
	  tooslow = true;
	  break;
	else
	  rate = max(0.9*rate, newnrm / oldnrm);
	  havrate = true;                 
	  errit = newnrm * rate / (1 - rate);
	  if errit <= 0.5*rtol             
	    gotynew = true;
	    break;
	  elseif iter == maxit            
	    tooslow = true;
	    break;
	  elseif 0.5*rtol < errit*rate^(maxit-iter)
	    tooslow = true;
	    break;
	  end
	end
	
	oldnrm = newnrm;
      end 				% end of Newton loop
      nfevals = nfevals + iter; 	% stats
      nsolves = nsolves + iter; 	% stats
      
      if tooslow
	nfailed = nfailed + 1; 		% stats
	if absh <= hmin
	  fprintf('Convergence failure at %e with step size of %e\n',t,h);
	  if nargout ~= 0
	    tout = tout(1:nout);
	    yout = yout(1:nout,:);
	    stats = [nsteps; nfailed; nfevals; npds; ndecomps; nsolves];
	  end
	  return;
	end
	% Speed up the iteration by forming J(t,y) or reducing h.
	if ~JMcurrent
	  if ~constantJ
	    if notanalyticJ
	      f0 = feval(ydot,t,y);
	      [dfdy,fac,g,nF] = numjac(ydot,t,y,f0,atol,fac,vectorized,Js,g);
	      nfevals = nfevals + nF + 1; % stats
	    else
	      dfdy = feval(analyticJ,t,y);
	    end
	    npds = npds + 1; 		% stats
	  end
	  JMcurrent = true;
	else
	  hnew = max(0.3 * absh, hmin);
	  last = false;

	  difRU = cumprod((kI - 1 - kJ*(hnew/absh)) ./ kI) * difU;
	  dif(:,K) = dif(:,K) * difRU(K,K);
	  absh = hnew;
	  h = tdir * absh;
	  
	  hinvGak = h * invGa(k);
	  nconhk = 0;
	end
	[L,U] = lu(Mt - hinvGak * dfdy);
	ndecomps = ndecomps + 1; 	% stats
	havrate = false;
      end   
    end     % end of while loop for getting ynew
    
    % difkp1 is now the backward difference of ynew of order k+1.
    err = norm(difkp1 .* invwt,inf) * erconst(k);
    
    if err > rtol 			% Failed step
      nfailed = nfailed + 1; 		% stats
      if absh <= hmin
	fprintf('Step failure at %e with a minimum step size of %e\n', t, h);
	if nargout ~= 0
	  tout = tout(1:nout);
	  yout = yout(1:nout,:);
	  stats = [nsteps; nfailed; nfevals; npds; ndecomps; nsolves];
	end
	return;
      end
      
      if nofailed
	nofailed = false;
	hnew = absh * max(0.1, 0.833*(rtol/err)^(1/(k+1))); % 1/1.2
	if k > 1
	  errkm1 = norm((dif(:,k) + difkp1) .* invwt,inf) * erconst(k-1);
	  hkm1 = absh * max(0.1, 0.769*(rtol/errkm1)^(1/k)); % 1/1.3
	  if hkm1 > hnew
	    k = k - 1;
	    K = 1:k;
	    hnew = hkm1;
	  end
	end
      else
	hnew = 0.5 * absh;
      end
      hnew = max(hnew, hmin);
      last = false;

      difRU = cumprod((kI - 1 - kJ*(hnew/absh)) ./ kI) * difU;
      dif(:,K) = dif(:,K) * difRU(K,K);
      absh = hnew;
      h = tdir * absh;

      hinvGak = h * invGa(k);
      nconhk = 0;
      [L,U] = lu(Mt - hinvGak * dfdy);
      ndecomps = ndecomps + 1; 		% stats
      havrate = false;
      
      if haveoutfun
	if feval(outfun,tnew,ynew,'failed') == 1
	  if nargout ~= 0
	    tout = tout(1:nout);
	    yout = yout(1:nout,:);
	    stats = [nsteps; nfailed; nfevals; npds; ndecomps; nsolves];
	  end
	  return;
	end
      end
      
    else 				% Successful step
      break;
      
    end
  end

  dif(:,k+2) = difkp1 - dif(:,k+1);
  dif(:,k+1) = difkp1;
  for j = k:-1:1
    dif(:,j) = dif(:,j) + dif(:,j+1);
  end
  
  if nargout ~= 0
    if outflag == 2 			% computed points, no refinement
      nout = nout + 1;
      if nout > length(tout)
	tout = [tout; zeros(chunk,1)];
	yout = [yout; zeros(chunk,neq)];
      end
      tout(nout) = tnew;
      yout(nout,:) = ynew.';
    elseif outflag == 3 		% computed points, with refinement
      i = nout+1:nout+refine-1;
      nout = nout + refine;
      if nout > length(tout)
	tout = [tout; zeros(chunk,1)]; 	% requires chunk >= refine
	yout = [yout; zeros(chunk,neq)];
      end
      tout(i) = tnew + h*S';
      yout(i,:) = (ynew(:,ones1r) + dif(:,K) * p(K,:)).';
      tout(nout) = tnew;
      yout(nout,:) = ynew.';
    elseif outflag == 1 		% output only at tspan points
      oldnout = nout;
      while true
	if next > ntspan 
	  break;
	elseif tdir * (tnew - tspan(next)) < 0
	  break;
	end
	nout = nout + 1; 		% tout and yout are already allocated
	tout(nout) = tspan(next);
	s = (tspan(next) - tnew) / h;
	yout(nout,:) = (ynew + dif(:,K) * cumprod((s+K-1) ./ K).').';
	next = next + 1;
      end
    end
    
    if haveoutfun
      if outflag == 3
	for i = nout-refine+1:nout-1
	  feval(outfun,tout(i),yout(i,:).','interp');
	end
      elseif outflag == 1
	for i = oldnout+1:nout
	  feval(outfun,tout(i),yout(i,:).','interp');
	end
      end	
      if feval(outfun,tnew,ynew) == 1
	tout = tout(1:nout);
	yout = yout(1:nout,:);
	stats = [nsteps; nfailed; nfevals; npds; ndecomps; nsolves];
	return;
      end
    end
    
  elseif haveoutfun
    if outflag == 3 			% computed points, with refinement
      for i = 1:refine-1
	yinterp = ynew + dif(:,K) * cumprod((S(i)+K-1) ./ K).';
	feval(outfun,tnew + h*S(i),yinterp,'interp');
      end
    elseif outflag == 1 		% output only at tspan points
      while true
	if next > ntspan 
	  break;
	elseif tdir * (tnew - tspan(next)) < 0
	  break;
	end
	s = (tspan(next) - tnew) / h;
	yinterp = ynew + dif(:,K) * cumprod((s+K-1) ./ K).';
	feval(outfun,tspan(next),yinterp,'interp');
	next = next + 1;
      end
    end
    if feval(outfun,tnew,ynew) == 1
      return;
    end
  end
  
  nconhk = min(nconhk+1,maxk+2);
  if nconhk >= k + 2
    hopt = min(hmax,absh/max(0.1,1.2*(err/rtol)^(1/(k+1))));
    kopt = k;
    if k > 1
      errkm1 = norm(dif(:,k) .* invwt,inf) * erconst(k-1);
      hkm1 = min(hmax,absh/max(0.1,1.3*(errkm1/rtol)^(1/k)));
      if hkm1 > hopt 
	hopt = hkm1;
	kopt = k - 1;
      end
    end
    if k < maxk
      errkp1 = norm(dif(:,k+2) .* invwt,inf) * erconst(k+1);
      hkp1 = min(hmax,absh/max(0.1,1.4*(errkp1/rtol)^(1/(k+2))));
      if hkp1 > hopt 
	hopt = hkp1;
	kopt = k + 1;
      end
    end
    if hopt > absh
      hnew = hopt;
      if k ~= kopt
	k = kopt;
	K = 1:k;
      end
      newhk = true;
    end
  end
  
  % Advance the integration one step.
  t = tnew;
  y = ynew;
  nsteps = nsteps + 1; 			% stats
  JMcurrent = constantJ & constantM;
  if ~constantM
    Mt = Mtnew;
  end
  
end

if tdir * (tfinal - tnew) > 0
   fprintf('Singularity likely near %g.\n',tnew)
end

if printstats 				% print cost statistics
  fprintf('%g successful steps\n', nsteps);
  fprintf('%g failed attempts\n', nfailed);
  fprintf('%g calls to ydot\n', nfevals);
  fprintf('%g partial derivatives\n', npds);
  fprintf('%g LU decompositions\n', ndecomps);
  fprintf('%g solutions of linear systems\n', nsolves);
end

if nargout ~= 0
  tout = tout(1:nout);
  yout = yout(1:nout,:);
  stats = [nsteps; nfailed; nfevals; npds; ndecomps; nsolves];
end