engine: start implementing trilateration

engine.py

@@ -77,6 +77,27 @@ def spherical_dist(pos1, pos2, r=6378137):
     return r * np.arccos(cos_lat_d - cos_lat1 * cos_lat2 * (1 - cos_lon_d))
 
 
+StationT = tuple[tuple[float, float], float]
+
+
+def trilat_error(stations: list[StationT], position: tuple[float, float]) -> float:
+    """Calculate the error in a trilaterated position."""
+    sq_errors = [(sdist - spherical_dist(np.array(position), np.array(spos))) ** 2 for spos, sdist in stations]
+
+    return sum(sq_errors) / len(sq_errors)
+
+
+def trilaterate(stations: list[StationT]) -> tuple[float, float]:
+    """Trilaterate a position, given a list of stations.
+
+    Each station is of the format ((lat, lon), distance).
+    """
+
+    # TODO scipy optimise using trilat_error
+
+    return (0., 0.)
+
+
 def encode(location: tuple[float, float]) -> EncodedLocation:
     """Encode a location."""