% lsq_res.m 9-oct-10 
% do nonlinear least squares with function collin
% which evaluates the collinearity equations at current
% parameter estimates, use numerical approximation of
% partial derivatives
% add some output to describe convergence

function par = lsq_res(ep,pdel,ip,pname,x,y,X,Y,Z)

[xm,xn]=size(x);
numpar=6;
numpnt=xm;
numeqn=numpnt*2;

B=zeros(numeqn,numpar);
f=zeros(numeqn,1);

for iteration=1:10
  iteration
  for i=1:numpnt
    eqn=(i-1)*2 + 1;
    F0=collin(ep,ip,x(i),y(i),X(i),Y(i),Z(i));
    for j=1:numpar
      ep2=ep;
      ep2(j)=ep(j) + pdel(j);
      F1=collin(ep2,ip,x(i),y(i),X(i),Y(i),Z(i));
      B(eqn,  j)=(F1(1)-F0(1))/pdel(j);
      B(eqn+1,j)=(F1(2)-F0(2))/pdel(j);
      end;
    f(eqn)  =-F0(1);
    f(eqn+1)=-F0(2);
    end;
  del=inv(B'*B)*B'*f;
  del
  ep=ep+del;
  converge=0;
  if (all(abs(del) < pdel))
    converge=1;
    break
    end
  end

if(converge == 1)
  disp('we have converged');
else
  disp('we did not converge');
  end

disp('residuals');
rms=0.0;
qx=zeros(numpnt);
qy=zeros(numpnt);
qu=zeros(numpnt);
qv=zeros(numpnt);
for i=1:numpnt
  disp(pname(i));
  F0=collin(ep,ip,x(i),y(i),X(i),Y(i),Z(i));
  F0
  qx(i)=x(i);
  qy(i)=y(i);
  qu(i)=F0(1);
  qv(i)=F0(2);
  rms=rms + F0(1)^2 + F0(2)^2;  
  end;
par=ep;
rms=sqrt(rms/(numpnt*2));
rms
%quiver(qx,qy,qu,qv);
