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.

Solution

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

# Position A:
lat_EA <- rad(1)
lon_EA <- rad(2)
z_EA <- 3

# Position B:
lat_EB <- rad(4)
lon_EB <- rad(5)
z_EB <- 6

Step 1: Convert to n-vectors, n_EA^E and n_EB^E

(n_EA_E <- lat_lon2n_E(lat_EA, lon_EA))
#> [1] 0.99923861 0.03489418 0.01745241
(n_EB_E <- lat_lon2n_E(lat_EB, lon_EB))
#> [1] 0.99376802 0.08694344 0.06975647

Step 2: Find p_AB^E (delta decomposed in E). WGS-84 ellipsoid is default

(p_AB_E <-  n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB))
#> [1] -34798.44 331985.66 331375.96

Step 3: Find R_EN for position A

(R_EN <- n_E2R_EN(n_EA_E))
#>               [,1]       [,2]        [,3]
#> [1,] -0.0174417749 -0.0348995 -0.99923861
#> [2,] -0.0006090802  0.9993908 -0.03489418
#> [3,]  0.9998476952  0.0000000 -0.01745241

Step 4: Find p_AB^N = R_NEp_AB^E

# (Note the transpose of R_EN: The "closest-rule" says that when
# decomposing, the frame in the subscript of the rotation matrix that is
# closest to the vector, should equal the frame where the vector is
# decomposed. Thus the calculation R_NE*p_AB_E is correct, since the vector
# is decomposed in E, and E is closest to the vector. In the above example
# we only had R_EN, and thus we must transpose it: base::t(R_EN) = R_NE)
(p_AB_N <- base::t(R_EN) %*% p_AB_E %>%  
  as.vector())
#> [1] 331730.23 332997.87  17404.27

The vector p_AB^N connects A to B in the North-East-Down framework. The line-of-sight distance, in meters, from A to B is

(los_distance <- norm(p_AB_N, type = "2"))
#> [1] 470356.7

while the altitude (elevation above the horizon), in decimal degrees, is

(elevation <- atan2(-p_AB_N[3], p_AB_N[2]) %>% deg())
#> [1] -2.991865

Step 5: Also find the direction to B (azimuth), in decimal degrees, relative to true 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 p_BC^B (i.e. the vector from B to C, decomposed in B).

The position of B is given as n_EB^E and z_EB, and the orientation (attitude) of B is given as 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 (n_EC^E and z_EC), assuming Earth ellipsoid with semi-major axis a and flattening f.

For WGS-72, use a = 6378135 m and $f = \dfrac{1}{298.26}$.

B and delta to C.

Solution

p_BC_B <- c(3000, 2000, 100)

# Position and orientation of B is given:
(n_EB_E <- unit(c(1, 2, 3))) # unit() to get unit length of vector
#> [1] 0.2672612 0.5345225 0.8017837
z_EB <- -400
(R_NB <- zyx2R(rad(10),rad(20),rad(30))) # the three angles are yaw, pitch, and roll
#>            [,1]       [,2]       [,3]
#> [1,]  0.9254166 0.01802831  0.3785223
#> [2,]  0.1631759 0.88256412 -0.4409696
#> [3,] -0.3420201 0.46984631  0.8137977

# A custom reference ellipsoid is given (replacing WGS-84):
# (WGS-72)
a <- 6378135
f <- 1 / 298.26

Step 1: Find R_EN

(R_EN <- n_E2R_EN(n_EB_E))
#>            [,1]       [,2]       [,3]
#> [1,] -0.3585686 -0.8944272 -0.2672612
#> [2,] -0.7171372  0.4472136 -0.5345225
#> [3,]  0.5976143  0.0000000 -0.8017837

Step 2: Find R_EB from R_EN and R_NB

(R_EB <- R_EN %*% R_NB) # Note: closest frames cancel
#>            [,1]       [,2]        [,3]
#> [1,] -0.3863656 -0.9214254  0.04119242
#> [2,] -0.4078587  0.1306225 -0.90365318
#> [3,]  0.8272684 -0.3659411 -0.42627939

Step 3: Decompose the delta vector p_BC^B in E

(p_BC_E <- R_EB %*% p_BC_B) # no transpose of R_EB, since the vector is in B)
#>           [,1]
#> [1,] -2997.828
#> [2,] -1052.696
#> [3,]  1707.295

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

l <- n_EA_E_and_p_AB_E2n_EB_E(n_EB_E, p_BC_E, z_EB, a, f)
(n_EB_E <- l[['n_EB_E']])
#> [1] 0.2667916 0.5343565 0.8020507
(z_EB <- l[['z_EB']])
#> [1] -406.0072

Convert to latitude and longitude, and height

lat_lon_EB <- n_E2lat_lon(n_EB_E)
(latitude  <- lat_lon_EB[1])
#> [1] 0.9307209
(longitude <- lat_lon_EB[2])
#> [1] 1.107728

# height (= - depth)
(height <- -z_EB)
#> [1] 406.0072

Example 3: ECEF-vector to geodetic latitude

Position B is given as an “ECEF-vector” 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.

Position B is given as p_EB^E, i.e. “ECEF-vector”

(p_EB_E <- 6371e3 * c(0.9, -1, 1.1)) # m
#> [1]  5733900 -6371000  7008100

Solution

Find n-vector from the p-vector

l <- p_EB_E2n_EB_E(p_EB_E)
(n_EB_E <- l[['n_EB_E']])
#> [1]  0.5170890 -0.5745433  0.6344439
(z_EB <- l[['z_EB']])
#> [1] -4702060

Convert to latitude and longitude, and height

lat_lon_EB <- n_E2lat_lon(n_EB_E)
(latEB  <- lat_lon_EB[1])
#> [1] 0.6872888
(lonEB <- lat_lon_EB[2])
#> [1] -0.8379812

# height (= - depth)
(hEB <- -z_EB)
#> [1] 4702060

Example 4: Geodetic latitude to ECEF-vector

Find the ECEF-vector 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.

Solution

lat_EB <- rad(1)
lon_EB <- rad(2)
h_EB <- 3

Step 1: Convert to n-vector

(n_EB_E <- lat_lon2n_E(lat_EB, lon_EB))
#> [1] 0.99923861 0.03489418 0.01745241

Step 2: Find the ECEF-vector p_EB_E

(p_EB_E <- n_EB_E2p_EB_E(n_EB_E, -h_EB))
#> [1] 6373290.3  222560.2  110568.8

Example 5: Surface distance

Given two positions A n_EA^E and B 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 km.

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

Surface distance.

Solution

n_EA_E <- lat_lon2n_E(rad(88), rad(0));
n_EB_E <- lat_lon2n_E(rad(89), rad(-170))
r_Earth <- 6371e3

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 π/2, and not valid for angles greater than π/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 atan2 of equation (6) in (Gade 2010) is instead well conditioned for all angles:

s_AB = r_roc ⋅ atan2 (|n_EA^E×n_EB^E|, n_EA^E⋅n_EB^E)

(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 ⋅ |n_EB^E − n_EA^E|

(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

geosphere::distGeo(c(0, 88), c(-170, 89))
#> [1] 333947.5

Example 6: Interpolated position

Given the position of B at time t₀ and t₁, n_EB^E(t₀) and n_EB^E(t₁).

Find an interpolated position at time t_i, n_EB^E(t_i). All positions are given as n-vectors.

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) $$

n_EB_E_t0 <- lat_lon2n_E(rad(89.9), rad(-150))
n_EB_E_t1 <- lat_lon2n_E(rad(89.9), rad(150))

# The times are given as:
t0 <- 10
t1 <- 20
ti <- 16 # time of interpolation

Using the expression above

t_frac <- (ti - t0) / (t1 - t0) 
(n_EB_E_ti <- unit(n_EB_E_t0 + t_frac * (n_EB_E_t1 - n_EB_E_t0) ))
#> [1] -0.0015114993  0.0001745329  0.9999988425

and converting back to longitude and latitude

(l  <- n_E2lat_lon(n_EB_E_ti) %>% deg())
#> [1]  89.91282 173.41322
(latitude  <- l[1])
#> [1] 89.91282
(longitude <- l[2])
#> [1] 173.4132

Example 7: Mean position (center/midpoint)

Given three positions A, B, and C as n-vectors n_EA^E, n_EB^E, and n_EC^E, find the mean position, M, as n-vector n_EM^E.

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

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

n_EA_E <- lat_lon2n_E(rad(90), rad(0))
n_EB_E <- lat_lon2n_E(rad(60), rad(10))
n_EC_E <- lat_lon2n_E(rad(50), rad(-20))

find the horizontal mean position

(n_EM_E <- unit(n_EA_E + n_EB_E + n_EC_E))
#> [1]  0.38411717 -0.04660241  0.92210749

and convert to longitude/latitude

(l  <- n_E2lat_lon(n_EM_E) %>% deg())
#> [1] 67.236153 -6.917511
(latitude  <- l[1])
#> [1] 67.23615
(longitude <- l[2])
#> [1] -6.917511

Example 8: A and azimuth/distance to B

Given a position A as n-vector n_EA^E, an initial direction of travel as an azimuth (bearing), α, relative to north (clockwise), and finally the distance to travel along a great circle, s_AB find the destination point B, given as 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.

Solution

Given the initial values

n_EA_E <- lat_lon2n_E(rad(80),rad(-90))
azimuth <- rad(200)
s_AB <- 1000 # distance (m)
r_Earth <- 6371e3 # mean Earth radius (m)

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

$$ $$

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 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

(l  <- n_E2lat_lon(n_EB_E) %>% deg())
#> [1]  79.99155 -90.01770
(latitude  <- l[1])
#> [1] 79.99155
(longitude <- l[2])
#> [1] -90.0177

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₁ and A₂, while path B is given by B₁ and B₂.

Find the position C where the two great circles intersect.

Intersection of two paths.

Solution

n_EA1_E <- lat_lon2n_E(rad(50), rad(180))
n_EA2_E <- lat_lon2n_E(rad(90), rad(180))
n_EB1_E <- lat_lon2n_E(rad(60), rad(160))
n_EB2_E <- lat_lon2n_E(rad(80), rad(-140))

# These are from the python version (results are the same ;-)
# n_EA1_E <- lat_lon2n_E(rad(10), rad(20))
# n_EA2_E <- lat_lon2n_E(rad(30), rad(40))
# n_EB1_E <- lat_lon2n_E(rad(50), rad(60))
# n_EB2_E <- lat_lon2n_E(rad(70), rad(80))

Find the intersection between the two paths, 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)))

n_{EC_tmp}^E is one of two solutions, the other is −n_{EC_tmp}^E. Select the one that is closest to n_EA₁^E, by selecting sign from the dot product between n_{EC_tmp}^E and n_EA₁^E

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

Convert to longitude/latitude

(l  <- n_E2lat_lon(n_EC_E) %>% deg())
#> [1]  74.16345 180.00000
(latitude  <- l[1])
#> [1] 74.16345
(longitude <- l[2])
#> [1] 180

Example 10: Cross track distance (cross track error)

Path A is given by the two positions A₁ and A₂ (similar to the previous example).

Find the cross track distance s_xt between the path A (i.e. the great circle through A₁ and A₂) 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 km.

Cross track distance (cross track error).

Solution

Given

n_EA1_E <- lat_lon2n_E(rad(0), rad(0))
n_EA2_E <- lat_lon2n_E(rad(10),rad(0))
n_EB_E  <- lat_lon2n_E(rad(1), rad(0.1))

r_Earth <- 6371e3  # mean Earth radius (m)

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.

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₁ and A₂ (similar to the previous example).

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

Cross track intersection.

Solution

Given (note that B doesn’t necessarily need to lie in between A₁ and A₂ as per Figure above)

n_EA1_E <- lat_lon2n_E(rad(0), rad(3))
n_EA2_E <- lat_lon2n_E(rad(0),rad(10))
n_EB_E  <- lat_lon2n_E(rad(-1), rad(-1))

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):

n_EC_E_tmp <- unit(
  pracma::cross(
    n_EN_E,
    pracma::cross(n_EN_E, n_EB_E)
  )
)

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

(l  <- n_E2lat_lon(n_EC_E) %>% deg())
#> [1]  0 -1
(latitude  <- l[1])
#> [1] 0
(longitude <- l[2])
#> [1] -1

References

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

Simple and Exact Solutions to Position Calculation

Example 1: A and B to delta

Solution

Example 2: B and delta to C

Solution

Example 3: ECEF-vector to geodetic latitude

Solution

Example 4: Geodetic latitude to ECEF-vector

Solution

Example 5: Surface distance

Solution

Spherical model

Elliptical model (WGS-84 ellipsoid)

Example 6: Interpolated position

Solution

Example 7: Mean position (center/midpoint)

Solution

Example 8: A and azimuth/distance to B

Solution

Example 9: Intersection of two paths

Solution

Example 10: Cross track distance (cross track error)

Solution

Example 11: Cross track intersection

Solution

References