Skip to content

Instantly share code, notes, and snippets.

@tuupola
Created May 31, 2017 08:05
Show Gist options
  • Save tuupola/296fa2d87fbb033a4bca0d1a5e91cd5f to your computer and use it in GitHub Desktop.
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/
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