Created
May 31, 2017 08:05
-
-
Save tuupola/296fa2d87fbb033a4bca0d1a5e91cd5f to your computer and use it in GitHub Desktop.
Geometrical trilateration with 3 points in R: https://appelsiini.net/2017/trilateration-with-n-points/
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
library("pracma") | |
# Earth radius in metres | |
EARTH_RADIUS <- 6378137 | |
# Helper function for converting to ECEH coordinates | |
eceh <- function (s) { | |
vx <- EARTH_RADIUS * (cos(deg2rad(s[1])) * cos(deg2rad(s[2]))) | |
vy <- EARTH_RADIUS * (cos(deg2rad(s[1])) * sin(deg2rad(s[2]))) | |
vz <- EARTH_RADIUS * (sin(deg2rad(s[1]))) | |
c(vx, vy, vz) | |
} | |
# Helper function for normalizing a vector | |
normalize <- function (x) { x / sqrt(sum(x^2)) } | |
# Three spheres | |
s1 <- c(59.43250050, 24.76253500, 36) | |
s2 <- c(59.43170784, 24.76271075, 54) | |
s3 <- c(59.43226950, 24.76160953, 62) | |
P1 <- eceh(s1) | |
P2 <- eceh(s2) | |
P3 <- eceh(s3) | |
# ex is the unit vector in the direction from P1 to P2. | |
ex <- normalize(P2 - P1); | |
# i is the signed magnitude of the x component, in the figure 1 | |
# coordinate system, of the vector from P1 to P3. | |
i <- dot(ex, P3 -P1) | |
# ey is the unit vector in the y direction. Note that the points P1, P2 | |
# and P3 are all in the z = 0 plane of the figure 1 coordinate system. | |
ey <- normalize(P3 - P1 - ex * i); | |
# ez is third basis vector. | |
ez <- cross(ex, ey) | |
# d is the distance between the centers P1 and P2. | |
d = dist(rbind(P2, P1)) | |
# j is the signed magnitude of the y component, in the figure 1 | |
# coordinate system, of the vector from P1 to P3. | |
j = dot(ey, P3 -P1) | |
x = ( | |
s1[3] ^ 2 - | |
s2[3] ^ 2 + | |
d ^ 2 | |
) / (2 * d); | |
y = (( | |
s1[3] ^ 2 - | |
s3[3] ^ 2 + | |
i ^ 2 + j ^ 2 | |
) / (2 * j)) - ((i / j) * x); | |
# If $z = NaN if circle does not touch sphere. No solution. | |
# If $z = 0 circle touches sphere at exactly one point. | |
# If $z < 0 > z circle touches sphere at two points. | |
z = sqrt(s1[3] ^ 2 - x ^ 2 - y ^ 2) | |
# triPt is vector with ECEF x,y,z of trilateration point. | |
triPt = P1 + ex * x + ey * y + ez * z; | |
# Convert back to lat/long from ECEF. Convert to degrees. | |
latitude = rad2deg(asin(triPt[3] / EARTH_RADIUS)); | |
longitude = rad2deg(atan2(triPt[2], triPt[1])); | |
paste(latitude, longitude, sep=",") |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment