Assume you are located at a position \(s\) and you want to know in which direction to look in order to see a target at position \(t\). In other words, you want to calculate the bearing of \(t\) at position \(s\). This problem is omnipresent in wireless point-to-point communication (antenna alignment), and also occurs in target observation (camera alignment) or virtual compasses.
The present article assumes that both source and target position are available in WGS84 coordinates, such as provided by most GPS devices. The approach per se is not limited to WGS84, however, and can easily be adapted to other reference systems.
The orientation of the source mount (e.g. antenna mount, camera tripod orientation) is assumed to be available in yaw, pitch and roll notation, which is a special case of Euler angles and common in flight dynamics.
The approach can be summarized as follows: both source and target position are transformed to Euclidian space (x, y, z). The vector pointing from the source to the target is then rotated into the reference frame of the source mount orientation. Finally, the horizontal and vertical angles are calculated.
Calculating the Bearing
The problem is defined by the following 9 variables:
- Position of the source: \(\slatitude\), \(\slongitude\), \(\saltitude\)
- Orientation of the source mount: \(\syaw\) (heading), \(\spitch\), and \(\sroll\)
- Position of the target: \(\tlatitude\), \(\tlongitude\), \(\taltitude\)
The source and target positions are first transformed to a global Euclidean coordinate system. Ideally, this should be done with the WGS84 reference ellipsoid, but using the following spherical approximation works fine for most practical purposes:
\begin{eqnarray} s & = & \left[ \begin{array}{c} s_x \\ s_y \\ s_z \end{array} \right] = \left( \saltitude + r_e \right) \left[ \begin{array}{c} \cos(\slatitude) \cos(\slongitude) \\ \cos(\slatitude) \sin(\slongitude) \\ \sin(\slatitude) \end{array} \right] \\ t & = & \left[ \begin{array}{c} t_x \\ t_y \\ t_z \end{array} \right] = \left( \taltitude + r_e \right) \left[ \begin{array}{c} \cos(\tlatitude) \cos(\tlongitude) \\ \cos(\tlatitude) \sin(\tlongitude) \\ \sin(\tlatitude) \end{array} \right] \end{eqnarray}
\(r_e \approx 6378100\) denotes the radius of the earth. The vector
\begin{equation} d = t - s \end{equation}pointing from the source to the target position is the solution to the problem expressed in global coordinates.
Transforming this vector into the source mount coordinate system is accomplished using 3D rotation matrices. For that, let us define the following three basic rotation matrices:
\begin{eqnarray} R_x(\theta) & = & \left[ \begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{array} \right] \\ R_y(\theta) & = & \left[ \begin{array}{ccc} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{array} \right] \\ R_z(\theta) & = & \left[ \begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array} \right] \end{eqnarray}Assuming that the earth is a sphere, the rotation to account for the orientation of the earth's surface at the source position \(s\) is
\begin{equation} \rsurface = R_x(\slongitude) R_y(\frac{\pi}{2} - \slatitude) \end{equation}The rotation matrix to project the vector into the coordinate system of the source mount can be written as
\begin{equation} \rmount = R_z(\syaw) R_y(\spitch) R_x(\sroll) \end{equation}Note that if the antenna mount is leveled with respect to the surface and facing north, \(\rmount\) is the identity matrix.
The vector \(d\) is now rotated into the coordinate system of the source mount
\begin{equation} d' = (\rsurface \rmount)^{-1} d \end{equation}and used to calculate the horizontal and vertical angles
\begin{eqnarray} r & = & \sqrt{{d'_x}^2 + {d'_y}^2 + {d'_z}^2} \\ \alpha_h & = & \mbox{atan2}(d'_y, d'_x) \\ \alpha_v & = & \mbox{asin}(d'_z, r) \end{eqnarray}Implementation
Implementing this procedure is straightforward. Depending on the application, one or both rotation matrices can be calculated in advance, saving a few arithmetic operations in case the algorithm is applied multiple times.
An online calculator and an open source C++ implementation are available.