
a=0
b=0
c=0
Xa=[36589.41;37631.08;39100.97;40426.54]
Ya=[25273.32;31324.51;24934.98;30319.81]
Za=[2195.17;728.69;2386.50;757.31]
Xs=sum(Xa)/4
Ys=sum(Ya)/4
Zs=7000
H=[Xs Ys Zs 0 0 0 ]
xx=[-86.15 -68.99;-53.40 82.21;-14.78 -76.63;10.46 64.43]/1000
x0=0
y0=0
f=153.24/1000
for j=1:1000
    Xs=H(1);Ys=H(2);Zs=H(3);a=H(4);b=H(5);c=H(6);
    
for i=1:4
a1=cos(a)*cos(c)-sin(a)*sin(b)*sin(c)
a2=-cos(a)*sin(c)-sin(a)*sin(b)*cos(c)
a3=-sin(a)*cos(b)
b1=cos(b)*sin(c)
b2=cos(b)*cos(c)
b3=-sin(b)
c1=sin(a)*cos(c)+cos(a)*sin(b)*sin(c)
c2=-sin(a)*sin(c)+cos(a)*sin(b)*cos(c)
c3=cos(a)*cos(b)
R=[a1 a2 a3;b1 b2 b3;c1 c2 c3]
z(i)=(a3*(Xa(i,1)-Xs)+b3*(Ya(i,1)-Ys)+c3*(Za(i,1)-Zs))
x(i)=-f*(a1*(Xa(i,1)-Xs)+b1*(Ya(i,1)-Ys)+c1*(Za(i,1)-Zs))/z(i)
y(i)=-f*(a2*(Xa(i,1)-Xs)+b2*(Ya(i,1)-Ys)+c2*(Za(i,1)-Zs))/z(i)
a11=(a1*f+a3*(xx(i,1)-x0))/z(i)
a12=(b1*f+b3*(xx(i,1)-x0))/z(i)
a13=(c1*f+c3*(xx(i,1)-x0))/z(i)
a21=(a2*f+a3*(xx(i,2)-y0))/z(i)
a22=(b2*f+b3*(xx(i,2)-y0))/z(i)
a23=(c2*f+c3*(xx(i,2)-y0))/z(i)
a14=(xx(i,2)-y0)*sin(b)-(   (xx(i,1)-x0)*  ((xx(i,1)-x0)*cos(c)-(xx(i,2)-y0)*sin(c)) /f+  f*cos(c)   )*cos(b)
a15=-f*sin(c)-(xx(i,1)-x0)* ((xx(i,1)-x0)*sin(c)+(xx(i,2)-y0)*cos(c)) /f
a16=xx(i,2)-y0
a24=-(xx(i,1)-x0)*sin(b)-((xx(i,2)-y0)*((xx(i,1)-x0)*cos(c)-(xx(i,2)-y0)*sin(c))/f-f*sin(c))*cos(b)
a25=-f*cos(c)-(xx(i,2)-y0)*((xx(i,1)-x0)*sin(c)+(xx(i,2)-y0)*cos(c))/f
a26=-(xx(i,1)-x0)

A(2*i-1:2*i,1:6)=[a11 a12 a13 a14 a15 a16;a21 a22 a23 a24 a25 a26]

L(2*i-1:2*i)=[xx(i,1)-x(i);xx(i,2)-y(i)]
end

X=inv(A'*A)*A'*L';
V=A*X-L';
H=H+X'

if max(abs(X))<0.1
    H
    R
    break;
end
end