{"metadata":{"kernelspec":{"name":"python3","display_name":"Python 3","language":"python"},"language_info":{"name":"python","version":"3.11.11","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":31040,"isInternetEnabled":true,"language":"python","sourceType":"notebook","isGpuEnabled":false}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"markdown","source":"# Kalman filter and LSTM correction","metadata":{}},{"cell_type":"markdown","source":"This work was done during a Machine Learning course at Saint Petersburg State University.\nScore achieved:3.882（top 30%）\nTop score:0.883","metadata":{}},{"cell_type":"code","source":"!pip install pymap3d\n!pip install --upgrade \"ipywidgets==7.7.0\"  \nimport matplotlib.pyplot as plt\nimport numpy as np\nimport pandas as pd\nimport pymap3d as pm\nimport pymap3d.vincenty as pmv\nimport os\nimport glob as gl\nimport scipy.optimize\nfrom tqdm.auto import tqdm\nfrom scipy.interpolate import InterpolatedUnivariateSpline, interp1d\nfrom scipy.spatial import distance\nimport torch\nimport torch.nn as nn\nimport torch.optim as optim\nfrom torch.utils.data import DataLoader, TensorDataset\nfrom sklearn.preprocessing import StandardScaler\nimport joblib\nimport time\nfrom numpy.lib.stride_tricks import sliding_window_view\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":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:02:05.290465Z","iopub.execute_input":"2025-05-31T05:02:05.290792Z","iopub.status.idle":"2025-05-31T05:02:12.823902Z","shell.execute_reply.started":"2025-05-31T05:02:05.290746Z","shell.execute_reply":"2025-05-31T05:02:12.822811Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"## Import parameters and libraries","metadata":{}},{"cell_type":"markdown","source":"## Kalman filter method from a public notebook","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]\n\n# 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\n\n# 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\n\n# 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\n\n# 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\n\n# 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()\n\n# Kalman filter\ndef Kalman_filter(zs, us, phone):\n \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:02:12.826274Z","iopub.execute_input":"2025-05-31T05:02:12.826595Z","iopub.status.idle":"2025-05-31T05:02:12.871119Z","shell.execute_reply.started":"2025-05-31T05:02:12.826567Z","shell.execute_reply":"2025-05-31T05:02:12.870249Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# LSTM model definition","metadata":{}},{"cell_type":"code","source":"class PositionLSTM(nn.Module):\n    def __init__(self, input_size=2, hidden_size=16, num_layers=1, output_size=2):\n        super(PositionLSTM, self).__init__()\n        self.hidden_size = hidden_size\n        self.num_layers = num_layers\n        self.lstm = nn.LSTM(input_size, hidden_size, num_layers, batch_first=True)\n        self.fc = nn.Sequential(\n            nn.Linear(hidden_size, 64),\n            nn.ReLU(),\n            nn.Dropout(0.02),\n            nn.Linear(64, output_size)\n        )\n    \n    def forward(self, x):\n        h0 = torch.zeros(self.num_layers, x.size(0), self.hidden_size).to(x.device)\n        c0 = torch.zeros(self.num_layers, x.size(0), self.hidden_size).to(x.device)\n        out, _ = self.lstm(x, (h0, c0))\n        out = out[:, -1, :]\n        out = self.fc(out)\n        return out\n\n# Fast distance calculation function\ndef fast_distance(llh1, llh2):\n    lat1, lon1 = llh1\n    lat2, lon2 = llh2\n    avg_lat = np.radians((lat1 + lat2) / 2)\n    dx = (lon2 - lon1) * 111000 * np.cos(avg_lat)\n    dy = (lat2 - lat1) * 111000\n    return np.sqrt(dx**2 + dy**2)\n\n# Preparing LSTM training data\ndef prepare_data(kf_points, gt_points, seq_length=5):\n    \"\"\"\n   Prepare LSTM training data: use Kalman filter output as input sequence and ground truth as target\n\nArgs:\nkf_points: Kalman filter output latitude and longitude sequence [n, 2]\ngt_points: ground truth latitude and longitude sequence [n, 2]\nseq_length: sequence length\n\nReturns:\nX: input sequence [num_samples, seq_length, 2]\ny: target value [num_samples, 2]\n    \"\"\"\n    n = kf_points.shape[0]\n    X, y = [], []\n    \n# Create a sliding window sequence (each window predicts the correction value of the center point of the window)\n    half_len = seq_length // 2\n    \n    for i in range(half_len, n - half_len):\n        seq = kf_points[i-half_len:i+half_len+1]\n        target = gt_points[i]  # Use the current ground_truth as the target\n        \n        X.append(seq)\n        y.append(target)\n    \n    return np.array(X), np.array(y)\n\n# Optimized LSTM correction function\ndef optimized_lstm_correction(kf_points, model, scaler, seq_length=5, distance_threshold=3.0):\n    \"\"\"\n   Optimized LSTM correction function: Use Kalman filter output as input and apply LSTM correction\n\nArgs:\nkf_points: Latitude and longitude sequence of Kalman filter output [n, 2]\nmodel: trained LSTM model\nscaler: normalizer\nseq_length: sequence length\ndistance_threshold: distance threshold (meters)\n        \n    Returns:\n        corrected: Corrected position sequence [n, 2]\n    \"\"\"\n    device = next(model.parameters()).device\n    model.eval()\n    \n    n = kf_points.shape[0]\n    corrected = np.copy(kf_points)\n    \n    if n <= seq_length:\n        return corrected\n    \n   # Create a sliding window sequence (each window predicts the correction value of the center point of the window)\n    half_len = seq_length // 2\n    sequences = []\n    valid_indices = []\n    \n    for i in range(half_len, n - half_len):\n        seq = kf_points[i-half_len:i+half_len+1]\n        sequences.append(seq)\n        valid_indices.append(i)\n    \n    sequences = np.array(sequences)\n    \n# Standardization\n    sequences_scaled = scaler.transform(sequences.reshape(-1, 2)).reshape(sequences.shape)\n    sequences_tensor = torch.tensor(sequences_scaled, dtype=torch.float32).to(device)\n    \n# Batch prediction\n    batch_size = 16\n    predictions_scaled = []\n    with torch.no_grad():\n        for i in range(0, len(sequences_tensor), batch_size):\n            batch = sequences_tensor[i:i+batch_size]\n            pred_batch = model(batch)\n            predictions_scaled.append(pred_batch.cpu().numpy())\n    \n    predictions_scaled = np.vstack(predictions_scaled)\n    predictions = scaler.inverse_transform(predictions_scaled)\n    \n# Apply conditional filtering and weighted fusion\n    for idx, center_idx in enumerate(valid_indices):\n        kf_point = kf_points[center_idx]\n        lstm_point = predictions[idx]\n        \n# Calculate the distance between the Kalman point and the LSTM prediction point\n        d = fast_distance(kf_point, lstm_point)\n        \n        # Distance threshold screening + weighted fusion\n        if d < distance_threshold:\n            # Adaptive weight: the smaller the distance, the larger the LSTM weight\n            alpha = max(0.5, 1.0 - d/distance_threshold)\n            corrected[center_idx] = alpha * lstm_point + (1-alpha) * kf_point\n    \n    return corrected","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:02:12.872144Z","iopub.execute_input":"2025-05-31T05:02:12.872438Z","iopub.status.idle":"2025-05-31T05:02:12.897335Z","shell.execute_reply.started":"2025-05-31T05:02:12.872411Z","shell.execute_reply":"2025-05-31T05:02:12.896186Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"# Main processing flow","metadata":{}},{"cell_type":"markdown","source":"## 1. Processing training data","metadata":{}},{"cell_type":"code","source":"print(\"=\"*50)\nprint(\"Step 1/4: Processing training data\")\nprint(\"=\"*50)\n\n# Configuration path\ntrain_path =  '/kaggle/input/smartphone-decimeter-2023/sdc2023/train/2023-09-07-22-48-us-ca-routebc2/pixel4xl'\nval_path =  '/kaggle/input/smartphone-decimeter-2023/sdc2023/train/2023-09-07-22-47-us-ca-routebc2/pixel6pro'\nmodel_path = 'gnss_lstm_model.pth'\nscaler_path = 'gnss_scaler.pkl'\n\n# Process training data\n\ntrain_gnss_df = pd.read_csv(os.path.join(train_path, 'device_gnss.csv'), low_memory=False)\n\n#train_gnss_df = pd.read_csv(os.path.join(train_path, 'device_gnss.csv'))\ntrain_gt_df = pd.read_csv(os.path.join(train_path, 'ground_truth.csv'))\ntrain_utc, train_x_wls, train_v_wls = point_positioning(train_gnss_df)\n    \ntrain_x_wls, train_v_wls = exclude_interpolate_outlier(train_x_wls, train_v_wls)\ntrain_x_kf, _, _ = Kalman_smoothing(train_x_wls, train_v_wls, 'pixel4xl')\ntrain_llh_kf = np.array(pm.ecef2geodetic(train_x_kf[:, 0], train_x_kf[:, 1], train_x_kf[:, 2])).T[:, :2]\ntrain_llh_gt = train_gt_df[['LatitudeDegrees', 'LongitudeDegrees']].to_numpy()\n\n# Processing verification data\nval_gnss_df = pd.read_csv(os.path.join(val_path, 'device_gnss.csv'), low_memory=False)\n#val_gnss_df = pd.read_csv(os.path.join(val_path, 'device_gnss.csv'))\nval_gt_df = pd.read_csv(os.path.join(val_path, 'ground_truth.csv'))\nval_utc, val_x_wls, val_v_wls = point_positioning(val_gnss_df)\nval_x_wls, val_v_wls = exclude_interpolate_outlier(val_x_wls, val_v_wls)\nval_x_kf, _, _ = Kalman_smoothing(val_x_wls, val_v_wls, 'pixel6pro')\nval_llh_kf = np.array(pm.ecef2geodetic(val_x_kf[:, 0], val_x_kf[:, 1], val_x_kf[:, 2])).T[:, :2]\nval_llh_gt = val_gt_df[['LatitudeDegrees', 'LongitudeDegrees']].to_numpy()","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:02:12.899674Z","iopub.execute_input":"2025-05-31T05:02:12.900037Z","iopub.status.idle":"2025-05-31T05:03:10.764768Z","shell.execute_reply.started":"2025-05-31T05:02:12.900012Z","shell.execute_reply":"2025-05-31T05:03:10.764028Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"## 2. Training the LSTM model","metadata":{}},{"cell_type":"code","source":"print(\"=\" * 50)\nprint(\"Step 2/4: Train the LSTM model\")\nprint(\"=\" * 50)\n\n# Configuration parameters\nseq_length = 5\nhidden_size = 16\nnum_layers = 2\nnum_epochs = 7\nbatch_size = 16\ninput_size = 2\noutput_size = 2\n\n# Prepare training data - use Kalman filter output as input and ground truth as target\nX_train, y_train = prepare_data(train_llh_kf, train_llh_gt, seq_length)\nX_val, y_val = prepare_data(val_llh_kf, val_llh_gt, seq_length)\n\n# Data Standardization\nscaler = StandardScaler()\nX_train_scaled = scaler.fit_transform(X_train.reshape(-1, input_size)).reshape(X_train.shape)\ny_train_scaled = scaler.transform(y_train)\nX_val_scaled = scaler.transform(X_val.reshape(-1, input_size)).reshape(X_val.shape)\ny_val_scaled = scaler.transform(y_val)\n\n# Save the normalizer\nscaler_path = \"/kaggle/working/normalizer.pkl\"\njoblib.dump(scaler, scaler_path)\nprint(f\"Normalizer saved to: {scaler_path}\")\n\n# Convert to PyTorch tensors\nX_train_tensor = torch.tensor(X_train_scaled, dtype=torch.float32)\ny_train_tensor = torch.tensor(y_train_scaled, dtype=torch.float32)\nX_val_tensor = torch.tensor(X_val_scaled, dtype=torch.float32)\ny_val_tensor = torch.tensor(y_val_scaled, dtype=torch.float32)\n\n# DataLoader\ntrain_dataset = TensorDataset(X_train_tensor, y_train_tensor)\nval_dataset = TensorDataset(X_val_tensor, y_val_tensor)\ntrain_loader = DataLoader(train_dataset, batch_size=batch_size, shuffle=True)\nval_loader = DataLoader(val_dataset, batch_size=batch_size, shuffle=False)\n\n# Device config\ndevice = torch.device('cuda' if torch.cuda.is_available() else 'cpu')\nprint(f\"Device used: {device}\")\n\n# Model definition\nclass PositionLSTM(nn.Module):\n    def __init__(self, input_size, hidden_size, num_layers, output_size):\n        super(PositionLSTM, self).__init__()\n        self.lstm = nn.LSTM(input_size, hidden_size, num_layers, batch_first=True)\n        self.fc = nn.Linear(hidden_size, output_size)\n\n    def forward(self, x):\n        out, _ = self.lstm(x)\n        out = self.fc(out[:, -1, :])\n        return out\n\n# Instantiate model\nmodel = PositionLSTM(\n    input_size=input_size,\n    hidden_size=hidden_size,\n    num_layers=num_layers,\n    output_size=output_size\n).to(device)\n\n# Optimizer and Loss\noptimizer = optim.Adam(model.parameters(), lr=0.001)\ncriterion = nn.MSELoss()\n\n# Training loop\nmodel_path = '/kaggle/working/gnss_lstm_model.pth'\ntrain_losses, val_losses = [], []\nbest_val_loss = float('inf')\n\nfor epoch in range(num_epochs):\n    # Training\n    model.train()\n    epoch_train_loss = 0.0\n    for inputs, labels in train_loader:\n        inputs, labels = inputs.to(device), labels.to(device)\n        optimizer.zero_grad()\n        outputs = model(inputs)\n        loss = criterion(outputs, labels)\n        loss.backward()\n        optimizer.step()\n        epoch_train_loss += loss.item() * inputs.size(0)\n\n    # Validation\n    model.eval()\n    epoch_val_loss = 0.0\n    with torch.no_grad():\n        for inputs, labels in val_loader:\n            inputs, labels = inputs.to(device), labels.to(device)\n            outputs = model(inputs)\n            loss = criterion(outputs, labels)\n            epoch_val_loss += loss.item() * inputs.size(0)\n\n    # Compute average losses\n    train_loss = epoch_train_loss / len(train_loader.dataset)\n    val_loss = epoch_val_loss / len(val_loader.dataset)\n    train_losses.append(train_loss)\n    val_losses.append(val_loss)\n\n    # Save the best model\n    if val_loss < best_val_loss:\n        best_val_loss = val_loss\n        torch.save({\n            'model_state_dict': model.state_dict(),\n            'hidden_size': hidden_size,\n            'num_layers': num_layers\n        }, model_path)\n\n    print(f\"Epoch {epoch+1}/{num_epochs}: Training loss = {train_loss:.6f}, Validation loss = {val_loss:.6f}\")\n\n# View model parameters\nprint(\"\\nModel weight information:\")\nprint(\"=\" * 50)\nfor name, param in model.named_parameters():\n    print(f\"Layer Name: {name}\")\n    print(f\"Shape: {param.shape}\")\n    print(f\"First 5 weight values: {param.data.flatten()[:5].cpu().numpy()}\")\n    print(\"-\" * 50)\n\n# Draw loss curve\nplt.figure(figsize=(10, 6))\nplt.plot(train_losses, label='Training loss')\nplt.plot(val_losses, label='Validation loss')\nplt.title('Training and validation losses')\nplt.xlabel('Epoch')\nplt.ylabel('Loss')\nplt.legend()\nplt.grid(True)\nplt.savefig('/kaggle/working/loss_curve.png')\nplt.show()\n\nprint(f\"Model saved at: {model_path}\")","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:03:10.765844Z","iopub.execute_input":"2025-05-31T05:03:10.766163Z","iopub.status.idle":"2025-05-31T05:03:14.621160Z","shell.execute_reply.started":"2025-05-31T05:03:10.766138Z","shell.execute_reply":"2025-05-31T05:03:14.620357Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"## 3. Evaluate validation set performance","metadata":{}},{"cell_type":"code","source":"\nprint(\"=\"*50)\nprint(\"Step 2/4: Train the LSTM model\")\nprint(\"=\"*50)\n\n# Configuration parameters\nseq_length = 5\nhidden_size = 16\nnum_layers = 2\nnum_epochs = 7\nbatch_size = 16\n\ninput_size = 2\noutput_size = 2\n\n# Prepare training and validation data\nX_train, y_train = prepare_data(train_llh_kf, train_llh_gt, seq_length)\nX_val, y_val = prepare_data(val_llh_kf, val_llh_gt, seq_length)\n\n# Data Standardization\nscaler = StandardScaler()\nX_train_scaled = scaler.fit_transform(X_train.reshape(-1, input_size)).reshape(X_train.shape)\ny_train_scaled = scaler.transform(y_train)\nX_val_scaled = scaler.transform(X_val.reshape(-1, input_size)).reshape(X_val.shape)\ny_val_scaled = scaler.transform(y_val)\n\n# Save the normalizer\nscaler_path = '/kaggle/working/scaler.pkl'\njoblib.dump(scaler, scaler_path)\nprint(f\"Normalizer saved to: {scaler_path}\")\n\n# Convert to PyTorch tensor\nX_train_tensor = torch.tensor(X_train_scaled, dtype=torch.float32)\ny_train_tensor = torch.tensor(y_train_scaled, dtype=torch.float32)\nX_val_tensor = torch.tensor(X_val_scaled, dtype=torch.float32)\ny_val_tensor = torch.tensor(y_val_scaled, dtype=torch.float32)\n\n# Dataset and DataLoader\ntrain_dataset = TensorDataset(X_train_tensor, y_train_tensor)\nval_dataset = TensorDataset(X_val_tensor, y_val_tensor)\n\ntrain_loader = DataLoader(train_dataset, batch_size=batch_size, shuffle=True)\nval_loader = DataLoader(val_dataset, batch_size=batch_size, shuffle=False)\n\n# Device\ndevice = torch.device('cuda' if torch.cuda.is_available() else 'cpu')\nprint(f\"Using device: {device}\")\n\n# Model\nmodel = PositionLSTM(\n    input_size=input_size,\n    hidden_size=hidden_size,\n    num_layers=num_layers,\n    output_size=output_size\n).to(device)\n\n# Loss and optimizer\ncriterion = nn.MSELoss()\noptimizer = optim.Adam(model.parameters(), lr=0.001)\n\n# Training loop\nmodel_path = '/kaggle/working/gnss_lstm_model.pth'\ntrain_losses, val_losses = [], []\nbest_val_loss = float('inf')\n\nfor epoch in range(num_epochs):\n    # Training\n    model.train()\n    epoch_train_loss = 0.0\n    for inputs, labels in train_loader:\n        inputs, labels = inputs.to(device), labels.to(device)\n        optimizer.zero_grad()\n        outputs = model(inputs)\n        loss = criterion(outputs, labels)\n        loss.backward()\n        optimizer.step()\n        epoch_train_loss += loss.item() * inputs.size(0)\n    \n    # Validation\n    model.eval()\n    epoch_val_loss = 0.0\n    with torch.no_grad():\n        for inputs, labels in val_loader:\n            inputs, labels = inputs.to(device), labels.to(device)\n            outputs = model(inputs)\n            loss = criterion(outputs, labels)\n            epoch_val_loss += loss.item() * inputs.size(0)\n    \n    train_loss = epoch_train_loss / len(train_loader.dataset)\n    val_loss = epoch_val_loss / len(val_loader.dataset)\n    train_losses.append(train_loss)\n    val_losses.append(val_loss)\n    \n    # Save best model\n    if val_loss < best_val_loss:\n        best_val_loss = val_loss\n        torch.save({\n            'model_state_dict': model.state_dict(),\n            'hidden_size': hidden_size,\n            'num_layers': num_layers\n        }, model_path)\n    \n    print(f'Epoch {epoch+1}/{num_epochs}: Training loss: {train_loss:.6f}, Validation loss: {val_loss:.6f}')\n\n# Load best model (optional step)\ncheckpoint = torch.load(model_path)\nmodel.load_state_dict(checkpoint['model_state_dict'])\nmodel.eval()\n\n# Print model weights\nprint(\"\\nModel weight information:\")\nprint(\"=\"*50)\nfor name, param in model.named_parameters():\n    print(f\"Layer Name: {name}\")\n    print(f\"Shape: {param.shape}\")\n    print(f\"First 5 values: {param.data.flatten()[:5].cpu().numpy()}\")\n    print(\"-\"*50)\n\n# Draw loss curve\nplt.figure(figsize=(10, 6))\nplt.plot(train_losses, label='Training Loss')\nplt.plot(val_losses, label='Validation Loss')\nplt.title('Training and Validation Loss Curves')\nplt.xlabel('Epoch')\nplt.ylabel('Loss')\nplt.legend()\nplt.grid(True)\nplt.savefig('/kaggle/working/loss_curve.png')\nplt.show()","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:03:14.622610Z","iopub.execute_input":"2025-05-31T05:03:14.622972Z","iopub.status.idle":"2025-05-31T05:03:18.678192Z","shell.execute_reply.started":"2025-05-31T05:03:14.622951Z","shell.execute_reply":"2025-05-31T05:03:18.677292Z"}},"outputs":[],"execution_count":null},{"cell_type":"markdown","source":"## 4. Process test data and generate submission files","metadata":{}},{"cell_type":"code","source":"# Configuration\npath = '/kaggle/input/smartphone-decimeter-2023/sdc2023'\nmodel_path = 'gnss_lstm_model.pth'\nscaler_path = 'gnss_scaler.pkl'\nsubmission_path = 'submission.csv'\n\n# Sample submission\nsample_df = pd.read_csv(f'{path}/sample_submission.csv')\n\n# Load model and scaler\ndevice = torch.device('cuda' if torch.cuda.is_available() else 'cpu')\n\n# Hyperparameters (make sure these are defined)\nhidden_size = 16\nnum_layers = 2\n\nmodel = PositionLSTM(\n    input_size=2,\n    hidden_size=hidden_size,\n    num_layers=num_layers,\n    output_size=2\n).to(device)\n\n# Load model weights\ncheckpoint = torch.load(model_path, map_location=device)\nif isinstance(checkpoint, dict) and 'model_state_dict' in checkpoint:\n    state_dict = checkpoint['model_state_dict']\nelse:\n    state_dict = checkpoint\nmodel.load_state_dict(state_dict)\nmodel.eval()\n\nscaler = joblib.load(scaler_path)\n\n# Initialize submission list\ntest_dfs = []\n\n# Loop through test data\nfor dirname in tqdm(sorted(gl.glob(f'{path}/test/*/*/')), desc=\"Processing itinerary\"):\n    drive, phone = dirname.split('/')[-3:-1]\n    tripID = f\"{drive}/{phone}\"\n    print(f\"\\nProcessing itinerary: {tripID}\")\n\n    gnss_df = pd.read_csv(f'{dirname}/device_gnss.csv', low_memory=False)\n\n    # Positioning and preprocessing\n    utc, x_wls, v_wls = point_positioning(gnss_df)\n    x_wls, v_wls = exclude_interpolate_outlier(x_wls, v_wls)\n\n    try:\n        x_kf, _, _ = Kalman_smoothing(x_wls, v_wls, phone)\n    except:\n        print(\"Warning: Kalman smoothing failed, using original positions\")\n        x_kf = x_wls\n\n    # Convert ECEF to Lat-Lon\n    llh_kf = np.array(pm.ecef2geodetic(x_kf[:, 0], x_kf[:, 1], x_kf[:, 2])).T[:, :2]\n\n    # Apply LSTM correction\n    llh_corrected = optimized_lstm_correction(\n        llh_kf, model, scaler, seq_length=5, distance_threshold=3.0\n    )\n\n    # Interpolate positions for submission timestamps\n    trip_timestamps = sample_df[sample_df['tripId'] == tripID]['UnixTimeMillis'].values\n\n    if len(utc) > 1:\n        spline_lat = InterpolatedUnivariateSpline(utc, llh_corrected[:, 0], k=1, ext=3)\n        spline_lng = InterpolatedUnivariateSpline(utc, llh_corrected[:, 1], k=1, ext=3)\n        lat = spline_lat(trip_timestamps)\n        lng = spline_lng(trip_timestamps)\n    else:\n        lat = np.full(len(trip_timestamps), llh_corrected[0, 0])\n        lng = np.full(len(trip_timestamps), llh_corrected[0, 1])\n\n    trip_df = pd.DataFrame({\n        'tripId': tripID,\n        'UnixTimeMillis': trip_timestamps,\n        'LatitudeDegrees': lat,\n        'LongitudeDegrees': lng\n    })\n    test_dfs.append(trip_df)\n\n# Final submission\ntest_df = pd.concat(test_dfs)\ntest_df.to_csv(submission_path, index=False)\n","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-05-31T05:03:18.678978Z","iopub.execute_input":"2025-05-31T05:03:18.679198Z","iopub.status.idle":"2025-05-31T05:03:18.780679Z","shell.execute_reply.started":"2025-05-31T05:03:18.679182Z","shell.execute_reply":"2025-05-31T05:03:18.779547Z"}},"outputs":[],"execution_count":null}]}