clear
% Koordinaten

K=[
 48177.62 6531.28 % Punkt 1
 49600.15 7185.19 % Punkt 2
 49830.93 5670.69 % Punkt 3
 47863.91 5077.24 % Punkt 4
 48565.2  6059.0  % Punkt N
];

% Beobachtungen

L=[
356.2465
 47.3114
118.9497
239.4920
];

X0=[
  K(5,1) % Rechts
  K(5,2) % Hoch
  0.0    % Orientierungsunbekannte
];

sHz = 0.001; %  1mgon

for i=1:size(L)
  dx = K(i,1)-X0(1,1);
  dy = K(i,2)-X0(2,1);
  s = sqrt(dx^2+dy^2);

  L0(i,1) = atan2(dx,dy)*200.0/pi - X0(3,1);
  if L0(i,1)<0 
    L0(i,1) = L0(i,1) + 400.0 ;
  end

  A(i,1) =  dy/s^2 * 200/pi;
  A(i,2) = -dx/s^2 * 200/pi;
  A(i,3) = -1;
  P(i,i) = 1.0/sHz^2;
end

l = L-L0;
N=A'*P*A;
n=A'*P*l;
Qxx=inv(N);
x=Qxx*n
X=X0+x
v=A*x-l
s0=sqrt(v'*P*v/(4-3));
sx=s0*sqrt(Qxx(1,1));
sy=s0*sqrt(Qxx(2,2));	
fid = fopen('goesch.log','w');
fprintf(fid,'Neupunkt-Koordinaten\n\n');
fprintf(fid,'Rechts= %12.6f  +/- %9.6f mm\n',X(1,1),sx*1000.0);
fprintf(fid,'Hoch  = %12.6f  +/- %9.6f mm\n',X(2,1),sy*1000.0);
fprintf(fid,'\n');
fprintf(fid,'Verbesserungen\n\n');
for i=1:size(v)
  fprintf(fid,'%12.6f mm\n',v(i,1)*1000.0);  
end
fprintf(fid,'\n');
fprintf(fid,'Verbesserungen\n\n');
fprintf(fid,'s0 = %12.6f\n',s0);
fclose(fid);