Skip to content
Fetching contributors… Cannot retrieve contributors at this time
669 lines (477 sloc) 18.1 KB
 --- title: "Simple and Exact Solutions to Position Calculation" author: "Enrico Spinielli" date: "r Sys.Date()" output: bookdown::html_document2: base_format: rmarkdown::html_vignette toc: true toc_depth: 2 number_sections: false fig_caption: true references: - id: gade2010 title: A Nonsingular Horizontal Position Representation author: - family: Gade given: Kenneth container-title: The Journal of Navigation volume: 63 URL: '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_chunkset( collapse = TRUE, comment = "#>", out.width = "220px", fig.align = 'center' ) library(png) library(nvctr) library(knitcitations)  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())  **Step 5**: Also find the direction (azimuth) to$B$, relative to north {r example-01-step05} (azimuth <- atan2(p_AB_N, p_AB_N) %>% # 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) (longitude <- lat_lon_EB) # 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) (lonEB <- lat_lon_EB) # 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) (longitude <- l)  ## 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) (longitude <- l)  ## 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 (#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 vectord_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) (longitude <- l)  ## 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) (longitude <- l)  ## 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) (longitude <- l)  # References
You can’t perform that action at this time.