---
title: "Simple and Exact Solutions to Position Calculation"
author: "Enrico Spinielli"
date: "`r Sys.Date()`"
output:
  bookdown::html_document2:
    toc: true
    toc_depth: 2
    number_sections: false
    fig_caption: true
pkgdown:
  as_is: true
references:
- id: gade2010
  title: A Nonsingular Horizontal Position Representation
  author:
  - family: Gade
    given: Kenneth
  container-title: The Journal of Navigation
  volume: 63
  URL: 'https://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf'
  issue: 03
  page: 395-417
  type: article-journal
  issued:
    year: 2010
    month: 7
link-citations: yes
vignette: >
  %\VignetteIndexEntry{Simple and Exact Solutions to Position Calculation}
  %\VignetteKeyword{Geodesic}
  %\VignetteKeyword{Position calculation}
  %\VignetteEngine{knitr::rmarkdown}
  %\VignetteEncoding{UTF-8}
---

```{r setup, include = FALSE}
knitr::opts_chunk$set(
  collapse = TRUE,
  comment = "#>",
  out.width = "220px",
  fig.align = 'center'
)

library(png)
library(nvctr)
```

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 [@gade2010].

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 {#example-01}

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.


```{r example-01-fig,fig.cap='A and B to delta.',fig.pos='hbt',fig.width=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex1img.png")
```


### Solution

Transform the positions $A$ and $B$ to (decimal) degrees and depths:
```{r example-01-init}
# 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, $\mathbf{n}_{EA}^E$ and $\mathbf{n}_{EB}^E$
```{r example-01-step01}
(n_EA_E <- lat_lon2n_E(lat_EA, lon_EA))
(n_EB_E <- lat_lon2n_E(lat_EB, lon_EB))
```

**Step 2**: Find $\mathbf{p}_{AB}^E$ (delta decomposed in E). WGS-84 ellipsoid is default
```{r example-01-step02}
(p_AB_E <-  n_EA_E_and_n_EB_E2p_AB_E(n_EA_E, n_EB_E, z_EA, z_EB))
```

**Step 3**: Find $\mathbf{R}_{EN}$ for position $A$
```{r example-01-step03}
(R_EN <- n_E2R_EN(n_EA_E))
```


**Step 4**: Find $\mathbf{p}_{AB}^N = \mathbf{R}_{NE} \mathbf{p}_{AB}^E$
```{r example-01-step04}
# (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())
```

The vector $\mathbf{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

```{r line-of-sight_distance}
(los_distance <- norm(p_AB_N, type = "2"))
```

while the [altitude](https://en.wikipedia.org/wiki/Horizontal_coordinate_system)
(elevation above the horizon), in decimal degrees, is
```{r elevation}
(elevation <- atan2(-p_AB_N[3], p_AB_N[2]) %>% deg())
```


**Step 5**: Also find the direction to $B$ ([azimuth](https://en.wikipedia.org/wiki/Azimuth)),
in decimal degrees, relative to true North
```{r example-01-step05}
(azimuth <- atan2(p_AB_N[2], p_AB_N[1]) %>%   # positive angle about down-axis
  deg())
```




## Example 2: B and delta to C {#example-02}

A radar or sonar attached to a vehicle $B$ (**B**ody 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}$.

```{r example-02-fig,fig.cap='B and delta to C.',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex2img.png")
```


### Solution


```{r example-02-init}
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
z_EB <- -400
(R_NB <- zyx2R(rad(10),rad(20),rad(30))) # the three angles are yaw, pitch, and roll

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


**Step 1**: Find $\mathbf{R}_{EN}$
```{r example-02-step01}
(R_EN <- n_E2R_EN(n_EB_E))
```



**Step 2**: Find $\mathbf{R}_{EB}$ from $\mathbf{R}_{EN}$ and $\mathbf{R}_{NB}$
```{r example-02-step02}
(R_EB <- R_EN %*% R_NB) # Note: closest frames cancel
```


**Step 3**: Decompose the delta vector $\mathbf{p}_{BC}^B$ in E
```{r example-02-step03}
(p_BC_E <- R_EB %*% p_BC_B) # no transpose of R_EB, since the vector is in B)
```


**Step 4**: Find the position of $C$, using the functions that goes from one
position and a delta, to a new position
```{r example-02-step04}
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']])
(z_EB <- l[['z_EB']])
```

Convert to latitude and longitude, and height
```{r example-02-final}
lat_lon_EB <- n_E2lat_lon(n_EB_E)
(latitude  <- lat_lon_EB[1])
(longitude <- lat_lon_EB[2])

# height (= - depth)
(height <- -z_EB)
```






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

```{r example-03-fig,fig.cap='ECEF-vector to geodetic latitude.',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex3img.png")
```

Position $B$ is given as $\mathbf{p}_{EB}^E$, i.e. "ECEF-vector"

```{r example-03-init}
(p_EB_E <- 6371e3 * c(0.9, -1, 1.1)) # m
```

### Solution

Find n-vector from the p-vector
```{r example-03-step01}
l <- p_EB_E2n_EB_E(p_EB_E)
(n_EB_E <- l[['n_EB_E']])
(z_EB <- l[['z_EB']])
```


Convert to latitude and longitude, and height
```{r example-03-final}
lat_lon_EB <- n_E2lat_lon(n_EB_E)
(latEB  <- lat_lon_EB[1])
(lonEB <- lat_lon_EB[2])

# height (= - depth)
(hEB <- -z_EB)
```





## 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}$.

```{r example-04-fig,fig.cap='Geodetic latitude to ECEF-vector.',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex4img.png")
```

### Solution

```{r example-04-init}
lat_EB <- rad(1)
lon_EB <- rad(2)
h_EB <- 3
```


**Step 1**: Convert to n-vector
```{r example-04-step01}
(n_EB_E <- lat_lon2n_E(lat_EB, lon_EB))
```


**Step 2**: Find the ECEF-vector p_EB_E

```{r example-04-step02}
(p_EB_E <- n_EB_E2p_EB_E(n_EB_E, -h_EB))
```



## Example 5: Surface distance {#example-05}

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.

```{r example-05-fig,fig.cap='Surface distance.',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex5img.png")
```

### Solution

```{r example-05-init}
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 [@gade2010]
(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 [@gade2010] 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}$

```{r example-05-spherical}
(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))
```

The Euclidean distance is given by

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

```{r example-05-step02}
(d_AB <- base::norm(n_EB_E - n_EA_E, type = "2") * r_Earth)
```


#### Elliptical model (WGS-84 ellipsoid)

The distance between $A$ and $B$ ca be calculated via `geosphere` package
```{r example-05-geodesic}
geosphere::distGeo(c(0, 88), c(-170, 89))
```




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


```{r example-06-fig,fig.cap='Interpolated position.',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex6img.png")
```

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

```{r example-06-init}
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

```{r exemple-06-interpolation}
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) ))
```

and converting back to longitude and latitude

```{r example-06-conversion}
(l  <- n_E2lat_lon(n_EB_E_ti) %>% deg())
(latitude  <- l[1])
(longitude <- l[2])
```


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

```{r example-07-fig,fig.cap='Mean position (center/midpoint).',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex7img.png")
```

### Solution

The (geographical) mean position $B_{GM}$ is simply given equation (17) in [@gade2010]
(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
```{r example-07-init}
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

```{r example-07-geographical-mean}
(n_EM_E <- unit(n_EA_E + n_EB_E + n_EC_E))
```

and convert to longitude/latitude
```{r example-07-conversion}
(l  <- n_E2lat_lon(n_EM_E) %>% deg())
(latitude  <- l[1])
(longitude <- l[2])
```


## 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](#example-02),
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](#example-01) and [5](#example-05).) 

```{r example-08-fig,fig.cap='A and azimuth/distance to B.',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex8img.png")
```

### Solution

Given the initial values
```{r example-08-init}
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 [@gade2010]

$$
\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}
$$

```{r}
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$
```{r}
d_E <- k_north_E * cos(azimuth) + k_east_E * sin(azimuth)
```

**Step 3**: Find $\mathbf{n}_{EB}^E$
```{r}
n_EB_E <- n_EA_E * cos(s_AB / r_Earth) + d_E * sin(s_AB / r_Earth)
```

Convert to longitude/latitude
```{r example-08-conversion}
(l  <- n_E2lat_lon(n_EB_E) %>% deg())
(latitude  <- l[1])
(longitude <- l[2])
```


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

```{r example-09-fig,fig.cap='Intersection of two paths.',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex9img.png")
```

### Solution

```{r example-09-init}
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, $\mathbf{n}_{EC}^E$
```{r}
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$

```{r}
n_EC_E <- sign(pracma::dot(n_EC_E_tmp, n_EA1_E)) * n_EC_E_tmp
```

Convert to longitude/latitude
```{r example-09-conversion}
(l  <- n_E2lat_lon(n_EC_E) %>% deg())
(latitude  <- l[1])
(longitude <- l[2])
```

## 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}$.


```{r example-10-fig,fig.cap='Cross track distance (cross track error).',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex10img.png")
```

### Solution
Given 
```{r}
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).

```{r}
c_E <- unit(pracma::cross(n_EA1_E, n_EA2_E))
```
```{r solution-10-fig, fig.cap='Vectors for cross track distance  calculation.', fig.pos='hbt', fig.height=4, echo=FALSE, warning=FALSE, message=FALSE}
knitr::include_graphics("solution10img.png")
```


Find the great circle cross track distance
```{r}
(s_xt <- (acos(pracma::dot(c_E, n_EB_E)) - pi / 2) * r_Earth)
```

Find the Euclidean cross track distance
```{r}
(d_xt <- -pracma::dot(c_E, n_EB_E) * r_Earth)
```


## 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$.


```{r example-11-fig,fig.cap='Cross track intersection.',fig.pos='hbt',fig.height=4,echo=FALSE,warning=FALSE,message=FALSE}
knitr::include_graphics("ex11img.png")
```

### Solution

Given (note that $B$ doesn't necessarily need to lie in between $A_1$ and $A_2$ as per Figure above)
```{r}
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:

```{r}
n_EN_E <- unit(pracma::cross(n_EA1_E, n_EA2_E))
```


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

```{r}
n_EC_E_tmp <- unit(
  pracma::cross(
    n_EN_E,
    pracma::cross(n_EN_E, n_EB_E)
  )
)
```

Choose the one closest to B:

```{r}
n_EC_E <- sign(pracma::dot(n_EC_E_tmp, n_EB_E)) * n_EC_E_tmp
```

Convert to longitude/latitude
```{r example-11-conversion}
(l  <- n_E2lat_lon(n_EC_E) %>% deg())
(latitude  <- l[1])
(longitude <- l[2])
```

# References