diff --git a/engine.py b/engine.py index 0bed80c..7fe3cfc 100755 --- a/engine.py +++ b/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