Instantly share code, notes, and snippets.

Embed
What would you like to do?
JavaScript Trilateration
// Created by Derrick Cohodas (dav-)
// Based on the Python example by StackExchange user wwnick from http://gis.stackexchange.com/a/415/41129
// Requires the Mathjs library - http://mathjs.org/
var math = require('mathjs')
/**
* Represents a coordinate with a distance
* @param {Number} lat Latitude
* @param {Number} lon Longitude
* @param {Number} dist Distance from [lat, lon] to some point in kilometers
*/
var Beacon = function(lat, lon, dist) {
this.lat = lat
this.lon = lon
this.dist = dist
return this
}
/**
* Perform a trilateration calculation to determine a location
* based on 3 beacons and their respective distances (in kilometers) to the desired point.
*
* @param {Array} beacons Array of 3 Beacon objects
* @return {Array} Array of the format [latitude, longitude]
*/
var trilaterate = function(beacons) {
// #assuming elevation = 0
var earthR = 6371
, rad = function(deg) {
return deg * (math.pi/180)
}
, deg = function(rad) {
return rad * (180/math.pi)
}
// #using authalic sphere
// #if using an ellipsoid this step is slightly different
// #Convert geodetic Lat/Long to ECEF xyz
// # 1. Convert Lat/Long to radians
// # 2. Convert Lat/Long(radians) to ECEF
var P1 = [ earthR *(math.cos(rad(beacons[0].lat)) * math.cos(rad(beacons[0].lon)))
, earthR *(math.cos(rad(beacons[0].lat)) * math.sin(rad(beacons[0].lon)))
, earthR *(math.sin(rad(beacons[0].lat)))
]
var P2 = [ earthR *(math.cos(rad(beacons[1].lat)) * math.cos(rad(beacons[1].lon)))
, earthR *(math.cos(rad(beacons[1].lat)) * math.sin(rad(beacons[1].lon)))
, earthR *(math.sin(rad(beacons[1].lat)))
]
var P3 = [ earthR *(math.cos(rad(beacons[2].lat)) * math.cos(rad(beacons[2].lon)))
, earthR *(math.cos(rad(beacons[2].lat)) * math.sin(rad(beacons[2].lon)))
, earthR *(math.sin(rad(beacons[2].lat)))
]
// #from wikipedia
// #transform to get circle 1 at origin
// #transform to get circle 2 on x axis
var ex = math.divide(math.subtract(P2, P1), math.norm( math.subtract(P2, P1) ))
var i = math.dot(ex, math.subtract(P3, P1) )
var ey = math.divide(
math.subtract(
math.subtract(P3, P1),
math.multiply(i, ex)
),
math.norm(
math.subtract(
math.subtract(P3, P1),
math.multiply(i, ex)
)
)
)
var ez = math.cross(ex, ey)
var d = math.norm(math.subtract(P2, P1))
var j = math.dot(ey, math.subtract(P3, P1))
// #from wikipedia
// #plug and chug using above values
var x = (math.pow(beacons[0].dist, 2) - math.pow(beacons[1].dist,2) + math.pow(d,2))/(2*d)
var y = ((math.pow(beacons[0].dist, 2) - math.pow(beacons[2].dist,2) + math.pow(i,2) + math.pow(j,2))/(2*j)) - ((i/j)*x)
// # only one case shown here
//
// I was having problems with the number in the radical being negative,
// so I took the absolute value. Not sure if this is always going to work
var z = math.sqrt( math.abs(math.pow(beacons[0].dist, 2) - math.pow(x, 2) - math.pow(y, 2)) )
// #triPt is an array with ECEF x,y,z of trilateration point
var triPt = math.add(
math.add(
math.add(P1,
math.multiply(x, ex)
),
math.multiply(y, ey)
),
math.multiply(z, ez)
)
// #convert back to lat/long from ECEF
// #convert to degrees
var lat = deg(math.asin(math.divide(triPt[2], earthR)))
var lon = deg(math.atan2(triPt[1], triPt[0]))
return [lat, lon]
}
var beacons = [ new Beacon(35.000000, -120.000000, 189.419265289145)
, new Beacon(35.000005, -120.000010, 189.420325082156)
, new Beacon(35.000000, -120.000020, 189.420689733286)
]
console.log(trilaterate(beacons))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment