function matchtopts(initparams, epts)
% function matchtopts(initparams, pts)
% initparams = 2+2*N parameters for an N linkage arm.
% pts = Mx2 matrix of points to match.

params = initparams;

while 1,

  plot(epts(:,1),epts(:,2),'.');
  hold on
  linkpts = [];
  linkpts(1,:) = params(1:2);
  for i=3:2:length(params),
    linkpts((i+1)/2,:) = linkpts((i-1)/2,:)+params(i+1)*[cos(params(i)) ...
          sin(params(i))];
  end
  plot(linkpts(:,1), linkpts(:,2), 'r');
  hold off
  drawnow
  e = [];
  J = [];
  for p = 1:size(epts,1),
    [d dp]  = linkage(params,epts(p,:));
    e(p) = d;
    J(p,:) = dp;
  end
  
  
  
  delta = pinv(J) * (e')

  % because of the lack of stabilization/regularization, and since
  % we're not using the Levenberg-Marquardt method, and probably also
  % because we're matching all the points, this algorithm tends to be
  % unstable even for systems where the initial guess is reasonable.
  %
  % For this reason, take only a partial Newton step.
  params = params - 0.4*(delta')
end