{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"name":"python","version":"3.10.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":30635,"isInternetEnabled":true,"language":"python","sourceType":"notebook","isGpuEnabled":false}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"markdown","source":"**the work was carried out as part of the training at the Saint Petersburg State University.**\n\n1. My private score is 3.561. (top-25% in leaderboard).\n2. Top-1 score is 0.883.\n\nThis solution is based on [GSDC23 | Kalman filter | WLS 🌍🛰️](https://www.kaggle.com/code/muhannadmansour/gsdc23-kalman-filter-wls).","metadata":{}},{"cell_type":"code","source":"\nimport sys\nprint(sys.executable)  # 检查当前 Python 解释器路径\nprint(sys.version)     # 检查 Python 版本\nimport sys\nprint(sys.version)\nimport numpy as np\nimport pandas as pd\nimport pymap3d as pm\nimport pymap3d.vincenty as pmv\nimport scipy.optimize\nfrom tqdm.auto import tqdm\nfrom scipy.interpolate import InterpolatedUnivariateSpline\nfrom scipy.spatial import distance\nimport tensorflow as tf\nfrom tensorflow.keras.models import Sequential\nfrom tensorflow.keras.layers import LSTM, Dense\nimport matplotlib.pyplot as plt","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.503831Z","iopub.execute_input":"2025-05-31T05:33:30.504309Z","iopub.status.idle":"2025-05-31T05:33:30.535203Z","shell.execute_reply.started":"2025-05-31T05:33:30.504269Z","shell.execute_reply":"2025-05-31T05:33:30.532951Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"!pip install pymap3d\n!pip install --upgrade \"ipywidgets==7.7.0\"  \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","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.536778Z","iopub.status.idle":"2025-05-31T05:33:30.537336Z","shell.execute_reply.started":"2025-05-31T05:33:30.537066Z","shell.execute_reply":"2025-05-31T05:33:30.537091Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# Constants","metadata":{}},{"cell_type":"code","source":"# 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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.539087Z","iopub.status.idle":"2025-05-31T05:33:30.539664Z","shell.execute_reply.started":"2025-05-31T05:33:30.539367Z","shell.execute_reply":"2025-05-31T05:33:30.539394Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# Satellite Selection","metadata":{}},{"cell_type":"markdown","source":"This function filters out low-quality satellite signals using multiple conditions to improve positioning accuracy:\n\nLow elevation satellites: prone to multipath effects due to ground reflections.\n\nCarrier-to-noise ratio (C/N0): indicates signal strength — lower values mean weaker signals.\n\nMultipath flag: directly removes signals known to be affected by multipath interference.","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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.541305Z","iopub.status.idle":"2025-05-31T05:33:30.541835Z","shell.execute_reply.started":"2025-05-31T05:33:30.541561Z","shell.execute_reply":"2025-05-31T05:33:30.541588Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# Pseudorange/Doppler Residuals and Jacobian\n","metadata":{}},{"cell_type":"markdown","source":"1. Line-of-sight vector function los_vector\nCalculates unit vectors from the user to each satellite and the geometric distance, which are used in pseudorange residual and Jacobian computations.\n\n2. Jacobian matrix function jac_pr_residuals\nComputes the Jacobian matrix, which expresses the partial derivatives of pseudorange residuals with respect to state variables.\n\n3. Pseudorange residual function pr_residuals\nCorrects for the Sagnac effect, which causes signal path deviation due to Earth's rotation, based on coordinate differences between satellites and the user.\n\n4. Pseudorange rate residual function prr_residuals\nThe pseudorange rate reflects the rate of change in distance, accounting for relative velocities and differential rotational effects of the Earth.","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# 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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.543446Z","iopub.status.idle":"2025-05-31T05:33:30.544110Z","shell.execute_reply.started":"2025-05-31T05:33:30.543751Z","shell.execute_reply":"2025-05-31T05:33:30.543888Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# Carrier Smoothing\n","metadata":{}},{"cell_type":"markdown","source":"Carrier Smoothing of Pseudorange: carrier_smoothing\nUses the high resolution of the carrier phase measurements (mm level) to smooth the pseudorange measurements (meter level) and reduce noise.","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.2# carrier phase jump threshold [m] 2->1.5 (best)->1.0\n    pr_th =  15.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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.545430Z","iopub.status.idle":"2025-05-31T05:33:30.545983Z","shell.execute_reply.started":"2025-05-31T05:33:30.545722Z","shell.execute_reply":"2025-05-31T05:33:30.545749Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# score","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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.638107Z","iopub.execute_input":"2025-05-31T05:33:30.638655Z","iopub.status.idle":"2025-05-31T05:33:30.646002Z","shell.execute_reply.started":"2025-05-31T05:33:30.638614Z","shell.execute_reply":"2025-05-31T05:33:30.644760Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"#  Robust WLS","metadata":{}},{"cell_type":"markdown","source":"Main Positioning Workflow: point_positioning\nCore steps:\n\nFor GLONASS satellites (which use FDMA), compute carrier frequency errors to aid satellite filtering.\n\nIndependently estimate position and velocity for each timestamp.\n\nCorrect for satellite clock bias, ionospheric/tropospheric delays using known models.","metadata":{}},{"cell_type":"code","source":"# GNSS single point positioning using pseudorange\n\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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.648736Z","iopub.execute_input":"2025-05-31T05:33:30.649175Z","iopub.status.idle":"2025-05-31T05:33:30.666741Z","shell.execute_reply.started":"2025-05-31T05:33:30.649134Z","shell.execute_reply":"2025-05-31T05:33:30.665307Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# outlier detection","metadata":{}},{"cell_type":"markdown","source":"Outlier Removal and Interpolation: exclude_interpolate_outlier\nOutlier detection: Uses a vertical velocity threshold (v_up_th = 2 m/s) to eliminate unrealistic jumps (e.g., due to sensor noise).\n\nInterpolation and smoothing:\n\nForward/backward fill to patch NaNs at the start and end.\n\nApply cubic spline interpolation to smooth remaining NaNs, improving continuity.","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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.668808Z","iopub.execute_input":"2025-05-31T05:33:30.669842Z","iopub.status.idle":"2025-05-31T05:33:30.684447Z","shell.execute_reply.started":"2025-05-31T05:33:30.669798Z","shell.execute_reply":"2025-05-31T05:33:30.683316Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# Kalman \nApplication of Kalman smoother; integration of velocity and position obtained by WLS. The 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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.686043Z","iopub.execute_input":"2025-05-31T05:33:30.686911Z","iopub.status.idle":"2025-05-31T05:33:30.705515Z","shell.execute_reply.started":"2025-05-31T05:33:30.686869Z","shell.execute_reply":"2025-05-31T05:33:30.704232Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# Train Data","metadata":{}},{"cell_type":"code","source":"# Target course/phone\npath = '/kaggle/input/smartphone-decimeter-2023/sdc2023/train/2023-09-07-22-48-us-ca-routebc2/pixel4xl'\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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:30.707882Z","iopub.execute_input":"2025-05-31T05:33:30.708286Z","iopub.status.idle":"2025-05-31T05:33:31.791047Z","shell.execute_reply.started":"2025-05-31T05:33:30.708253Z","shell.execute_reply":"2025-05-31T05:33:31.789519Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# Test and submission","metadata":{}},{"cell_type":"code","source":"path = '/kaggle/input/smartphone-decimeter-2023/sdc2023'\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', low_memory=False)\n\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   \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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:33:31.791905Z","iopub.status.idle":"2025-05-31T05:33:31.792365Z","shell.execute_reply.started":"2025-05-31T05:33:31.792161Z","shell.execute_reply":"2025-05-31T05:33:31.792182Z"}},"outputs":[],"execution_count":null}]}