bestfit.m

基于声源的方向定位程序

function [xopt,actual,calc,err,m] = bestFit(m1,m2,d,arr,limits,resolution,c,xx,yy)

%[xopt,actual,calc,err] = bestFit(m1, m2, delays, arr, limits, resolution, c)
%
% Calculate the best-fit location for the given time delays d.
%
% Inputs:
%   m1,m2	(each is 1xN) a list of pairs of phones
%   delays	(1xN) delays between those phone pairs
%   arr		(2xK) phone positions
%   limits	(1x4) limits of plot [xmin xmax ymin ymax] (optional)
%   resolution	(scalar) # of initial points in x and y (optional; default=10)
%   c		(scalar) speed of sound
%   xx,yy (optional)  (scalars) initial guess of the position

n = length(m1);
if (isempty(limits))
  limits = defaultLimits(arr);
end
minX = limits(1);
maxX = limits(2);
minY = limits(3);
maxY = limits(4);

if (isempty(resolution)), resolution = 10; end		% default value

if (nargin < 8)
  % Calculate point of least square delay error coarsely, using grid.
  disp('')
  x = linspace(minX,maxX,resolution)' * ones(1,resolution);
  y = ones(resolution,1) * linspace(minY,maxY,resolution);
  e = zeros(length(x),length(y));
  
  for i = 1:n
    d1 = sqrt((x - arr(1,m1(i))).^2 + (y - arr(2,m1(i))).^2);
    d2 = sqrt((x - arr(1,m2(i))).^2 + (y - arr(2,m2(i))).^2);
    dd = (d2 - d1) - d(i)*c;
    
    % Change this for minimizing Chebychev or norm other than L2
    e  = e + dd .* dd;
  end
  
  [m,opt] = min(e(:));
  opt     = opt(1);
  xx      = x(opt);
  yy      = y(opt);
end

% Apply Levenberg-Marquardt least squares optimization.
if (exist('optimset'))		% options handling changed in Matlab version 5
  op = optimset('lsqnonlin');
  op = optimset(op, 'Jacobian', 'CalcDeltaJacobian');
  xopt = lsqnonlin('CalcDeltaTimes',[xx;yy], [], [], op, arr, m1, m2, -d * c);
else
  options    = foptions;
  options(1) = -1;		% no warning messages
  xopt = lsq('CalcDeltaTimes',[xx;yy], options, 'CalcDeltaJacobian',...
                                 arr, m1, m2, -d * c);
end


calcDeltaTimes = CalcDeltaTimes(xopt, arr, m1, m2, -d*c);
calcTimes = d' - calcDeltaTimes / c;
m = norm(calcDeltaTimes)^2;

h1     = m1';
h2     = m2';
actual = -d';
calc   = -calcTimes;
err    = calcDeltaTimes / c;

i = find(h2 < h1);
if (length(i) > 0)
   temp       = h1(i);		% swap h1 and h2...
   h1(i)      = h2(i);
   h2(i)      = temp;
   actual(i)  = -actual(i);		% ...and invert delays appropriately
   calc(i)    = -calc(i);
   err(i)     = -err(i);
end