Coding a coordinate system rotation

57 Views Asked by At

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?