I am trying to calculate the azimuth, elevation and range of a point above the Earth's surface.
A handy wikipedia picture to clarify the azer coordinates
The latter two are simple, but I am having trouble finding the azimuth. Currently I am looking into rotating the coordinate system, so that the OX axis points North and the XY plane is parallel to the plane tangetial to Earth's ellipsoid in the observation point. In the transfromed coordinate system the azimuth should be as simple as $atan2(r_y,r_x)$, where the vector $\mathbf r$ is the vector pointing from the observation point to the object above Earth's surface.
I have written a simple program to try and do the transformation. The first transformation rotates the system around the OZ axis, resulting in OX pointing north, the second rotates the system around the OY axis, resulting in OZ pointing along the normal to the ellipsoid's tangential plane. n is the normal vector.
cos_rot_z = -ntx/np.sqrt(ntx*ntx+nty*nty)
sin_rot_z = np.sqrt(1. - cos_rot_z*cos_rot_z)
rtx = rtx*cos_rot_z - rty*sin_rot_z
rty = rtx*sin_rot_z + rty*cos_rot_z
rtz = rtz
ntx = ntx*cos_rot_z - nty*sin_rot_z
nty = ntx*sin_rot_z + nty*cos_rot_z
ntz = ntz
ntmod = np.sqrt(ntx*ntx+nty*nty+ntz*ntz)
rtmod = np.sqrt(rtx*rtx+rty*rty+rtz*rtz)
print("Transformation 1\nntmod, rtmod",ntmod,rtmod)
cos_rot_y = ntz/nmod
sin_rot_y = np.sqrt(1. - cos_rot_y*cos_rot_y)
rtx = rx*cos_rot_y + rz*sin_rot_y
rty = ry
rtz = -rx*sin_rot_y + rz*cos_rot_y
ntx = nx*cos_rot_y + nz*sin_rot_y
nty = ny
ntz = -nx*sin_rot_y + nz*cos_rot_y
ntmod = np.sqrt(ntx*ntx+nty*nty+ntz*ntz)
rtmod = np.sqrt(rtx*rtx+rty*rty+rtz*rtz)
print("Transformation 2\nntmod, rtmod",ntmod,rtmod)
However, in the output the moduli of the vectors change after the first transfromation:
nmod, rmod [1.56801421e-07] [6096634.62776758]
Transformation 1
ntmod, rtmod [1.46743607e-07] [6096426.73822206]
Transformation 2
ntmod, rtmod [1.56801421e-07] [6096634.62776758]
I am lost as to why this happens. Could it help, if, instead of multiplying the rotation matrices with the vectors I multiplied the matrices with each other and then multiplied the product with the vectors?