from scipy.optimize import least_squares
EARTH_RADIUS_KM = 6371.0
def haversine(lat1, lon1, lat2, lon2, r=EARTH_RADIUS_KM):
φ1, φ2 = math.radians(lat1), math.radians(lat2)
Δφ, Δλ = math.radians(lat2 - lat1), math.radians(lon2 - lon1)
a = math.sin(Δφ / 2) ** 2 + math.cos(φ1) * math.cos(φ2) * math.sin(Δλ / 2) ** 2
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
def _residuals(x, anchors):
haversine(lat, lon, lat_i, lon_i) - d_i
for (lat_i, lon_i, d_i) in anchors
def trilaterate(anchors, guess=None):
lats, lons = zip(*[(a[0], a[1]) for a in anchors])
guess = (sum(lats) / len(lats), sum(lons) / len(lons))
res = least_squares(_residuals, guess, args=(anchors,), method="lm")
lat_est, lon_est = res.x
rms_err = (2 * res.cost / len(anchors)) ** 0.5
return lat_est, lon_est, rms_err
if __name__ == "__main__":
(30.611436, 114.195324, 2.3),
(30.634363, 114.202718, 2.8),
(30.643873, 114.162743, 2.5),
lat, lon, err = trilaterate(anchors)
print(f"Estimated location: {lat:.6f}, {lon:.6f} (±{err:.2f} km RMS)")
# Estimated location: 30.624245, 114.176051 (±0.02 km RMS)