{"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":"This notebook attempts to derive the weighted-least-square (WLS) positions, \nprovided as `WlsPositionXYZEcefMeters` from the GNSS data, but\nthe agreement so far is ~ 1-2 m.\n\n### Update\nVersion 4: I added satellite selection based on `CarrierFrequencyHz` and the accuracy improved.\nTwo WSL results do not agree for 1-2 meters, but the score against ground truth are now similar\nfor the first drive `2020-05-15-US-MTV-1`.\n\n\"in progress\" forever: I realized writing \"in progress\" in the title was not a good idea because\nit was part of the URL. Although I am not sure if I can do anything beter, I leave it in the title.\n\n## Introduction\n\n### Simplest theory\n\nWe know the GNSS satelite positions, $\\vec{x}_i$, from human civilization and the distance to them $r_i$ using the time differences. Our (receiver) location $\\vec{y}$ satisfies,\n\n$$ \\lVert \\vec{x}_i - \\vec{y} \\rVert = r_i, $$\n\nfor each satellite $i$, where $\\lVert \\cdot \\rVert$ is the usual vector length, aka Euclidean norm, or\nthe $L^2$ norm in 3 dimenssion.\n\n### More realistically\n\nAll data contain some uncertainties and after various corrections, the uncertainty\nin the receiver clock remains. This becomes an unknown offset $b$ to the distance $r_i$ and the distance including this bias is called the *pseudo range*: $\\rho_i = r_i + b$.\n\nWLS solution minimizes the error:\n\n$$ \\mathrm{WMSE} = \\sum_i \\left( \\frac{\\lVert \\vec{x}_i - \\vec{y} \\rVert - \\rho_i + b}{\\Delta \\rho_i} \\right)^2$$\n\nwhere $\\Delta \\rho_i$ is the uncertainty for $\\rho_i$ provided as `RawPseudorangeUncertaintyMeters`.\n\n4 unknowns, receiver position $\\vec{y}$ and clock bias $b$, can be solved with 4 or more satellites.\n\n### Earth rotation\n\nThe satellite positions `SvPositionXYZEcefMeters` are provided in Earth-centered Earth-fixed (ECEF) coordinate system at *signal emission time*, but the earth rotates while\nthe signal reaches the receiver. We want the satellite position in the current ECEF *coordinate at receiving time* and need to rotate the given satellite coordinates. Note that this is \n*not* satellite motion; we want satellite position at emission time, this is fixed, but the\ncoordinate system is rotating. After the signal emission, our foucus is in the motion of signal and the electromagnetic signal certainly does not rotate together with Earth at 0th order, although there could be some corrections from the atmosphere that feels the Earth rotation. It must be closer to a straight line in the non-rotating frame than a straight line in the rotating ECEF coordinate.\n\n## Reference\n\nThere are excellent WLS notebooks in the 2021 Google Smartphone Decimeter Challenge and I basically only update the column names. \n\n[1] https://www.kaggle.com/code/foreveryoung/least-squares-solution-from-gnss-derived-data\n\n[2] https://www.kaggle.com/code/hyperc/gsdc-reproducing-baseline-wls-on-one-measurement\n\n(2) adds Earth rotations to (1). I add bais corrections for Earth roation in (2) but the change is negligible unless the receiver clock error is huge.\n\nExcellent link in (2)\n\n[3] Calculating Position from Raw GPS Data: https://www.telesens.co/2017/07/17/calculating-position-from-raw-gps-data/\n\nI am watching a YouTube course:\nhttps://www.youtube.com/watch?v=o1Fyn_h6LKU&list=PLGvhNIiu1ubyEOJga50LJMzVXtbUq6CPo\n\nlinked from standford.edu:\nhttps://scpnt.stanford.edu/about/gps-mooc-massive-open-online-course\n\n\n### Disclaimer\n\nThis is my first experience handling GPS/GNSS data with no prior knowledge.\nI am writing this for the sake of my own study; correction comments are welcome.\n","metadata":{}},{"cell_type":"code","source":"import numpy as np\nimport pandas as pd\nimport matplotlib.pyplot as plt\nimport glob\nimport scipy.optimize\nfrom tqdm.auto import tqdm\n\nc = 299_792_458  # m/s in vacuum\nomega = 7.292115e-5  # angular velocity [rad/s] in WGS 84","metadata":{"execution":{"iopub.status.busy":"2022-05-19T10:19:23.495224Z","iopub.execute_input":"2022-05-19T10:19:23.495513Z","iopub.status.idle":"2022-05-19T10:19:23.501299Z","shell.execute_reply.started":"2022-05-19T10:19:23.495483Z","shell.execute_reply":"2022-05-19T10:19:23.500195Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Data\n\nOne epoch (one time) contains multiple rows from many satellites","metadata":{}},{"cell_type":"code","source":"path = '/kaggle/input/smartphone-decimeter-2022/train/2020-05-15-US-MTV-1/GooglePixel4XL'\ngnss = pd.read_csv('%s/device_gnss.csv' % path, dtype={'SignalType': str})\ntruth = pd.read_csv('%s/ground_truth.csv' % path)\n\n# Add standard Frequency column\nfrequency_median = gnss.groupby('SignalType')['CarrierFrequencyHz'].median()\ngnss = gnss.merge(frequency_median, how='left', on='SignalType', suffixes=('', 'Ref'))\n\n# One epoch\nfor t_nano, df1 in gnss.groupby('TimeNanos'):\n    break\ndf1[['Svid', 'ConstellationType', 'SignalType', 'CarrierFrequencyHz', 'CarrierFrequencyHzRef']]","metadata":{"execution":{"iopub.status.busy":"2022-05-19T10:19:23.558374Z","iopub.execute_input":"2022-05-19T10:19:23.558658Z","iopub.status.idle":"2022-05-19T10:19:24.701081Z","shell.execute_reply.started":"2022-05-19T10:19:23.558624Z","shell.execute_reply":"2022-05-19T10:19:24.700237Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# WLS Position estimation","metadata":{}},{"cell_type":"code","source":"n = len(truth)\ny_wls = np.zeros((n, 3))\nerrs = []\nn_sat_sum = 0   # cumulative number of satellites in data\nn_sat_used = 0  # cumulative number of satellites used for WLS\n\n# Loop over epochs, i.e. single time with multiple satellites\nfor i, (t_nano, df1) in enumerate(tqdm(gnss.groupby('TimeNanos'), total=n)):\n    n_sat_sum += len(df1)\n        \n    # Satellite selection\n    idx = (df1['CarrierFrequencyHz'] - df1['CarrierFrequencyHzRef']) < 2.5e6\n    # one-side cut seems fine for this drive but abs() < th is also natural\n    #idx &= df1['Cn0DbHz'] >= 20.0\n    df1 = df1[idx]\n    \n    n_sat_used += len(df1)\n    \n    # Corrected pseudo range ρ [m]\n    rho = (df1['RawPseudorangeMeters'] + df1['SvClockBiasMeters'] - df1['IsrbMeters']\n           - df1['IonosphericDelayMeters'] - df1['TroposphericDelayMeters']).values\n\n    # Satellite positions at emmision time t_i in ECEF(t_i)\n    x_sat = df1[['SvPositionXEcefMeters', 'SvPositionYEcefMeters', 'SvPositionZEcefMeters']].values\n\n    # Inverse uncertainty weight\n    w = 1 / df1['RawPseudorangeUncertaintyMeters'].values\n\n    def f(y):\n        \"\"\"\n        Compute error for guess y\n\n        y (y1, y2, y3, b):\n          y: recerver position at receiving time\n          b: receiver clock bias in meters\n        \"\"\"\n        b = y[3]\n        r = rho - b  # distance to each satellite [m]\n        tau = r / c  # signal flight time\n\n        # Rotate satellite positions at emission for present ECEF coordinate\n        x = np.empty_like(x_sat)\n        cosO = np.cos(omega * tau)\n        sinO = np.sin(omega * tau)\n        x[:, 0] =  cosO * x_sat[:, 0] + sinO * x_sat[:, 1]\n        x[:, 1] = -sinO * x_sat[:, 0] + cosO * x_sat[:, 1]\n        x[:, 2] = x_sat[:, 2]\n\n        return w * (np.sqrt(np.sum((x - y[:3])**2, axis=1)) - r)\n\n    \n    # Fit receiver position y and clock bias b\n    x0 = np.zeros(4)  # Initial guess\n    opt = scipy.optimize.least_squares(f, x0)\n    y = opt.x[:3]\n    b = opt.x[3]\n    \n    y_wls[i, :] = y\n    errs.append(opt.fun)\n    \nprint(n_sat_used, '/', n_sat_sum)","metadata":{"execution":{"iopub.status.busy":"2022-05-19T10:19:24.702997Z","iopub.execute_input":"2022-05-19T10:19:24.703242Z","iopub.status.idle":"2022-05-19T10:20:36.317363Z","shell.execute_reply.started":"2022-05-19T10:19:24.703208Z","shell.execute_reply":"2022-05-19T10:20:36.316547Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# Provided WLS position\ny_baseline = gnss.groupby('TimeNanos')[['WlsPositionXEcefMeters', 'WlsPositionYEcefMeters', 'WlsPositionZEcefMeters']].mean().values\ny_baseline.shape","metadata":{"execution":{"iopub.status.busy":"2022-05-19T10:20:36.319066Z","iopub.execute_input":"2022-05-19T10:20:36.319622Z","iopub.status.idle":"2022-05-19T10:20:36.334235Z","shell.execute_reply.started":"2022-05-19T10:20:36.319576Z","shell.execute_reply":"2022-05-19T10:20:36.333342Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Compare to WLS baseline\n\nComparison between two calculations supposed to be same in method and data.","metadata":{}},{"cell_type":"code","source":"plt.figure(figsize=(14, 6))\nplt.suptitle('Position estimation (xyz)')\nfor k in range(3):\n    plt.subplot(2, 3, k + 1)\n    plt.title('xyz'[k])\n    plt.plot(y_baseline[:, k], label='Baseline WLS')\n    plt.plot(y_wls[:, k], label='This notebook')\n    \n    if k == 0:\n        plt.ylabel('Position [m]')\n        plt.legend(frameon=False)\n    \n    plt.subplot(2, 3, k + 4)\n    plt.axhline(0, color='gray', alpha=0.5)\n    plt.plot(y_wls[:, k] - y_baseline[:, k], label='This notebook')\n    \n    if k == 0:\n        plt.ylabel('WLS disagreement [m]')\n\n# Error\nprint('rms error', np.sqrt(np.mean((y_wls - y_baseline)**2, axis=0)), '[m]')","metadata":{"execution":{"iopub.status.busy":"2022-05-19T10:20:36.336742Z","iopub.execute_input":"2022-05-19T10:20:36.337570Z","iopub.status.idle":"2022-05-19T10:20:37.055649Z","shell.execute_reply.started":"2022-05-19T10:20:36.337516Z","shell.execute_reply":"2022-05-19T10:20:37.054645Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Comprare to ground truth","metadata":{}},{"cell_type":"code","source":"\"\"\"\nxyz to latitude longitude\nThanks to: Akio Saito\nhttps://www.kaggle.com/code/saitodevel01/gsdc2-baseline-submission\n\"\"\"\n    \nWGS84_SEMI_MAJOR_AXIS = 6378137.0\nWGS84_SEMI_MINOR_AXIS = 6356752.314245\n\ndef to_blh(xyz, *, unit='degree'):\n    \"\"\"\n    Args:\n      x, y, z (float): ecef coordinate in meters\n      unit (str): Unit for latitude longitude, degree or radian\n    Returns:\n      B: geodetic latitude\n      L: geodesic longitude\n      H: height above the ellipsoid\n    \"\"\"\n    assert unit == 'degree' or unit == 'radian'\n    \n    x = xyz[:, 0]\n    y = xyz[:, 1]\n    z = xyz[:, 2]\n    blh = np.empty_like(xyz)\n\n    # Ellipsoidal parameters\n    a = WGS84_SEMI_MAJOR_AXIS\n    b = WGS84_SEMI_MINOR_AXIS\n    f = (a - b) / a\n    e2 = 2*f - f**2\n    ep2 = (a**2 - b**2) / b**2\n\n    # Transformation\n    r = np.sqrt(x**2 + y**2)\n    theta = np.arctan2(z * (a / b), r)\n    B = np.arctan2(z + (ep2 * b) * np.sin(theta)**3, r - (e2 * a) * np.cos(theta)**3)\n    blh[:, 0] = B\n    blh[:, 1] = np.arctan2(y, x)\n    n = a / np.sqrt(1 - e2 * np.sin(B)**2)\n    blh[:, 2] = (r / np.cos(B)) - n\n\n    if unit == 'degree':\n        blh[:, 0] = np.degrees(blh[:, 0])\n        blh[:, 1] = np.degrees(blh[:, 1])\n\n    return blh\n\n\ndef haversine_distance(x, y, *, unit='degree'):\n    \"\"\"\n    Compute haversine on sphere\n    x (latitude, longitude)\n    y (latitude, longitude)\n    \"\"\"\n    assert unit in ['degree', 'radian']\n    HAVERSINE_RADIUS = 6371_000\n\n    if unit == 'degree':\n        x = np.radians(x)\n        y = np.radians(y)\n    \n    phi_x = x[:, 0]\n    phi_y = y[:, 0]\n    db = phi_x - phi_y\n    dl = x[:, 1] - y[:, 1]\n\n    a = np.sin(db / 2)**2 + np.cos(phi_x) * np.cos(phi_y) * np.sin(dl / 2)**2\n    dist = 2 * HAVERSINE_RADIUS * np.arcsin(np.sqrt(a))\n\n    return dist\n\ndef score(x, y):\n    dists = haversine_distance(x, y)\n    score = np.mean([np.quantile(dists, 0.50), np.quantile(dists, 0.95)])    \n    return score","metadata":{"execution":{"iopub.status.busy":"2022-05-19T10:20:37.056941Z","iopub.execute_input":"2022-05-19T10:20:37.057235Z","iopub.status.idle":"2022-05-19T10:20:37.078966Z","shell.execute_reply.started":"2022-05-19T10:20:37.057195Z","shell.execute_reply":"2022-05-19T10:20:37.078224Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"blh = to_blh(y_wls)\nblh_baseline = to_blh(y_baseline)\nbl_truth = truth[['LatitudeDegrees', 'LongitudeDegrees']].values\n\nplt.figure(figsize=(12, 6))\n\nfor k in range(2):\n    plt.subplot(2, 2, k + 1)\n    plt.title(['Latitude', 'Longitude'][k])\n    plt.plot(bl_truth[:, k], color='gray', label='truth')\n    plt.plot(blh_baseline[:, k], label='baseline')\n    plt.plot(blh[:, k], label='this notebook')\n    \n    if k == 0:\n        plt.ylabel('lat/lon [degree]')\n    \n    plt.subplot(2, 2, k + 3)\n    plt.axhline(0, color='gray')\n    plt.plot(blh_baseline[:, k] - bl_truth[:, k], label='baseline')\n    plt.plot(blh[:, k] - bl_truth[:, k], alpha=0.5, label='this notebook')\n    \n    if k == 0:\n        plt.ylabel('Error [degree]')\n        plt.legend(frameon=False)","metadata":{"execution":{"iopub.status.busy":"2022-05-19T10:20:37.080198Z","iopub.execute_input":"2022-05-19T10:20:37.080559Z","iopub.status.idle":"2022-05-19T10:20:37.686706Z","shell.execute_reply.started":"2022-05-19T10:20:37.080521Z","shell.execute_reply":"2022-05-19T10:20:37.685975Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"print('Scores')\nprint('This work     %.4f [m]' % score(blh[:, :2], bl_truth))\nprint('Baseline WLS  %.4f [m]' % score(blh_baseline[:, :2], bl_truth))","metadata":{"execution":{"iopub.status.busy":"2022-05-19T10:20:37.687824Z","iopub.execute_input":"2022-05-19T10:20:37.688082Z","iopub.status.idle":"2022-05-19T10:20:37.696418Z","shell.execute_reply.started":"2022-05-19T10:20:37.688049Z","shell.execute_reply":"2022-05-19T10:20:37.695812Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"|                | RMSE [m] | score [m] | nunber of data  |\n|:---             |---      |---        |---              |\n|No selection    | 3.8214   | 5.4925    | 90153 / 90153   |\n|f cut 2.5MHz    | 3.0545   | 4.2920    | 89713 / 90153   |\n|+Cn0DbHz' ≧ 20.0| 3.1383   | 4.3750    | 86612 / 90153   |\n|Baseline        | 3.0707   | 4.3185    |                 |","metadata":{}},{"cell_type":"markdown","source":"* My WLS disagree with the WLS baseline for ~ 1-2 m in each direction;\n* but both WLS perform similarly against ground truth after CarrierFrequency selection.\n* The error is obviously large without CarrierFrequency selection.\n* Carrier-to-noise ≧ 20 db-Hz does not improve the score.","metadata":{}},{"cell_type":"markdown","source":"## Remark\n\nOther attempts I made.\n\n### 1. Signal-type dependent biases\n\nIn the discussion in 2021,\nhttps://www.kaggle.com/c/google-smartphone-decimeter-challenge/discussion/238583\n    \nThe organizer said:\n\n> It has 4+N states, where 4 refers to the user's position in ECEF and clock offset (x, y, z, t), and N states are inter-signal biases (ISB) for the number of non-GPS-L1 signal types. For instance, if the device measures signals of GPS L1 frequency, GLO G1 frequency, GPS L5 frequency, GAL E1 frequency at the same epoch, the number of non-GPS-L1 signal types equals 3 (i.e. N=3).\n\nThis probably means that there are biases for each signal types, 4 + N parameters in the least-square fitting.\n\nHowever, this only made less than 1e-3 m change.\n\n### 2. Remove atmospheric correction for tau\nWhen we correct for the ECEF rotation during signal arrival, what we want is time and I assume speed of light in vacuum tau = r / c\nto get the time.\nThe atmospheric corrections, IonosphericDelayMeters and TroposphericDelayMeters, are probably correction to the effective positions of satellites for non-vacuume speed of light, and the delays are true delays in time. Therefore using time difference without those atmospheric corrections might be better for tau:\n\n```\ntau_c = (df1['RawPseudorangeMeters'] + df1['SvClockBiasMeters'])  # without two delays\ntau = (tau_c - b) / c\n```\n\nHowever, this only makes 1e-3 m difference to the xyz estimation and I leave the equation simple.\n\n### 3. ReceivedSvTimeUncertaintyNanos\n\n`ReceivedSvTimeUncertaintyNanos` is exactly proportional to `RawPseudorangeUncertaintyMeters`; using which of them for weight does not matter.\n\n### 4. Invalid measurements\n\nThe discussion above describes several criteria for \"invalid measurements.\" Same critera are written in\nthe data section for `[train/test]/[drive_id]/[phone_name]/supplemental/rinex.o`, e.g.,\n\n- CN0 is less than 20 dB-Hz\n- Carrier frequency is out of nominal range of each band\n\nI read the `supplemental/gnss_rinex.20o` and remove the data with blank pseudo range in that file, but\nagreement between two WLS did not change very much.\n\n","metadata":{}},{"cell_type":"markdown","source":"### Computation speed\n\nThis notebook is not aiming for computation speed. Textbook example of least-square fitting is linearizing the equation and using linear solver. The trigonometric functions for rotations can also be Taylor expended for small rotation angles.","metadata":{"execution":{"iopub.status.busy":"2022-05-14T11:15:33.630263Z","iopub.execute_input":"2022-05-14T11:15:33.630648Z","iopub.status.idle":"2022-05-14T11:15:33.642723Z","shell.execute_reply.started":"2022-05-14T11:15:33.630594Z","shell.execute_reply":"2022-05-14T11:15:33.641126Z"}}}]}