{"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":"code","source":"!pip install pymap3d\n\nimport numpy as np\nimport pandas as pd\nimport pymap3d as pm\nimport pymap3d.vincenty as pmv\nimport matplotlib.pyplot as plt\nimport glob as gl\nimport scipy.optimize\nfrom tqdm.auto import tqdm\nfrom scipy.interpolate import InterpolatedUnivariateSpline\nfrom scipy.spatial import distance\n\n# Constants\nCLIGHT = 299_792_458   # speed of light (m/s)\nRE_WGS84 = 6_378_137   # earth semimajor axis (WGS84) (m)\nOMGE = 7.2921151467E-5  # earth angular velocity (IS-GPS) (rad/s)","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:51:47.425961Z","iopub.execute_input":"2022-06-06T17:51:47.426462Z","iopub.status.idle":"2022-06-06T17:52:25.143024Z","shell.execute_reply.started":"2022-06-06T17:51:47.426428Z","shell.execute_reply":"2022-06-06T17:52:25.141932Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Satellite Selection","metadata":{}},{"cell_type":"code","source":"# Satellite selection using carrier frequency error, elevation angle, and C/N0\ndef satellite_selection(df, column):\n    \"\"\"\n    Args:\n        df : DataFrame from device_gnss.csv\n        column : Column name\n    Returns:\n        df: DataFrame with eliminated satellite signals\n    \"\"\"\n    idx = df[column].notnull()\n    idx &= df['CarrierErrorHz'] < 2.0e6  # carrier frequency error (Hz)\n    idx &= df['SvElevationDegrees'] > 10.0  # elevation angle (deg)\n    idx &= df['Cn0DbHz'] > 15.0  # C/N0 (dB-Hz)\n    idx &= df['MultipathIndicator'] == 0 # Multipath flag\n\n    return df[idx]","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:52:25.144513Z","iopub.execute_input":"2022-06-06T17:52:25.145000Z","iopub.status.idle":"2022-06-06T17:52:25.154517Z","shell.execute_reply.started":"2022-06-06T17:52:25.144955Z","shell.execute_reply":"2022-06-06T17:52:25.153546Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Pseudorange/Doppler Residuals and Jacobian","metadata":{}},{"cell_type":"code","source":"# Compute line-of-sight vector from user to satellite\ndef los_vector(xusr, xsat):\n    \"\"\"\n    Args:\n        xusr : user position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n    Returns:\n        u: unit line-of-sight vector in ECEF (m)\n        rng: distance between user and satellite (m)\n    \"\"\"\n    u = xsat - xusr\n    rng = np.linalg.norm(u, axis=1).reshape(-1, 1)\n    u /= rng\n    \n    return u, rng.reshape(-1)\n\n\n# Compute Jacobian matrix\ndef jac_pr_residuals(x, xsat, pr, W):\n    \"\"\"\n    Args:\n        x : current position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n        pr : pseudorange (m)\n        W : weight matrix\n    Returns:\n        W*J : Jacobian matrix\n    \"\"\"\n    u, _ = los_vector(x[:3], xsat)\n    J = np.hstack([-u, np.ones([len(pr), 1])])  # J = [-ux -uy -uz 1]\n\n    return W @ J\n\n\n# Compute pseudorange residuals\ndef pr_residuals(x, xsat, pr, W):\n    \"\"\"\n    Args:\n        x : current position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n        pr : pseudorange (m)\n        W : weight matrix\n    Returns:\n        residuals*W : pseudorange residuals\n    \"\"\"\n    u, rng = los_vector(x[:3], xsat)\n\n    # Approximate correction of the earth rotation (Sagnac effect) often used in GNSS positioning\n    rng += OMGE * (xsat[:, 0] * x[1] - xsat[:, 1] * x[0]) / CLIGHT\n\n    # Add GPS L1 clock offset\n    residuals = rng - (pr - x[3])\n\n    return residuals @ W\n\n\n# Compute Jacobian matrix\ndef jac_prr_residuals(v, vsat, prr, x, xsat, W):\n    \"\"\"\n    Args:\n        v : current velocity in ECEF (m/s)\n        vsat : satellite velocity in ECEF (m/s)\n        prr : pseudorange rate (m/s)\n        x : current position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n        W : weight matrix\n    Returns:\n        W*J : Jacobian matrix\n    \"\"\"\n    u, _ = los_vector(x[:3], xsat)\n    J = np.hstack([-u, np.ones([len(prr), 1])])\n\n    return np.dot(W, J)\n\n\n# Compute pseudorange rate residuals\ndef prr_residuals(v, vsat, prr, x, xsat, W):\n    \"\"\"\n    Args:\n        v : current velocity in ECEF (m/s)\n        vsat : satellite velocity in ECEF (m/s)\n        prr : pseudorange rate (m/s)\n        x : current position in ECEF (m)\n        xsat : satellite position in ECEF (m)\n        W : weight matrix\n    Returns:\n        residuals*W : pseudorange rate residuals\n    \"\"\"\n    u, rng = los_vector(x[:3], xsat)\n    rate = np.sum((vsat-v[:3])*u, axis=1) \\\n          + OMGE / CLIGHT * (vsat[:, 1] * x[0] + xsat[:, 1] * v[0]\n                           - vsat[:, 0] * x[1] - xsat[:, 0] * v[1])\n\n    residuals = rate - (prr - v[3])\n\n    return residuals @ W","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:52:25.157409Z","iopub.execute_input":"2022-06-06T17:52:25.158080Z","iopub.status.idle":"2022-06-06T17:52:25.181247Z","shell.execute_reply.started":"2022-06-06T17:52:25.158031Z","shell.execute_reply":"2022-06-06T17:52:25.180180Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Carrier Smoothing\nSmoothing noisy pseudoranges with accurate (but absolute distance bias exists) carrier phase. Note that this is a post-processing method. The average absolute distance is added to continuously observed carrier phase.","metadata":{}},{"cell_type":"code","source":"# Carrier smoothing of pseudarange\ndef carrier_smoothing(gnss_df):\n    \"\"\"\n    Args:\n        df : DataFrame from device_gnss.csv\n    Returns:\n        df: DataFrame with carrier-smoothing pseudorange 'pr_smooth'\n    \"\"\"\n    carr_th = 1.6 # carrier phase jump threshold [m] 2->1.5 (best)->1.0\n    pr_th =  20.0 # pseudorange jump threshold [m] 20->15\n\n    prsmooth = np.full_like(gnss_df['RawPseudorangeMeters'], np.nan)\n    # Loop for each signal\n    for (i, (svid_sigtype, df)) in enumerate((gnss_df.groupby(['Svid', 'SignalType']))):\n        df = df.replace(\n            {'AccumulatedDeltaRangeMeters': {0: np.nan}})  # 0 to NaN\n\n        # Compare time difference between pseudorange/carrier with Doppler\n        drng1 = df['AccumulatedDeltaRangeMeters'].diff() - df['PseudorangeRateMetersPerSecond']\n        drng2 = df['RawPseudorangeMeters'].diff() - df['PseudorangeRateMetersPerSecond']\n\n        # Check cycle-slip\n        slip1 = (df['AccumulatedDeltaRangeState'].to_numpy() & 2**1) != 0  # reset flag\n        slip2 = (df['AccumulatedDeltaRangeState'].to_numpy() & 2**2) != 0  # cycle-slip flag\n        slip3 = np.fabs(drng1.to_numpy()) > carr_th # Carrier phase jump\n        slip4 = np.fabs(drng2.to_numpy()) > pr_th # Pseudorange jump\n\n        idx_slip = slip1 | slip2 | slip3 | slip4\n        idx_slip[0] = True\n\n        # groups with continuous carrier phase tracking\n        df['group_slip'] = np.cumsum(idx_slip)\n\n        # Psudorange - carrier phase\n        df['dpc'] = df['RawPseudorangeMeters'] - df['AccumulatedDeltaRangeMeters']\n\n        # Absolute distance bias of carrier phase\n        meandpc = df.groupby('group_slip')['dpc'].mean()\n        df = df.merge(meandpc, on='group_slip', suffixes=('', '_Mean'))\n\n        # Index of original gnss_df\n        idx = (gnss_df['Svid'] == svid_sigtype[0]) & (\n            gnss_df['SignalType'] == svid_sigtype[1])\n\n        # Carrier phase + bias\n        prsmooth[idx] = df['AccumulatedDeltaRangeMeters'] + df['dpc_Mean']\n\n    # If carrier smoothing is not possible, use original pseudorange\n    idx_nan = np.isnan(prsmooth)\n    prsmooth[idx_nan] = gnss_df['RawPseudorangeMeters'][idx_nan]\n    gnss_df['pr_smooth'] = prsmooth\n\n    return gnss_df","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:52:25.185072Z","iopub.execute_input":"2022-06-06T17:52:25.185688Z","iopub.status.idle":"2022-06-06T17:52:25.200406Z","shell.execute_reply.started":"2022-06-06T17:52:25.185640Z","shell.execute_reply":"2022-06-06T17:52:25.199679Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Score Computation","metadata":{}},{"cell_type":"code","source":"# Compute distance by Vincenty's formulae\ndef vincenty_distance(llh1, llh2):\n    \"\"\"\n    Args:\n        llh1 : [latitude,longitude] (deg)\n        llh2 : [latitude,longitude] (deg)\n    Returns:\n        d : distance between llh1 and llh2 (m)\n    \"\"\"\n    d, az = np.array(pmv.vdist(llh1[:, 0], llh1[:, 1], llh2[:, 0], llh2[:, 1]))\n\n    return d\n\n\n# Compute score\ndef calc_score(llh, llh_gt):\n    \"\"\"\n    Args:\n        llh : [latitude,longitude] (deg)\n        llh_gt : [latitude,longitude] (deg)\n    Returns:\n        score : (m)\n    \"\"\"\n    d = vincenty_distance(llh, llh_gt)\n    score = np.mean([np.quantile(d, 0.50), np.quantile(d, 0.95)])\n\n    return score","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:52:25.201669Z","iopub.execute_input":"2022-06-06T17:52:25.202115Z","iopub.status.idle":"2022-06-06T17:52:25.215505Z","shell.execute_reply.started":"2022-06-06T17:52:25.202084Z","shell.execute_reply":"2022-06-06T17:52:25.214788Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Robust WLS\nI used `soft_l1` loss function. It is robust, but its computation speed is considerably slower than that of ordinary least squares...","metadata":{}},{"cell_type":"code","source":"# GNSS single point positioning using pseudorange\ndef point_positioning(gnss_df):\n    # Add nominal frequency to each signal\n    # Note: GLONASS is an FDMA signal, so each satellite has a different frequency\n    CarrierFrequencyHzRef = gnss_df.groupby(['Svid', 'SignalType'])[\n        'CarrierFrequencyHz'].median()\n    gnss_df = gnss_df.merge(CarrierFrequencyHzRef, how='left', on=[\n                            'Svid', 'SignalType'], suffixes=('', 'Ref'))\n    gnss_df['CarrierErrorHz'] = np.abs(\n        (gnss_df['CarrierFrequencyHz'] - gnss_df['CarrierFrequencyHzRef']))\n\n    # Carrier smoothing\n    gnss_df = carrier_smoothing(gnss_df)\n\n    # GNSS single point positioning\n    utcTimeMillis = gnss_df['utcTimeMillis'].unique()\n    nepoch = len(utcTimeMillis)\n    x0 = np.zeros(4)  # [x,y,z,tGPSL1]\n    v0 = np.zeros(4)  # [vx,vy,vz,dtGPSL1]\n    x_wls = np.full([nepoch, 3], np.nan)  # For saving position\n    v_wls = np.full([nepoch, 3], np.nan)  # For saving velocity\n\n    # Loop for epochs\n    for i, (t_utc, df) in enumerate(tqdm(gnss_df.groupby('utcTimeMillis'), total=nepoch)):\n        # Valid satellite selection\n        df_pr = satellite_selection(df, 'pr_smooth')\n        df_prr = satellite_selection(df, 'PseudorangeRateMetersPerSecond')\n\n        # Corrected pseudorange/pseudorange rate\n        pr = (df_pr['pr_smooth'] + df_pr['SvClockBiasMeters'] - df_pr['IsrbMeters'] -\n              df_pr['IonosphericDelayMeters'] - df_pr['TroposphericDelayMeters']).to_numpy()\n        prr = (df_prr['PseudorangeRateMetersPerSecond'] +\n               df_prr['SvClockDriftMetersPerSecond']).to_numpy()\n\n        # Satellite position/velocity\n        xsat_pr = df_pr[['SvPositionXEcefMeters', 'SvPositionYEcefMeters',\n                         'SvPositionZEcefMeters']].to_numpy()\n        xsat_prr = df_prr[['SvPositionXEcefMeters', 'SvPositionYEcefMeters',\n                           'SvPositionZEcefMeters']].to_numpy()\n        vsat = df_prr[['SvVelocityXEcefMetersPerSecond', 'SvVelocityYEcefMetersPerSecond',\n                       'SvVelocityZEcefMetersPerSecond']].to_numpy()\n\n        # Weight matrix for peseudorange/pseudorange rate\n        Wx = np.diag(1 / df_pr['RawPseudorangeUncertaintyMeters'].to_numpy())\n        Wv = np.diag(1 / df_prr['PseudorangeRateUncertaintyMetersPerSecond'].to_numpy())\n\n        # Robust WLS requires accurate initial values for convergence,\n        # so perform normal WLS for the first time\n        if len(df_pr) >= 4:\n            # Normal WLS\n            if np.all(x0 == 0):\n                opt = scipy.optimize.least_squares(\n                    pr_residuals, x0, jac_pr_residuals, args=(xsat_pr, pr, Wx))\n                x0 = opt.x \n            # Robust WLS for position estimation\n            opt = scipy.optimize.least_squares(\n                 pr_residuals, x0, jac_pr_residuals, args=(xsat_pr, pr, Wx), loss='soft_l1')\n            if opt.status < 1 or opt.status == 2:\n                 print(f'i = {i} position lsq status = {opt.status}')\n            else:\n                 x_wls[i, :] = opt.x[:3]\n                 x0 = opt.x\n                 \n        # Velocity estimation\n        if len(df_prr) >= 4:\n            if np.all(v0 == 0): # Normal WLS\n                opt = scipy.optimize.least_squares(\n                    prr_residuals, v0, jac_prr_residuals, args=(vsat, prr, x0, xsat_prr, Wv))\n                v0 = opt.x\n            # Robust WLS for velocity estimation\n            opt = scipy.optimize.least_squares(\n                prr_residuals, v0, jac_prr_residuals, args=(vsat, prr, x0, xsat_prr, Wv), loss='soft_l1')\n            if opt.status < 1:\n                print(f'i = {i} velocity lsq status = {opt.status}')\n            else:\n                v_wls[i, :] = opt.x[:3]\n                v0 = opt.x\n\n    return utcTimeMillis, x_wls, v_wls","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:52:25.216662Z","iopub.execute_input":"2022-06-06T17:52:25.217074Z","iopub.status.idle":"2022-06-06T17:52:25.237098Z","shell.execute_reply.started":"2022-06-06T17:52:25.217044Z","shell.execute_reply":"2022-06-06T17:52:25.236306Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Outlier Detection and Interpolation","metadata":{}},{"cell_type":"code","source":"# Simple outlier detection and interpolation\ndef exclude_interpolate_outlier(x_wls, v_wls):\n    # Up velocity threshold\n    v_up_th = 2.0 # m/s\n\n    # Coordinate conversion\n    x_llh = np.array(pm.ecef2geodetic(x_wls[:, 0], x_wls[:, 1], x_wls[:, 2])).T\n    v_enu = np.array(pm.ecef2enuv(\n        v_wls[:, 0], v_wls[:, 1], v_wls[:, 2], x_llh[0, 0], x_llh[0, 1])).T\n\n    # Up velocity jump detection\n    # Cars don't jump suddenly!\n    idx_v_out = np.abs(v_enu[:, 2]) > v_up_th\n    v_wls[idx_v_out, :] = np.nan\n    \n    # Interpolate NaNs at beginning and end of array\n    x_df = pd.DataFrame({'x': x_wls[:, 0], 'y': x_wls[:, 1], 'z': x_wls[:, 2]})\n    x_df = x_df.interpolate(limit_area='outside', limit_direction='both')\n    \n    # Interpolate all NaN data\n    v_df = pd.DataFrame({'x': v_wls[:, 0], 'y': v_wls[:, 1], 'z': v_wls[:, 2]})\n    v_df = v_df.interpolate(limit_area='outside', limit_direction='both')\n    v_df = v_df.interpolate('spline', order=3)\n\n    return x_df.to_numpy(), v_df.to_numpy()","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:52:25.238163Z","iopub.execute_input":"2022-06-06T17:52:25.238619Z","iopub.status.idle":"2022-06-06T17:52:25.255015Z","shell.execute_reply.started":"2022-06-06T17:52:25.238568Z","shell.execute_reply":"2022-06-06T17:52:25.254039Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Kalman Smoother\nApplication of Kalman smoother; integration of velocity and position obtained by WLS.\nThe covariance matrix is a fixed value.","metadata":{}},{"cell_type":"code","source":"# Kalman filter\ndef Kalman_filter(zs, us, phone):\n    # Parameters\n    # I don't know why only XiaomiMi8 seems to be inaccurate ... \n    sigma_v = 0.6 if phone == 'XiaomiMi8' else 0.1 # velocity SD m/s\n    sigma_x = 5.0  # position SD m\n    sigma_mahalanobis = 30.0 # Mahalanobis distance for rejecting innovation\n    \n    n, dim_x = zs.shape\n    F = np.eye(3)  # Transition matrix\n    Q = sigma_v**2 * np.eye(3)  # Process noise\n\n    H = np.eye(3)  # Measurement function\n    R = sigma_x**2 * np.eye(3)  # Measurement noise\n\n    # Initial state and covariance\n    x = zs[0, :3].T  # State\n    P = sigma_x**2 * np.eye(3)  # State covariance\n    I = np.eye(dim_x)\n\n    x_kf = np.zeros([n, dim_x])\n    P_kf = np.zeros([n, dim_x, dim_x])\n\n    # Kalman filtering\n    for i, (u, z) in enumerate(zip(us, zs)):\n        # First step\n        if i == 0:\n            x_kf[i] = x.T\n            P_kf[i] = P\n            continue\n\n        # Prediction step\n        x = F @ x + u.T\n        P = (F @ P) @ F.T + Q\n\n        # Check outliers for observation\n        d = distance.mahalanobis(z, H @ x, np.linalg.pinv(P))\n\n        # Update step\n        if d < sigma_mahalanobis:\n            y = z.T - H @ x\n            S = (H @ P) @ H.T + R\n            K = (P @ H.T) @ np.linalg.inv(S)\n            x = x + K @ y\n            P = (I - (K @ H)) @ P\n        else:\n            # If no observation update is available, increase covariance\n            P += 10**2*Q\n\n        x_kf[i] = x.T\n        P_kf[i] = P\n\n    return x_kf, P_kf\n\n\n# Forward + backward Kalman filter and smoothing\ndef Kalman_smoothing(x_wls, v_wls, phone):\n    n, dim_x = x_wls.shape\n\n    # Forward\n    v = np.vstack([np.zeros([1, 3]), (v_wls[:-1, :] + v_wls[1:, :])/2])\n    x_f, P_f = Kalman_filter(x_wls, v, phone)\n\n    # Backward\n    v = -np.flipud(v_wls)\n    v = np.vstack([np.zeros([1, 3]), (v[:-1, :] + v[1:, :])/2])\n    x_b, P_b = Kalman_filter(np.flipud(x_wls), v, phone)\n\n    # Smoothing\n    x_fb = np.zeros_like(x_f)\n    P_fb = np.zeros_like(P_f)\n    for (f, b) in zip(range(n), range(n-1, -1, -1)):\n        P_fi = np.linalg.inv(P_f[f])\n        P_bi = np.linalg.inv(P_b[b])\n\n        P_fb[f] = np.linalg.inv(P_fi + P_bi)\n        x_fb[f] = P_fb[f] @ (P_fi @ x_f[f] + P_bi @ x_b[b])\n\n    return x_fb, x_f, np.flipud(x_b)","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:52:25.258055Z","iopub.execute_input":"2022-06-06T17:52:25.258857Z","iopub.status.idle":"2022-06-06T17:52:25.280033Z","shell.execute_reply.started":"2022-06-06T17:52:25.258800Z","shell.execute_reply":"2022-06-06T17:52:25.278976Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Train data","metadata":{}},{"cell_type":"code","source":"# Target course/phone\npath = '/kaggle/input/smartphone-decimeter-2022/train/2021-03-16-US-MTV-1/GooglePixel4XL'\n\ndrive, phone = path.split('/')[-2:]\n\n# Read data\ngnss_df = pd.read_csv(f'{path}/device_gnss.csv')  # GNSS data\ngt_df = pd.read_csv(f'{path}/ground_truth.csv')  # ground truth\n\n# Point positioning\nutc, x_wls, v_wls = point_positioning(gnss_df)\n\n# Exclude velocity outliers\nx_wls, v_wls = exclude_interpolate_outlier(x_wls, v_wls)\n\n# Kalman smoothing\nx_kf, _, _ = Kalman_smoothing(x_wls, v_wls, phone)\n\n# Convert to latitude and longitude\nllh_wls = np.array(pm.ecef2geodetic(x_wls[:, 0], x_wls[:, 1], x_wls[:, 2])).T\nllh_kf = np.array(pm.ecef2geodetic(x_kf[:, 0], x_kf[:, 1], x_kf[:, 2])).T\n\n# Baseline\nx_bl = gnss_df.groupby('TimeNanos')[\n    ['WlsPositionXEcefMeters', 'WlsPositionYEcefMeters', 'WlsPositionZEcefMeters']].mean().to_numpy()\nllh_bl = np.array(pm.ecef2geodetic(x_bl[:, 0], x_bl[:, 1], x_bl[:, 2])).T\n\n# Ground truth\nllh_gt = gt_df[['LatitudeDegrees', 'LongitudeDegrees']].to_numpy()\n\n# Distance from ground truth\nvd_bl = vincenty_distance(llh_bl, llh_gt)\nvd_wls = vincenty_distance(llh_wls, llh_gt)\nvd_kf = vincenty_distance(llh_kf, llh_gt)\n\n# Score\nscore_bl = calc_score(llh_bl, llh_gt)\nscore_wls = calc_score(llh_wls, llh_gt)\nscore_kf = calc_score(llh_kf[:-1, :], llh_gt[:-1, :])\n\nprint(f'Score Baseline   {score_bl:.4f} [m]')\nprint(f'Score Robust WLS {score_wls:.4f} [m]')\nprint(f'Score KF         {score_kf:.4f} [m]')\n\n# Plot distance error\nplt.figure()\nplt.title('Distance error')\nplt.ylabel('Distance error [m]')\nplt.plot(vd_bl, label=f'Baseline, Score: {score_bl:.4f} m')\nplt.plot(vd_wls, label=f'Robust WLS, Score: {score_wls:.4f} m')\nplt.plot(vd_kf, label=f'Robust WLS + KF, Score: {score_kf:.4f} m')\nplt.legend()\nplt.grid()\nplt.ylim([0, 30])\n\n# Compute velocity error\nspeed_wls = np.linalg.norm(v_wls[:, :3], axis=1)\nspeed_gt = gt_df['SpeedMps'].to_numpy()\nspeed_rmse = np.sqrt(np.sum((speed_wls-speed_gt)**2)/len(speed_gt))\n\n# Plot velocity error\nplt.figure()\nplt.title('Speed error')\nplt.ylabel('Speed Error [m/s]')\nplt.plot(speed_wls - speed_gt, label=f'Speed RMSE: {speed_rmse:.4f} m')\nplt.legend()\nplt.grid()","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:52:25.281698Z","iopub.execute_input":"2022-06-06T17:52:25.282304Z","iopub.status.idle":"2022-06-06T17:52:59.552974Z","shell.execute_reply.started":"2022-06-06T17:52:25.282261Z","shell.execute_reply":"2022-06-06T17:52:59.552297Z"},"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Test data and Submission\nThis part is based on @saitodevel01 's [code](https://www.kaggle.com/code/saitodevel01/gsdc2-baseline-submission). Thank you!","metadata":{}},{"cell_type":"code","source":"path = '/kaggle/input/smartphone-decimeter-2022'\nsample_df = pd.read_csv(f'{path}/sample_submission.csv')\ntest_dfs = []\n\n# Loop for each trip\nfor i, dirname in enumerate(tqdm(sorted(gl.glob(f'{path}/test/*/*/')))):\n    drive, phone = dirname.split('/')[-3:-1]\n    tripID = f'{drive}/{phone}'\n    print(tripID)\n\n    # Read data\n    gnss_df = pd.read_csv(f'{dirname}/device_gnss.csv')\n\n    # Point positioning\n    utc, x_wls, v_wls = point_positioning(gnss_df)\n\n    # Exclude velocity outliers\n    x_wls, v_wls = exclude_interpolate_outlier(x_wls, v_wls)\n\n    # Kalman smoothing\n    x_kf, _, _ = Kalman_smoothing(x_wls, v_wls, phone)\n\n    # Convert to latitude and longitude\n    llh_kf = np.array(pm.ecef2geodetic(x_kf[:, 0], x_kf[:, 1], x_kf[:, 2])).T\n\n    # Interpolation for submission\n    UnixTimeMillis = sample_df[sample_df['tripId'] == tripID]['UnixTimeMillis'].to_numpy()\n    lat = InterpolatedUnivariateSpline(utc, llh_kf[:,0], ext=3)(UnixTimeMillis)\n    lng = InterpolatedUnivariateSpline(utc, llh_kf[:,1], ext=3)(UnixTimeMillis)\n    trip_df = pd.DataFrame({\n        'tripId' : tripID,\n        'UnixTimeMillis': UnixTimeMillis,\n        'LatitudeDegrees': lat,\n        'LongitudeDegrees': lng\n        })\n\n    test_dfs.append(trip_df)\n\n# Write submission.csv\ntest_df = pd.concat(test_dfs)\ntest_df.to_csv('submission.csv', index=False)","metadata":{"execution":{"iopub.status.busy":"2022-06-06T17:52:59.554061Z","iopub.execute_input":"2022-06-06T17:52:59.554809Z","iopub.status.idle":"2022-06-06T18:14:04.329111Z","shell.execute_reply.started":"2022-06-06T17:52:59.554770Z","shell.execute_reply":"2022-06-06T18:14:04.327981Z"},"trusted":true},"execution_count":null,"outputs":[]}]}