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."""
 
@@ -118,7 +139,7 @@ def decode(location: EncodedLocation) -> tuple[float, float]:
     dist_series_list = []
     for i in dist_matrix:
         dist_series_list.append(pd.Series(i).sort_values().head(len(location[0][1])+1)[1:])
-    
+
     #part 1: find the ID of each gregg's
     closest_greggs = []
     for i in range(len(location)):
@@ -130,10 +151,10 @@ def decode(location: EncodedLocation) -> tuple[float, float]:
         minerr = min(errors)
         if minerr > 1:
             print(f"warning: high error value of {minerr}")
-            
+
         closest_greggs = [errors.index(min(errors))]
 
-        
+
 
 
     #part 2: trilaterate