odr.m

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

function [b, delta, err, step] = ...
         odr (HOOK, x, y, b, OPTIONS, W, D, delta, ...
              P0, P1, P2, P3, P4, P5, P6, P7, P8, P9);
%ODR    Orthogonal Distance Regression
%       
%       [b, delta, err, step] = odr (HOOK, x, y, b, ...
%                               OPTIONS, W, D, delta, P0, ...);
%
%       determines eps[i], delta[i] so that
%         - HOOK ('f', x+delta, b) = y+eps
%         - norm(eps,2)^2 + norm(delta,2)^2 = minimal
%
%       You may think of it as a flexible total least
%       squares algorithm, or just an algorithm to fit
%       a curve with minimal geometric distances.
%
%       HOOK is user-supplied function evaluating
%         - ('f') --> f(x,b)
%         - ('df') --> [df/db, df/dx](x,b)

%       Algorithm by Boggs/Byrd/Schnabel
%       "A stable and efficient algorithm for nonlinear
%       orthogonal distance regression"
%       SIAM J. Sci. Stat. Comput. 8(6):1052--1078, nov. 87.

  fun = [HOOK];
  arg = [];
  if ~any(fun<48)
    fun = [fun, '('];
    arg = [arg, ', x+delta, b'];    
    for i = 1:nargin - 8,
      arg = [arg,',P',int2str(i-1)];
    end
    arg_open = arg;
    arg = [arg, ')'];
  end
  if (nargin < 8), delta = []; end
  if (nargin < 7), D = []; end
  if (nargin < 6), W = []; end
  if (nargin < 5), OPTIONS=[]; end
  
  OPTI_EPS = 1;
  OPTI_NOFSTEP = 3;
  OPTI_ERRPR = 4;
  OPTI_ONEW = 9;
  OPTI_ONED = 10;
  OPTI_DIFFCHK = 11;

  ERR_OK = 0;
  ERR_NOFSTEP = 1;

  epss = 1.0e-8;
  epsr = 1.0e-5;
  oned = 0;
  onew = 0;
  err  = ERR_OK;
  nofstep = 100;
  diffchk = 0;
  alpha = 0.01;

  [n, m] = size(x);
  p = size(b, 1);
  if (delta == []), delta = zeros(n,m); end
  if (D == []), oned = 1; end
  if (W == []), onew = 1; end

  for i = 1:size(OPTIONS,1),
    kind = OPTIONS(i,1);
    if     (kind == OPTI_EPS)     epsr = OPTIONS(i,2);
    elseif (kind == OPTI_NOFSTEP) nofstep = OPTIONS(i,2);
    elseif (kind == OPTI_ERRPR)   errpr = OPTIONS(i,2);
    elseif (kind == OPTI_ONEW)    onew = OPTIONS(i,2);
    elseif (kind == OPTI_ONED)    oned = OPTIONS(i,2);
    elseif (kind == OPTI_DIFFCHK) diffchk = OPTIONS(i,2);
    else                          error ('unknown option');
    end
  end

  if (onew), W = onew*ones(n,1); end
  if (oned), D = oned*ones(n,m); end
  if (size(y,2) ~= 1) error ('y must be column vector'); end
  if (size(y,1) ~= n) error ('x and y incompatible'); end
  if (size(b,2) ~= 1) error ('b must be column vector'); end     
  if (size(W,2) ~= 1) error ('W must be column vector'); end
  if (size(W,1) ~= n) error ('W incompatible'); end
  if (size(D,2) ~= m) error ('D incompatible'); end
  if (size(D,1) ~= n) error ('D incompatible'); end

% flag variables (scalar coeff?)
  wscalar = (onew ~= 0);
  dscalar = (oned ~= 0);
  scalar  = (wscalar & dscalar);

% size variables
  omega = zeros(n,1);
  M = zeros(n,1);
  yb = zeros(n,1);
  JB = zeros(n,p);
  t = zeros(n,m);

% loop until change small
% (or nof steps too large)
  step = 0;
  res  = -1;
  normr = 1;
  norma = 1;
  while (normr > epsr*norma),
    step = step + 1;
    if (step > nofstep), 
      err = ERR_NOFSTEP; 
      disp ('warning: number of steps exceeded limit');
      break; 
    end
    f    = eval ([fun, '''f''', arg]);
    df   = eval ([fun, '''df''', arg]);

if ((df == []) | (diffchk ~= 0)),
    save_x = x;
    save_b = b;
    h = 1e-5;
    DF = [];
    for i=1:size(b,1),
      b(i) = b(i) - h;
      f1 = eval ([fun, '''f''', arg]);
      b(i) = b(i) + 2*h;
      f2 = eval ([fun, '''f''', arg]);
      b(i) = b(i) - h;
      DF = [DF, (f2 - f1)/(2*h)];
    end
    hv = h*ones(size(x,1),1);
    for i=1:size(x,2), 
      x(:,i) = x(:,i) - hv;
      f1 = eval ([fun, '''f''', arg]);
      x(:,i) = x(:,i) + 2*hv;
      f2 = eval ([fun, '''f''', arg]);
      x(:,i) = x(:,i) - hv;
      DF = [DF, (f2 - f1)/(2*h)];
    end
    if (df == []),
      df = DF;
    else
      if (norm(DF-df) > epsr*norm(DF)),
        disp ('warning: differentiate may be inexact');
        if (diffchk == 2), disp('DF - df ='); disp(DF-df); end;
      else
        disp ('status: differentiate OK');
      end
    end
    x = save_x;
    b = save_b;
end

if (onew == 1),
    G1 = (f - y);
elseif (onew)
    G1 = onew*(f - y);
else 
    G1 = W.*(f - y);
end
if (onew*oned == 1),
    G2 = delta;
elseif (scalar),
    G2 = (oned*onew)*delta;
else
    G2 = D.*delta;
    for i=1:n,
      G2(i,:) = W(i)*G2(i,:);
    end
end
    V    = df(:,p+1:p+m);
    J    = df(1:n,1:p);

    alpha = alpha/2;
    while (1),
if (oned),
      E    = oned^2 + alpha;
      Ei   = 1/E;
      for i=1:n,
        omega(i) = (V(i,:)*Ei)*V(i,:)';
      end
      M = sqrt(1./(1+omega));
      Tmp = (Ei*oned)*G2;
else
      E    = D.^2 + alpha*ones(n,m);
      Ei   = 1./E;
      for i=1:n,
        omega(i) = (V(i,:).*Ei(i,:))*V(i,:)';
      end
      M = sqrt(1./(1+omega));
      Tmp = Ei.*D.*G2;
end

if (onew == 1),
      for i=1:n,
        JB(i,:) = M(i)*J(i,:);
      end
elseif (onew),
      for i=1:n,
        JB(i,:) = M(i)*onew*J(i,:);
      end
else
      for i=1:n,
        JB(i,:) = M(i)*W(i)*J(i,:);
      end
end
      for i=1:n,
        yb(i) = -M(i)*(G1(i) - V(i,:)*Tmp(i,:)');
      end
      s = [JB; sqrt(alpha)*eye(p)]\[yb; zeros(p,1)];
      tmp = -JB*s + yb;
      tmp = tmp.*M;
if (oned),
      for i=1:n,
        t(i,:) = tmp(i)*V(i,:)*Ei - Tmp(i,:);
      end
else
      for i=1:n,
        t(i,:) = tmp(i)*V(i,:).*Ei(i,:) - Tmp(i,:);
      end
end
      b = b + s;
      delta = delta + t;

      newres = 0;
      newf = eval ([fun, '''f''', arg]);
      epsilon = newf - y;
if (oned),
      for i=1:n,
        newres = newres + ...
          W(i)^2*(epsilon(i)^2 + (oned*norm(delta(i,:),2))^2);
      end
else
      for i=1:n,
        newres = newres + ...
          W(i)^2*(epsilon(i)^2 + norm(delta(i,:).*D(i,:),2)^2);
      end
end
      if ((res < 0) | (newres < res*(1+epsr))),
        norma = norm(delta) + norm(b);
        normr = norm(t) + norm(s);
        res   = newres;
        break;
      end

      b = b - s;
      delta = delta - t;
      alpha = alpha*3;
    end

  end

end % odr