varpro.m

椭圆拟合的相关介绍与数学运算方法

function [vn, vl, err, step] = varpro (HOOK, Y, vn, vl, OPTIONS, ...
                 P0, P1, P2, P3, P4, P5, P6, P7, P8, P9);
%VARPRO         Variable Projection Algorithm
%
% [vn, vl, err, step] = ...
%      varpro (HOOK, Y, vn, vl, OPTIONS, P0, P1, ... );
%
%   Computes values for the non-linear 'vn' and the linear 'vl'
%   to approximate 'Y' in the least-squares sense.
%
%   On input, 'vn' contains approximative values of the solution
%   (as good as possible); 'vl' is used for its size only.
%   
%   err == 0: everything OK.
%   err == 1: too many iterations.
%
%   Algorithm by GOLUB/PEREYRA:
%   "The differentiation of pseudo-inverses and nonlinear least
%    squares problems whose variables separate".
%   SIAM J. Num. Anal. 10(2), april 1973.
%
% HOOK is a user-supplied function receiving 
%   (what, vn, OPTIONS, P0, ...) as arguments.
%   It evaluates
%     - function values (what == 'Phi') with linear factor
%     - function values without linear factor ('Psi')
%     - derivatives ('DPhi' and 'DPhi_Inc' for packed version)
%       ('DPsi' for the independent term).
%     - incidence matrix ('Inc'), to tell that a function
%       does not depend on certain variables.
%     - (possibly) Cholesky factor of positive definite
%       symmetric matrix to be used in the marquart step.
%
% if     (what == 'Phi'),
%   Ret := Phi = [Phi(1) ... Phi(4)];
% elseif (what == 'DPhi' | 'DPhi_Inc'),
%   Ret := Derivative of Phi 
%     [dPhi/dvn(1) ... dPhi/dvn(nof-nonlinear)],
%     that is, jacobians are stored sequentially into Ret.
%   For 'DPhi_Inc', some columns are left out,
%     (all dPhi(j)/dvn(k) where INC(i,j) == 0),
%     INC is passed as the last arg to function. 
% elseif (what == 'Inc'),
%   Ret := incidence matrix, to tell the 'varpro' routine that
%     some Phi(i) does not depend from vn(j). See 'DPhi_Inc'
% elseif (what == 'Psi'),
%   Ret := Psi, function without linear factor
% elseif (what == 'DPsi'),
%   Ret := Derivative (jacobian) of 'Psi'
% elseif (what == 'LM'),
%   Ret := Cholesky factor for Levenberg-Marquardt 
%          positive-definite matrix (default eye)
% end     

  fun = [HOOK];
  arg = [];
  if ~any(fun<48)
    fun = [fun, '('];
    arg = [arg, ', vn, OPTIONS'];
    for i = 1:nargin - 4
      arg = [arg,',P',int2str(i-1)];
    end
    arg_open = arg;
    arg = [arg, ')'];
  end
  if (nargin < 5), OPTIONS=[]; end

  OPTI_EPS     = 1;
  OPTI_LMSPEC  = 2;
  OPTI_NOFSTEP = 3;
  OPTI_ERRPR   = 4;
  OPTI_PACKM   = 5; % should memory be packed ?
  OPTI_MARQ    = 6;
  OPTI_PSI     = 7;

  ERR_OK       = 0;
  ERR_NOFSTEP  = 1;

  epss  = 1.0e-8;
  epsr  = 1.0e-5;

  k = size(vn,1);
  m = size(Y,1);
  n = size(vl,1);

  F      = eye(k,k);
  lambda = 1.0;
  omega  = sqrt(0.5);
  nofstep= 100;
  err    = ERR_OK;
  errpr  = 1;
  packm  = 1;
  marq   = 1;
  psi    = 0;

  for i = 1:size(OPTIONS,1),
    kind = OPTIONS(i,1);
    if     (kind == OPTI_EPS)     epsr = OPTIONS(i,2);
    elseif (kind == OPTI_LMSPEC)  F = eval([fun, '''LM''', arg]);
    elseif (kind == OPTI_NOFSTEP) nofstep = OPTIONS(i,2);
    elseif (kind == OPTI_ERRPR)   errpr = OPTIONS(i,2);
    elseif (kind == OPTI_PACKM)   packm = OPTIONS(i,2);
    elseif (kind == OPTI_MARQ)    marq = OPTIONS(i,2);
    elseif (kind == OPTI_PSI)     psi = OPTIONS(i,2);
    else                          error ('unknown option');
    end
  end

  if (~psi), YY = Y; end;
  step = 0;
  if (k > 0), % number of non-linear variables  
    normr = 1;
    norma = 1;
    while (normr > epsr*norma),

      Phi  = eval ([fun, '''Phi''', arg]);
      %
      % Phi (i,k) is k-th component of phi (i)
      %

      if (packm),
        Inc  = eval ([fun, '''Inc''', arg]);
        DPhi = eval ([fun, '''DPhi_Inc''', arg_open, ',Inc', ')']);
      else
        DPhi = eval ([fun, '''DPhi''', arg]);
      end
      %
      % DPhi contains columns dphi(i)/dalpha(k), for
      %   - unpacked at DPhi(:, 1+(k-1)*n + i)
      %   - packed in the same order, but only for Inc(i,k) != 0.
      %

      [Q, T, S, r] = varpro_qr(Phi, epss);

      if (psi), 
        Psi = eval ([fun, '''Psi''', arg]);
        YY  = Y - Psi;
      end
      v = Q*YY;
      C = Q*DPhi;
      x = S(:,1:r)*(T(1:r,1:r)\v(1:r));

      if (packm),
        U = zeros(n,k); Dx = zeros(m-r,k);
        p = 0; % column into DPhi
        for i = 1:k,
          for j = 1:n,
            if (Inc(j,i)),
              p = p+1;
              U(j,i)  = C(r+1:m,p)'*v(r+1:m);
              Dx(:,i) = Dx(:,i) + C(r+1:m,p)*x(j);
            end
          end
        end 
      else
        U = []; Dx = [];
        for i = 1:k,
          U = [U, C(r+1:m,1+(i-1)*n:i*n)'*v(r+1:m)];
          Dx= [Dx, C(r+1:m,1+(i-1)*n:i*n)*x];
        end
      end        

      H = S'*U;
      W = T(1:r,1:r)'\H(1:r,:);

      B   = -Q'*[W; Dx];
      res = Q(r+1:m,:)'*v(r+1:m);

      if (psi),
        DPsi= eval ([fun, '''DPsi''', arg]);
        B   = B - Q(r+1:m,:)'*Q(r+1:m,:)*DPsi;
      end

      if (marq),
        vvn = vn;
        istep = 0;
        while (1),
          h   = - [B; lambda*F]\[res; zeros(k,1)];
          vn  = vvn + h;
          Phi = eval ([fun, '''Phi''', arg]);
          if (psi), 
            Psi = eval ([fun, '''Psi''', arg]);
            YY  = Y - Psi;
          end
          [Q, T, S, r] = varpro_qr (Phi, epss);
          tmp = Q(r+1:m,:)*YY;
          if (norm(tmp) <= norm(v(r+1:m))), 
            break; 
          end
          lambda = lambda/omega;
          istep = istep + 1;
        end
        if (istep == 0),
          lambda = lambda*omega;
        end
        vn = vvn;          
      else
        h = -B\res;
      end

      norma = norm(vn);
      normr = norm(h);

      vn = vn + h;

      step  = step + 1;
      if (step > nofstep),
        err = ERR_NOFSTEP;
        break;
      end

    end % while
  end % if vn == []

  if (n > 0),
    Phi = eval ([fun, '''Phi''', arg]);
    if (psi),
      Psi = eval ([fun, '''Psi''', arg]);
      YY  = Y - Psi;
    end
    vl = Phi\YY;
  end

  if (~(err == ERR_OK) & errpr),
    if     (err == ERR_OK),      
      disp ('no error');
    elseif (err == ERR_NOFSTEP),
      disp ('warning: number of steps exceeded limit'); 
    else
      error ('fatal: illegal error number');
    end
  end

end % varpro