{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"pygments_lexer":"ipython3","nbconvert_exporter":"python","version":"3.6.4","file_extension":".py","codemirror_mode":{"name":"ipython","version":3},"name":"python","mimetype":"text/x-python"}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"markdown","source":"# Intro\n\nWe 'll simply try to replicate the baseline WLS method so that we can iterate from there\n\nInspirations:\n- https://www.kaggle.com/c/google-smartphone-decimeter-challenge/discussion/238583\n- https://www.kaggle.com/foreveryoung/least-squares-solution-from-gnss-derived-data\n- https://www.telesens.co/2017/07/17/calculating-position-from-raw-gps-data/","metadata":{}},{"cell_type":"code","source":"import pandas as pd\nimport numpy as np\n\nimport scipy.optimize as opt\n\nfrom pathlib import Path\nroot = Path(\"../input/google-smartphone-decimeter-challenge/\")\n","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:16.343747Z","iopub.execute_input":"2021-06-27T13:29:16.344118Z","iopub.status.idle":"2021-06-27T13:29:16.349805Z","shell.execute_reply.started":"2021-06-27T13:29:16.344082Z","shell.execute_reply":"2021-06-27T13:29:16.348704Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Helper functions (ecef2lla and haversine formulas)","metadata":{}},{"cell_type":"code","source":"def ecef2lla(x, y, z):\n    # x, y and z are scalars or vectors in meters\n    x = np.array([x]).reshape(np.array([x]).shape[-1], 1)\n    y = np.array([y]).reshape(np.array([y]).shape[-1], 1)\n    z = np.array([z]).reshape(np.array([z]).shape[-1], 1)\n\n    a=6378137\n    a_sq=a**2\n    e = 8.181919084261345e-2\n    e_sq = 6.69437999014e-3\n\n    f = 1/298.257223563\n    b = a*(1-f)\n\n    # calculations:\n    r = np.sqrt(x**2 + y**2)\n    ep_sq  = (a**2-b**2)/b**2\n    ee = (a**2-b**2)\n    f = (54*b**2)*(z**2)\n    g = r**2 + (1 - e_sq)*(z**2) - e_sq*ee*2\n    c = (e_sq**2)*f*r**2/(g**3)\n    s = (1 + c + np.sqrt(c**2 + 2*c))**(1/3.)\n    p = f/(3.*(g**2)*(s + (1./s) + 1)**2)\n    q = np.sqrt(1 + 2*p*e_sq**2)\n    r_0 = -(p*e_sq*r)/(1+q) + np.sqrt(0.5*(a**2)*(1+(1./q)) - p*(z**2)*(1-e_sq)/(q*(1+q)) - 0.5*p*(r**2))\n    u = np.sqrt((r - e_sq*r_0)**2 + z**2)\n    v = np.sqrt((r - e_sq*r_0)**2 + (1 - e_sq)*z**2)\n    z_0 = (b**2)*z/(a*v)\n    h = u*(1 - b**2/(a*v))\n    phi = np.arctan((z + ep_sq*z_0)/r)\n    lambd = np.arctan2(y, x)\n\n    return phi*180/np.pi, lambd*180/np.pi, h\n\ndef calc_haversine(lat1, lon1, lat2, lon2):\n    \"\"\"Calculates the great circle distance between two points\n    on the earth. Inputs are array-like and specified in decimal degrees.\n    \"\"\"\n    RADIUS = 6_367_000\n    lat1, lon1, lat2, lon2 = map(np.radians, [lat1, lon1, lat2, lon2])\n    dlat = lat2 - lat1\n    dlon = lon2 - lon1\n    a = np.sin(dlat/2)**2 + \\\n      np.cos(lat1) * np.cos(lat2) * np.sin(dlon/2)**2\n    dist = 2 * RADIUS * np.arcsin(a**0.5)\n    return dist","metadata":{"_kg_hide-input":true,"execution":{"iopub.status.busy":"2021-06-27T13:29:16.927081Z","iopub.execute_input":"2021-06-27T13:29:16.927447Z","iopub.status.idle":"2021-06-27T13:29:16.946177Z","shell.execute_reply.started":"2021-06-27T13:29:16.927416Z","shell.execute_reply":"2021-06-27T13:29:16.945175Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Apply WLS on one collection and one measurement","metadata":{}},{"cell_type":"code","source":"collection_name=\"2020-05-29-US-MTV-1\"\nfile_path = Path(f\"train/{collection_name}\")\nphone = 'Pixel4'\nmeasurement_epoch_time = 1274827487438\n\n# baseline we'll compare our solution against\ndf_baseline = pd.read_csv(root/\"baseline_locations_train.csv\")\n\n# ground truth to compute methods performance\ndf_groundtruth = pd.read_csv(root/file_path/f\"{phone}/ground_truth.csv\")\n\n# Train df here only contains one collection and one measurement\ndf_train = pd.read_csv(root/file_path/f\"{phone}/{phone}_derived.csv\")\ndf_train = df_train[df_train['millisSinceGpsEpoch'] == measurement_epoch_time] ","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:17.629215Z","iopub.execute_input":"2021-06-27T13:29:17.629601Z","iopub.status.idle":"2021-06-27T13:29:18.028168Z","shell.execute_reply.started":"2021-06-27T13:29:17.629571Z","shell.execute_reply":"2021-06-27T13:29:18.026941Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"df_train.head()","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:18.029823Z","iopub.execute_input":"2021-06-27T13:29:18.030146Z","iopub.status.idle":"2021-06-27T13:29:18.058437Z","shell.execute_reply.started":"2021-06-27T13:29:18.030115Z","shell.execute_reply":"2021-06-27T13:29:18.057124Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Corrected pseudorange according to data instructions\ndf_train['correctedPrM'] = df_train.apply(\n    lambda r: r.rawPrM + r.satClkBiasM - r.isrbM - r.ionoDelayM - r.tropoDelayM,\n    axis=1\n)\n\n# Time it took for signal to travel\nlight_speed = 299_792_458\ndf_train['transmissionTimeSeconds'] = df_train['correctedPrM'] / light_speed","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:18.246270Z","iopub.execute_input":"2021-06-27T13:29:18.246693Z","iopub.status.idle":"2021-06-27T13:29:18.256622Z","shell.execute_reply.started":"2021-06-27T13:29:18.246651Z","shell.execute_reply":"2021-06-27T13:29:18.255287Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Compute true sat positions at arrival time\nomega_e = 7.2921151467e-5\ndf_train['xSatPosMRotated'] = \\\n    np.cos(omega_e * df_train['transmissionTimeSeconds']) * df_train['xSatPosM'] \\\n    + np.sin(omega_e * df_train['transmissionTimeSeconds']) * df_train['ySatPosM']\n    \ndf_train['ySatPosMRotated'] = \\\n    - np.sin(omega_e * df_train['transmissionTimeSeconds']) * df_train['xSatPosM'] \\\n    + np.cos(omega_e * df_train['transmissionTimeSeconds']) * df_train['ySatPosM']\n    \ndf_train['zSatPosMRotated'] = df_train['zSatPosM']","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:18.611949Z","iopub.execute_input":"2021-06-27T13:29:18.612316Z","iopub.status.idle":"2021-06-27T13:29:18.623470Z","shell.execute_reply.started":"2021-06-27T13:29:18.612283Z","shell.execute_reply":"2021-06-27T13:29:18.622344Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Uncertainty weight for the WLS method\ndf_train['uncertaintyWeight'] = 1 / df_train['rawPrUncM']","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:19.230983Z","iopub.execute_input":"2021-06-27T13:29:19.231334Z","iopub.status.idle":"2021-06-27T13:29:19.236726Z","shell.execute_reply.started":"2021-06-27T13:29:19.231303Z","shell.execute_reply":"2021-06-27T13:29:19.235637Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Set up least squares methods\ndef distance(sat_pos, x):\n    sat_pos_diff = sat_pos.copy(deep=True)\n    \n    sat_pos_diff['xSatPosMRotated'] = sat_pos_diff['xSatPosMRotated'] - x[0]\n    sat_pos_diff['ySatPosMRotated'] = sat_pos_diff['ySatPosMRotated'] - x[1]\n    sat_pos_diff['zSatPosMRotated'] = sat_pos_diff['zSatPosMRotated'] - x[2]\n\n    sat_pos_diff['d'] = sat_pos_diff.apply(\n        lambda r: r.uncertaintyWeight * \n            (np.sqrt((r.xSatPosMRotated**2 + r.ySatPosMRotated**2 + r.zSatPosMRotated**2)) + x[3] - r.correctedPrM),\n        axis=1\n    )\n\n    return sat_pos_diff['d']\n\ndef distance_fixed_satpos(x):\n    return distance(df_train[['xSatPosMRotated', 'ySatPosMRotated', 'zSatPosMRotated', 'correctedPrM', 'uncertaintyWeight']], x)","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:19.861536Z","iopub.execute_input":"2021-06-27T13:29:19.861941Z","iopub.status.idle":"2021-06-27T13:29:19.869653Z","shell.execute_reply.started":"2021-06-27T13:29:19.861904Z","shell.execute_reply":"2021-06-27T13:29:19.868437Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Start point for the optimiser\nx0= [0,0,0,0]\n\nopt_res = opt.least_squares(distance_fixed_satpos, x0)\n\n# Optimiser yields a position in the ECEF coordinates\nopt_res_pos = opt_res.x","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:20.822164Z","iopub.execute_input":"2021-06-27T13:29:20.822555Z","iopub.status.idle":"2021-06-27T13:29:21.424504Z","shell.execute_reply.started":"2021-06-27T13:29:20.822524Z","shell.execute_reply":"2021-06-27T13:29:21.423495Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# ECEF position to lat/long\nwls_estimated_pos = ecef2lla(*opt_res_pos[:3])\nwls_estimated_pos = np.squeeze(wls_estimated_pos)","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:32.408080Z","iopub.execute_input":"2021-06-27T13:29:32.408453Z","iopub.status.idle":"2021-06-27T13:29:32.413180Z","shell.execute_reply.started":"2021-06-27T13:29:32.408418Z","shell.execute_reply":"2021-06-27T13:29:32.412165Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"val_baseline = df_baseline[\n    (df_baseline.collectionName==collection_name)\n    & (df_baseline.phoneName==phone)\n    & (df_baseline.millisSinceGpsEpoch==measurement_epoch_time)\n].iloc[0]","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:32.726735Z","iopub.execute_input":"2021-06-27T13:29:32.727236Z","iopub.status.idle":"2021-06-27T13:29:32.789602Z","shell.execute_reply.started":"2021-06-27T13:29:32.727193Z","shell.execute_reply":"2021-06-27T13:29:32.788660Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"val_groundtruth = df_groundtruth[\n    (df_groundtruth.collectionName==collection_name)\n    & (df_groundtruth.phoneName==phone)\n    & (df_groundtruth.millisSinceGpsEpoch==measurement_epoch_time)\n].iloc[0]","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:29:33.014393Z","iopub.execute_input":"2021-06-27T13:29:33.015096Z","iopub.status.idle":"2021-06-27T13:29:33.024779Z","shell.execute_reply.started":"2021-06-27T13:29:33.015051Z","shell.execute_reply":"2021-06-27T13:29:33.023535Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"print(\"Baseline distance with groundtruth position (m)\")\ncalc_haversine(val_baseline['latDeg'], val_baseline['lngDeg'], val_groundtruth['latDeg'], val_groundtruth['lngDeg'])","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:30:54.987805Z","iopub.execute_input":"2021-06-27T13:30:54.988443Z","iopub.status.idle":"2021-06-27T13:30:54.995354Z","shell.execute_reply.started":"2021-06-27T13:30:54.988379Z","shell.execute_reply":"2021-06-27T13:30:54.994462Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"print(\"Our estimated position (with WLS) distance with groundtruth position (m)\")\ncalc_haversine(wls_estimated_pos[0], wls_estimated_pos[1], val_groundtruth['latDeg'], val_groundtruth['lngDeg'])","metadata":{"execution":{"iopub.status.busy":"2021-06-27T13:30:52.355750Z","iopub.execute_input":"2021-06-27T13:30:52.356179Z","iopub.status.idle":"2021-06-27T13:30:52.364839Z","shell.execute_reply.started":"2021-06-27T13:30:52.356124Z","shell.execute_reply":"2021-06-27T13:30:52.363807Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# We are ~10meters from the groundtruth like the baseline","metadata":{},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"###### ","metadata":{}}]}