gps_london.py

import math

import numpy as np
from scipy.optimize import least_squares

Offset = np.array(
    [
        [
            [51.48653658, -0.15787351],
            [51.49552138, -0.15750779],
            [51.50450284, -0.15714251],
            [51.5134942, -0.15678146],
            [51.522482, -0.15641665],
            [51.53147821, -0.15603593],
        ],
        [
            [51.48629708, -0.14348133],
            [51.4952862, -0.14311014],
            [51.50427476, -0.14273894],
            [51.51326159, -0.14237039],
            [51.52225695, -0.14200299],
            [51.53124625, -0.14162871],
        ],
        [
            [51.48606092, -0.12908871],
            [51.49505511, -0.12871558],
            [51.50405668, -0.12834281],
            [51.51303504, -0.1279733],
            [51.52202077, -0.12760482],
            [51.53100586, -0.12723739],
        ],
        [
            [51.485832, -0.11469519],
            [51.49482364, -0.11431929],
            [51.50381857, -0.11394519],
            [51.51280529, -0.11357625],
            [51.5217936, -0.11320131],
            [51.53077853, -0.11282795],
        ],
        [
            [51.48560681, -0.10029999],
            [51.49459821, -0.09992166],
            [51.50358628, -0.09954092],
            [51.51256733, -0.09916756],
            [51.52156189, -0.09879787],
            [51.53055204, -0.09841437],
        ],
        [
            [51.48537856, -0.08590049],
            [51.49436934, -0.08552391],
            [51.50335272, -0.08514402],
            [51.51233467, -0.08477007],
            [51.52132308, -0.08439054],
            [51.53031843, -0.08400393],
        ],
        [
            [51.48514379, -0.07149651],
            [51.49412855, -0.07111692],
            [51.50311466, -0.0707391],
            [51.51210374, -0.07036401],
            [51.52108874, -0.06998574],
            [51.53007984, -0.06960159],
        ],
        [
            [51.48490644, -0.05709119],
            [51.49389141, -0.05671215],
            [51.50287896, -0.05633446],
            [51.51186803, -0.05595725],
            [51.5208552, -0.05557937],
            [51.5298418, -0.05520037],
        ],
    ]
)


def model2gps(X):
    # [-40, -21] ~ [21, 20]
    x, y = X
    x0, y0 = math.floor(x / 1000), math.floor(y / 1000)
    xp, yp = x0 + 4, y0 + 3
    O = Offset[xp, yp]
    Ox = Offset[xp + 1, yp]
    Oy = Offset[xp, yp + 1]
    Oxy = Offset[xp + 1, yp + 1]
    dx, dy = x / 1000 - x0, y / 1000 - y0
    return (
        O * (1 - dx) * (1 - dy)
        + Ox * dx * (1 - dy)
        + Oy * (1 - dx) * dy
        + Oxy * dx * dy
    )


def gps2model(Y0):
    def risk(X):
        Y = model2gps(X)
        return Y - Y0

    result = least_squares(risk, np.r_[0, 0], gtol=1e-12, verbose=0)
    return result.x