{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"name":"python","version":"3.7.12","mimetype":"text/x-python","codemirror_mode":{"name":"ipython","version":3},"pygments_lexer":"ipython3","nbconvert_exporter":"python","file_extension":".py"},"kaggle":{"accelerator":"none","dataSources":[{"sourceId":60095,"databundleVersionId":6542333,"sourceType":"competition"}],"dockerImageVersionId":30191,"isInternetEnabled":true,"language":"python","sourceType":"notebook","isGpuEnabled":false}},"nbformat_minor":4,"nbformat":4,"cells":[{"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":"2024-04-22T10:49:57.204105Z","iopub.execute_input":"2024-04-22T10:49:57.205324Z","iopub.status.idle":"2024-04-22T10:49:57.210232Z","shell.execute_reply.started":"2024-04-22T10:49:57.205276Z","shell.execute_reply":"2024-04-22T10:49:57.209092Z"},"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 = '../input/smartphone-decimeter-2023/sdc2023/train/2020-06-25-00-34-us-ca-mtv-sb-101/pixel4'\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":"2024-04-22T10:49:57.211793Z","iopub.execute_input":"2024-04-22T10:49:57.212737Z","iopub.status.idle":"2024-04-22T10:49:57.855703Z","shell.execute_reply.started":"2024-04-22T10:49:57.212695Z","shell.execute_reply":"2024-04-22T10:49:57.854455Z"},"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\npr_obs = np.zeros(150)\nx_sat_1st_epoch = np.zeros((150, 3))\ntime = 0\n\n\n#objective function for WLS\ndef f(y,pr_obs,x_sat,w):\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 = pr_obs - 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# Loop over epochs, i.e. single time with multiple satellites\n# i is number of iteration, t_nano is the unique value of 'TimeNanos', df1 is data frame cotain data of 1 epoch\niteration = 100\nfor i, (t_nano, df1) in enumerate(tqdm(gnss.groupby('TimeNanos'), total=n)):\n    #Get the TimeNanos as wished, if not get the first epoch\n    if(i == iteration):\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'] >= 18.0\n        idx &= df1['MultipathIndicator'] == 0\n        idx &= df1['ReceivedSvTimeUncertaintyNanos'] < 500\n        df1 = df1[idx]\n\n        n_sat_used += len(df1)\n\n        # Corrected pseudo range ρ [m]\n        pr = (df1['RawPseudorangeMeters'] + df1['SvClockBiasMeters'] - df1['IsrbMeters']\n               - df1['IonosphericDelayMeters'] - df1['TroposphericDelayMeters']).values\n        pr_obs = pr\n        # Satellite positions at emmision time t_i in ECEF(t_i)\n        x_sat = df1[['SvPositionXEcefMeters', 'SvPositionYEcefMeters', 'SvPositionZEcefMeters']].values\n        x_estimated = df1[['WlsPositionXEcefMeters', 'WlsPositionYEcefMeters', 'WlsPositionZEcefMeters']].iloc[0].values\n        time = df1[['utcTimeMillis']].iloc[0].values\n        print(f'Base line position: {x_estimated}')\n        x_sat_1st_epoch = x_sat\n        # Inverse uncertainty weight\n        w = 1 / df1['RawPseudorangeUncertaintyMeters'].values\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, args=(pr,x_sat, w)) #Arguments for least square function\n        y = opt.x[:3]\n        b = opt.x[3]\n        \n        y_wls = y\n        errs.append(opt.fun)\n\n        break\nprint(f'My WLS calculation: {y_wls}')\nprint(n_sat_used, '/', n_sat_sum)\nprint(time)","metadata":{"execution":{"iopub.status.busy":"2024-04-22T10:49:57.857739Z","iopub.execute_input":"2024-04-22T10:49:57.858029Z","iopub.status.idle":"2024-04-22T10:49:57.953829Z","shell.execute_reply.started":"2024-04-22T10:49:57.857997Z","shell.execute_reply":"2024-04-22T10:49:57.952883Z"},"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 = y_baseline[iteration]\ny_baseline.shape\nprint(y_baseline)","metadata":{"execution":{"iopub.status.busy":"2024-04-22T10:49:57.955128Z","iopub.execute_input":"2024-04-22T10:49:57.955382Z","iopub.status.idle":"2024-04-22T10:49:57.966081Z","shell.execute_reply.started":"2024-04-22T10:49:57.955352Z","shell.execute_reply":"2024-04-22T10:49:57.965061Z"},"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":"# Error\nprint('rms error', np.sqrt(np.mean((y_wls - y_baseline)**2, axis=0)), '[m]')","metadata":{"execution":{"iopub.status.busy":"2024-04-22T10:49:57.968676Z","iopub.execute_input":"2024-04-22T10:49:57.968951Z","iopub.status.idle":"2024-04-22T10:49:57.976550Z","shell.execute_reply.started":"2024-04-22T10:49:57.968920Z","shell.execute_reply":"2024-04-22T10:49:57.975409Z"},"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\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\n\ndef lla2ecef(lat, lon, alt):\n    a = 6378137\n    a_sq = a**2\n    e = 8.181919084261345e-2\n    e_sq = e**2\n    b_sq = a_sq*(1 - e_sq)\n\n    lat = np.array([lat]).reshape(np.array([lat]).shape[-1], 1)*np.pi/180\n    lon = np.array([lon]).reshape(np.array([lon]).shape[-1], 1)*np.pi/180\n    alt = np.array([alt]).reshape(np.array([alt]).shape[-1], 1)\n\n    N = a/np.sqrt(1 - e_sq*np.sin(lat)**2)\n    ecef = np.array([0, 0, 0], dtype=np.float64)\n    ecef[0] = (N+alt)*np.cos(lat)*np.cos(lon)\n    ecef[1] = (N+alt)*np.cos(lat)*np.sin(lon)\n    ecef[2] = ((b_sq/a_sq)*N+alt)*np.sin(lat)\n\n    return ecef","metadata":{"execution":{"iopub.status.busy":"2024-04-22T10:49:57.978752Z","iopub.execute_input":"2024-04-22T10:49:57.979128Z","iopub.status.idle":"2024-04-22T10:49:58.004178Z","shell.execute_reply.started":"2024-04-22T10:49:57.979085Z","shell.execute_reply":"2024-04-22T10:49:58.002725Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"**PSEUDORANGE DIFFERENCE**\n\nConvert ground truth from LLA to ECEF and calculate the difference between exact and predicted pseudorange for each satellite\nAdd any data field name to df1 to get more information","metadata":{}},{"cell_type":"code","source":"blh = to_blh(y_wls)\nblh_baseline = to_blh(y_baseline)\nprint(time[0])\nfind_truth = truth[truth['UnixTimeMillis'] == time[0]]\nbl_truth = find_truth[['LatitudeDegrees', 'LongitudeDegrees', 'AltitudeMeters']].values[0]\necef_truth = lla2ecef(bl_truth[0], bl_truth[1], bl_truth[2])\nprint('ECEF of ground truth:')\nprint(ecef_truth)\npr_truth = np.sqrt(np.sum((x_sat_1st_epoch - ecef_truth)**2, axis=1))\ntransmitTime = ((pr_truth+ (df1['IonosphericDelayMeters'] + df1['TroposphericDelayMeters']).values) / c) * 10**9\nrxTime = transmitTime + df1['ReceivedSvTimeNanosSinceGpsEpoch'].values\ndeltaT = rxTime - (df1['ArrivalTimeNanosSinceGpsEpoch'] - df1['BiasNanos']).values\ndiff = pr_obs - b - pr_truth \ndf1['deltaT'] = (deltaT / 10**9) * c \ndf1['PseudorangeDifferenceMeters'] = diff\ndf1['RawPseudorangeMeters'] = df1['RawPseudorangeMeters'] + df1['deltaT']\ndf1[['Svid', 'ConstellationType', 'SignalType', 'CarrierFrequencyHz', 'CarrierFrequencyHzRef','deltaT', 'PseudorangeDifferenceMeters']]","metadata":{"execution":{"iopub.status.busy":"2024-04-22T10:49:58.006575Z","iopub.execute_input":"2024-04-22T10:49:58.007086Z","iopub.status.idle":"2024-04-22T10:49:58.050657Z","shell.execute_reply.started":"2024-04-22T10:49:58.007031Z","shell.execute_reply":"2024-04-22T10:49:58.049405Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"print('Scores')\nprint('This work     %.4f [m]' % score(blh[:2], bl_truth[:2]))\nprint('Baseline WLS  %.4f [m]' % score(blh_baseline[:2], bl_truth[:2]))","metadata":{"execution":{"iopub.status.busy":"2024-04-22T10:49:58.052192Z","iopub.execute_input":"2024-04-22T10:49:58.052576Z","iopub.status.idle":"2024-04-22T10:49:58.062011Z","shell.execute_reply.started":"2024-04-22T10:49:58.052514Z","shell.execute_reply":"2024-04-22T10:49:58.061048Z"},"trusted":true},"execution_count":null,"outputs":[]},{"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"}}}]}