This vignette contains solutions to various geographical position calculations. It is inspired and follows the 10 examples given at https://www.navlab.net/nvector/ .

Most of the content is based on (Gade 2010).

The color scheme in the Figures is as follows:

  • \(\mathbf{\color{red}{Red}}\): Given
  • \(\mathbf{\color{green}{Green}}\): Find this

Example 1: A and B to delta

Given two positions \(A\) and \(B\), find the exact vector from \(A\) to \(B\) in meters north, east and down, and find the direction (azimuth/bearing) to \(B\), relative to north. Use WGS-84 ellipsoid.

A and B to delta.

A and B to delta.

Solution

Transform the positions \(A\) and \(B\) to (decimal) degrees and depths:

Step 1: Convert to n-vectors, \(\mathbf{n}_{EA}^E\) and \(\mathbf{n}_{EB}^E\)

Step 2: Find \(\mathbf{p}_{AB}^E\) (delta decomposed in E). WGS-84 ellipsoid is default

Step 3: Find \(\mathbf{R}_{EN}\) for position \(A\)

Step 4: Find \(\mathbf{p}_{AB}^N = \mathbf{R}_{NE} \mathbf{p}_{AB}^E\)

Step 5: Also find the direction (azimuth) to \(B\), relative to north

(azimuth <- atan2(p_AB_N[2], p_AB_N[1]) %>%   # positive angle about down-axis
  deg())
#> [1] 45.10926

Example 2: B and delta to C

A radar or sonar attached to a vehicle \(B\) (Body coordinate frame) measures the distance and direction to an object \(C\).

We assume that the distance and two angles (typically bearing and elevation relative to \(B\)) are already combined to the vector \(\mathbf{p}_{BC}^B\) (i.e. the vector from \(B\) to \(C\), decomposed in B).

The position of \(B\) is given as \(\mathbf{n}_{EB}^E\) and \(z_{EB}\), and the orientation (attitude) of \(B\) is given as \(\mathbf{R}_{NB}\) (this rotation matrix can be found from roll/pitch/yaw by using zyx2R).

Find the exact position of object \(C\) as n-vector and depth (\(\mathbf{n}_{EC}^E\) and \(z_{EC}\)), assuming Earth ellipsoid with semi-major axis \(a\) and flattening \(f\).

For WGS-72, use \(a = 6378135~\mathrm{m}\) and \(f = \dfrac{1}{298.26}\).

B and delta to C.

B and delta to C.

Solution

Step 1: Find \(\mathbf{R}_{EN}\)

Step 2: Find \(\mathbf{R}_{EB}\) from \(\mathbf{R}_{EN}\) and \(\mathbf{R}_{NB}\)

Step 3: Decompose the delta vector \(\mathbf{p}_{BC}^B\) in E

Step 4: Find the position of \(C\), using the functions that goes from one position and a delta, to a new position

Convert to latitude and longitude, and height

Example 3: ECEF-vector to geodetic latitude

Position \(B\) is given as an “ECEF-vector” \(\mathbf{p}_{EB}^E\) (i.e. a vector from E, the center of the Earth, to \(B\), decomposed in E).

Find the geodetic latitude, longitude and height (latEB, lonEB and hEB), assuming WGS-84 ellipsoid.

ECEF-vector to geodetic latitude.

ECEF-vector to geodetic latitude.

Position \(B\) is given as \(\mathbf{p}_{EB}^E\), i.e. “ECEF-vector”

Example 4: Geodetic latitude to ECEF-vector

Find the ECEF-vector \(\mathbf{p}_{EB}^E\) for the geodetic position \(B\) given as latitude \(lat_{EB}\), longitude \(lon_{EB}\) and height \(h_{EB}\).

Geodetic latitude to ECEF-vector.

Geodetic latitude to ECEF-vector.

Solution

Step 1: Convert to n-vector

Step 2: Find the ECEF-vector p_EB_E

Example 5: Surface distance

Given two positions \(A\) \(\mathbf{n}_{EA}^E\) and \(B\) \(\mathbf{n}_{EB}^E\), find the surface distance \(s_{AB}\) (i.e. great circle distance). The heights of \(A\) and \(B\) are not relevant (i.e. if they don’t have zero height, we seek the distance between the points that are at the surface of the Earth, directly above/below \(A\) and \(B\)). Also find the Euclidean distance (chord length) \(d_{AB}\) using nonzero heights.

Assume a spherical model of the Earth with radius \(r_{Earth} = 6371~\mathrm{km}\).

Compare the results with exact calculations for the WGS-84 ellipsoid.

Surface distance.

Surface distance.

Solution

Spherical model

The great circle distance is given by equations (16) in (Gade 2010) (the \(\arccos\) is ill conditioned for small angles; the \(\arcsin\) is ill-conditioned for angles near \(\pi/2\), and not valid for angles greater than \(\pi/2\)) where \(r_{roc}\) is the radius of curvature, i.e. Earth radius + height:

\(\begin{align} s_{AB} & = r_{roc} \cdot \arccos \!\big(\mathbf{n}_{EA}^E \boldsymbol{\cdot} \mathbf{n}_{EB}^E\big)\\ & = r_{roc} \cdot \arcsin \!\big(\big|\mathbf{n}_{EA}^E \boldsymbol{\times} \mathbf{n}_{EB}^E\big|\big) \tag{16} \end{align}\)

The formulation via \(\operatorname{atan2}\) of equation (6) in (Gade 2010) is instead well conditioned for all angles:

\(s_{AB} = r_{roc} \cdot \operatorname{atan2}\big(\big|\mathbf{n}_{EA}^E \boldsymbol{\times} \mathbf{n}_{EB}^E\big|, \mathbf{n}_{EA}^E \boldsymbol{\cdot} \mathbf{n}_{EB}^E\big) \tag{6}\)

(s_AB <- (atan2(base::norm(pracma::cross(n_EA_E, n_EB_E), type = "2"),
                pracma::dot(n_EA_E, n_EB_E)) * r_Earth))
#> [1] 332456.4

The Euclidean distance is given by

\(d = r_{roc} \cdot \big| \mathbf{n}_{EB}^E - \mathbf{n}_{EA}^E \big|\)

(d_AB <- base::norm(n_EB_E - n_EA_E, type = "2") * r_Earth)
#> [1] 332418.7

Elliptical model (WGS-84 ellipsoid)

The distance between \(A\) and \(B\) ca be calculated via geosphere package

Example 6: Interpolated position

Given the position of \(B\) at time \(t_0\) and \(t_1\), \(\mathbf{n}_{EB}^E(t_0)\) and \(\mathbf{n}_{EB}^E(t_1)\).

Find an interpolated position at time \(t_i\), \(\mathbf{n}_{EB}^E(t_i)\). All positions are given as n-vectors.

Interpolated position.

Interpolated position.

Solution

Standard interpolation can be used directly with n-vector as

\[ \mathbf{n}_{EB}^E(t_i) = \operatorname{unit}\Bigg(\mathbf{n}_{EB}^E(t_0) + \frac{t_i − t_0}{t_1 − t_0} \Big(\mathbf{n}_{EB}^E(t_1) − \mathbf{n}_{EB}^E(t_0)\Big)\Bigg) \]

Using the expression above

and converting back to longitude and latitude

Example 7: Mean position (center/midpoint)

Given three positions \(A\), \(B\), and \(C\) as n-vectors \(\mathbf{n}_{EA}^E\), \(\mathbf{n}_{EB}^E\), and \(\mathbf{n}_{EC}^E\), find the mean position, \(M\), as n-vector \(\mathbf{n}_{EM}^E\).

Note that the calculation is independent of the depths of the positions.

Mean position (center/midpoint).

Mean position (center/midpoint).

Solution

The (geographical) mean position \(B_{GM}\) is simply given equation (17) in (Gade 2010) (assuming spherical Earth)

\[ \mathbf{n}_{EB_{GM}}^E = \operatorname{unit}\Big( \sum_{i = 1}^{m} \mathbf{n}_{EB_i}^E \Big) \tag{17} \]

and specifically for the three given points

\[ \mathbf{n}_{EM}^E = \mathrm{unit}\Big(\mathbf{n}_{EA}^E + \mathbf{n}_{EB}^E + \mathbf{n}_{EC}^E \Big) = \frac{\mathbf{n}_{EA}^E + \mathbf{n}_{EB}^E + \mathbf{n}_{EC}^E}{\Big | \mathbf{n}_{EA}^E + \mathbf{n}_{EB}^E + \mathbf{n}_{EC}^E \Big| } \] Given the three n-vectors

find the horizontal mean position

and convert to longitude/latitude

Example 8: A and azimuth/distance to B

Given a position \(A\) as n-vector \(\mathbf{n}_{EA}^E\), an initial direction of travel as an azimuth (bearing), \(\alpha\), relative to north (clockwise), and finally the distance to travel along a great circle, \(s_{AB}\) find the destination point \(B\), given as \(\mathbf{n}_{EB}^E\).

Use Earth radius \(r_{Earth}\).

In geodesy this is known as “The first geodetic problem” or “The direct geodetic problem” for a sphere, and we see that this is similar to Example 2, but now the delta is given as an azimuth and a great circle distance. (“The second/inverse geodetic problem” for a sphere is already solved in Examples 1 and 5.)

A and azimuth/distance to B.

A and azimuth/distance to B.

Solution

Given the initial values

Step 1: Find unit vectors for north and east as per equations (9) and (10) in (Gade 2010)

$$ \[\begin{align} \mathbf{k}_{east}^E & = \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix} \times \mathbf{n}^E \tag{9} \\ \mathbf{k}_{north}^E & = \mathbf{n}^E \times \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix} \times \mathbf{n}^E \tag{10} \end{align}\] $$

k_east_E <- unit(pracma::cross(base::t(R_Ee()) %*% c(1, 0, 0) %>% as.vector(), n_EA_E))
k_north_E <- pracma::cross(n_EA_E, k_east_E)

Step 2: Find the initial direction vector \(d_E\)

d_E <- k_north_E * cos(azimuth) + k_east_E * sin(azimuth)

Step 3: Find \(\mathbf{n}_{EB}^E\)

n_EB_E <- n_EA_E * cos(s_AB / r_Earth) + d_E * sin(s_AB / r_Earth)

Convert to longitude/latitude

Example 9: Intersection of two paths

Define a path from two given positions (at the surface of a spherical Earth), as the great circle that goes through the two points.

Path A is given by \(A_1\) and \(A_2\), while path B is given by \(B_1\) and \(B_2\).

Find the position C where the two great circles intersect.

Intersection of two paths.

Intersection of two paths.

Solution

Find the intersection between the two paths, \(\mathbf{n}_{EC}^E\)

n_EC_E_tmp <- unit(pracma::cross(
  pracma::cross(n_EA1_E, n_EA2_E),
  pracma::cross(n_EB1_E, n_EB2_E)))

\(\mathbf{n}_{{EC}_{tmp}}^E\) is one of two solutions, the other is \(-\mathbf{n}_{{EC}_{tmp}}^E\). Select the one that is closest to \(\mathbf{n}_{EA_1}^E\), by selecting sign from the dot product between \(\mathbf{n}_{{EC}_{tmp}}^E\) and \(\mathbf{n}_{EA_1}^E\)

n_EC_E <- sign(pracma::dot(n_EC_E_tmp, n_EA1_E)) * n_EC_E_tmp

Convert to longitude/latitude

Example 10: Cross track distance (cross track error)

Path A is given by the two positions \(A_1\) and \(A_2\) (similar to the previous example).

Find the cross track distance \(s_{xt}\) between the path A (i.e. the great circle through \(A_1\) and \(A_2\)) and the position \(B\) (i.e. the shortest distance at the surface, between the great circle and \(B\)).

Also find the Euclidean distance \(d_{xt}\) between \(B\) and the plane defined by the great circle.

Use Earth radius \(6371~\mathrm{km}\).

Cross track distance (cross track error).

Cross track distance (cross track error).

Solution

Given

Find the unit normal to the great circle between n_EA1_E and n_EA2_E as shown in the Figure @ref(fig:solution-10-fig).

c_E <- unit(pracma::cross(n_EA1_E, n_EA2_E))
Vectors for cross track distance  calculation.

Vectors for cross track distance calculation.

Find the great circle cross track distance

(s_xt <- (acos(pracma::dot(c_E, n_EB_E)) - pi / 2) * r_Earth)
#> [1] 11117.8

Find the Euclidean cross track distance

(d_xt <- -pracma::dot(c_E, n_EB_E) * r_Earth)
#> [1] 11117.79

Example 11: Cross track intersection

Path A is given by the two positions \(A_1\) and \(A_2\) (similar to the previous example).

Find the cross track intersection point \(C\) between the path A (i.e. the great circle through \(A_1\) and \(A_2\)) and the position \(B\), i.e. the shortest distance point at the surface, between the great circle and \(B\).

Cross track intersection.

Cross track intersection.

Solution

Given (note that \(B\) doesn’t necessarily need to lie in between \(A_1\) and \(A_2\) as per Figure above)

Find the normal to the great circle between n_EA1_E and n_EA2_E:

n_EN_E <- unit(pracma::cross(n_EA1_E, n_EA2_E))

Find the intersection points (one antipodal to the other):

Choose the one closest to B:

n_EC_E <- sign(pracma::dot(n_EC_E_tmp, n_EB_E)) * n_EC_E_tmp

Convert to longitude/latitude

References

Gade, Kenneth. 2010. “A Nonsingular Horizontal Position Representation.” The Journal of Navigation 63 (3): 395–417. www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf.