a=0
b=0
c=0
a1=cos(a)*cos(c)-sin(a)*sin(b)*sin(c)
a2=-cos(a)*sin(c)-sin(a)*sin(b)*sin(c)
a3=-sin(a)*cos(b)
b1=cos(b)*sin(c)
b2=cos(b)*cos(c)
b3=-sin(c)
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(c)
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=sum(Za)/4
xx=[-86.15 -68.99;-53.40 82.21;-14.78 -76.63;10.46 64.43]
x0=0
y0=0
f=153.24
for i=1:4
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)+x0
y(i)=-f*(a2*(Xa(i,1)-Xs)+b2*(Ya(i,1)-Ys)+c2*(Za(i,1)-Zs))/z(i)+y0
a11=(a1*f+a3*(x(i)-x0))/z(i)
a12=(b1*f+b3*(x(i)-x0))/z(i)
a13=(c1*f+c3*(x(i)-x0))/z(i)
a21=(a2*f+a3*(y(i)-y0))/z(i)
a22=(b2*f+b3*(y(i)-y0))/z(i)
a23=(c2*f+c3*(y(i)-y0))/z(i)
a14=(y(i)-y0)*sin(b)-((x(i)-x0)*((x(i)-x0)*cos(c)-(y(i)-y0)*sin(c))/f+f*cos(c))*cos(b)
a15=-f*sin(c)-(x(i)-x0)*((x(i)-x0)*sin(c)+(y(i)-y0)*cos(c))/f
a16=y(i)-y0
a24=-(x(i)-x0)*sin(b)-((y(i)-y0)*((x(i)-x0)*cos(c)-(y(i)-y0)*sin(c))/f-f*sin(c))*cos(b)
a25=-f*cos(c)-(y(i)-y0)*((x(i)-x0)*sin(c)+(y(i)-y0)*cos(c))/f
a26=-(x(i)-x0)
a17=(x(i)-x0)/f
a18=1
a19=0
a27=(y(i)-y0)/f
a28=0
a29=1
A(2*i-1:2*i,1:9)=[a11 a12 a13 a14 a15 a16 a17 a18 a19;a21 a22 a23 a24 a25 a26 a27 a28 a29]

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