{"metadata":{"kernelspec":{"language":"python","display_name":"Python 3","name":"python3"},"language_info":{"name":"python","version":"3.11.13","mimetype":"text/x-python","codemirror_mode":{"name":"ipython","version":3},"pygments_lexer":"ipython3","nbconvert_exporter":"python","file_extension":".py"},"kaggle":{"accelerator":"gpu","dataSources":[{"sourceId":60095,"databundleVersionId":6542333,"sourceType":"competition"},{"sourceId":163697332,"sourceType":"kernelVersion"}],"dockerImageVersionId":31090,"isInternetEnabled":true,"language":"python","sourceType":"notebook","isGpuEnabled":true}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"code","source":"# --- Cell 1 ---\n# Install the Kaggle API client and create a directory for credentials.\n# The \"-q\" flag makes the installation quiet.\n!pip install kaggle -q\n!mkdir -p ~/.kaggle\n\n# Download the competition dataset and unzip it into the specified directory.\n# This assumes you have a `kaggle.json` file with your API credentials\n# placed in `~/.kaggle/` (this is automatically handled on Kaggle notebooks).\n!kaggle competitions download -c smartphone-decimeter-2023 -p /kaggle/working/data\n!unzip -q /kaggle/working/data/*.zip -d /kaggle/working/data\n\n# Import necessary libraries for data manipulation and mathematical operations.\nimport os\nimport numpy as np\nimport pandas as pd","metadata":{"_uuid":"8f2839f25d086af736a60e9eeb907d3b93b6e0e5","_cell_guid":"b1076dfc-b9ad-4769-8c92-a6c4dae69d19","trusted":true,"execution":{"iopub.status.busy":"2025-08-20T15:12:05.519242Z","iopub.execute_input":"2025-08-20T15:12:05.519501Z","iopub.status.idle":"2025-08-20T15:12:10.963478Z","shell.execute_reply.started":"2025-08-20T15:12:05.519471Z","shell.execute_reply":"2025-08-20T15:12:10.962802Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"# --- Cell 2 ---\n# Set the path to a specific trip and phone from the downloaded dataset.\n# You can change these variables to analyze different trips.\ntrip_id = '2020-05-14-US-MTV-1'\nphone = 'SamsungGalaxyS21'  # Example phone\nbase_path = f'/kaggle/working/data/train/{trip_id}/{phone}/'\ngt_path = os.path.join(base_path, 'ground_truth.csv')\nimu_path = os.path.join(base_path, 'device_imu.csv')\ngnss_path = os.path.join(base_path, 'device_gnss.csv')\n\n# Read the CSV data files into pandas DataFrames.\nground_truth = pd.read_csv(gt_path)\nimu_data = pd.read_csv(imu_path)\ngnss_data = pd.read_csv(gnss_path)\n\n# Extract the initial position from the ground truth data.\n# The ground truth file provides high-accuracy latitude, longitude, and altitude.\nlat0 = ground_truth['lat_rx_gt_deg'].iloc[0]\nlon0 = ground_truth['lon_rx_gt_deg'].iloc[0]\nalt0 = ground_truth['alt_rx_gt_m'].iloc[0]\n\n# Define a function to convert geodetic coordinates (lat/lon/alt) to ECEF (x/y/z).\n# This is a standard coordinate transformation using WGS84 ellipsoid constants.\ndef geodetic_to_ecef(lat, lon, alt):\n    # WGS84 ellipsoid constants\n    a = 6378137.0\n    e2 = 6.69437999014e-3\n    lat, lon = np.deg2rad(lat), np.deg2rad(lon)\n    N = a / np.sqrt(1 - e2 * np.sin(lat)**2)\n    x = (N + alt) * np.cos(lat) * np.cos(lon)\n    y = (N + alt) * np.cos(lat) * np.sin(lon)\n    z = (N * (1 - e2) + alt) * np.sin(lat)\n    return x, y, z\n\n# Convert the initial ground truth position to ECEF coordinates.\nx0, y0, z0 = geodetic_to_ecef(lat0, lon0, alt0)","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-08-20T15:12:10.964299Z","iopub.execute_input":"2025-08-20T15:12:10.964653Z","iopub.status.idle":"2025-08-20T15:12:11.065836Z","shell.execute_reply.started":"2025-08-20T15:12:10.964619Z","shell.execute_reply":"2025-08-20T15:12:11.064619Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"# --- Cell 3 ---\n# Separate the IMU data into accelerometer and gyroscope readings based on 'MessageType'.\n# Rename columns for clarity and select only the necessary data.\nimu_acc = imu_data[imu_data['MessageType'] == 'UncalAccel'] \\\n    .rename(columns={'X': 'ax', 'Y': 'ay', 'Z': 'az'})[['TimeNanos', 'ax', 'ay', 'az']]\nimu_gyro = imu_data[imu_data['MessageType'] == 'UncalGyro'] \\\n    .rename(columns={'X': 'gx', 'Y': 'gy', 'Z': 'gz'})[['TimeNanos', 'gx', 'gy', 'gz']]\n\n# Sort the IMU data by timestamp to ensure chronological order.\nimu_acc = imu_acc.sort_values('TimeNanos').reset_index(drop=True)\nimu_gyro = imu_gyro.sort_values('TimeNanos').reset_index(drop=True)\n\n# Prepare the GNSS data, using the weighted-least-squares (WLS) positions as measurements.\ngnss_meas = gnss_data[['WlsPositionXEcefMeters', 'WlsPositionYEcefMeters', 'WlsPositionZEcefMeters', 'ArrivalTimeNanosSinceGpsEpoch']].copy()\ngnss_meas = gnss_meas.rename(columns={\n    'WlsPositionXEcefMeters': 'px',\n    'WlsPositionYEcefMeters': 'py',\n    'WlsPositionZEcefMeters': 'pz',\n    'ArrivalTimeNanosSinceGpsEpoch': 'TimeNanos'\n}).sort_values('TimeNanos').reset_index(drop=True)\n\n# Convert the timestamps from nanoseconds to relative seconds for both GNSS and IMU data.\n# This makes time calculations (e.g., `dt`) easier and more readable.\ngnss_start = gnss_meas['TimeNanos'].iloc[0]\ngnss_meas['t_sec'] = (gnss_meas['TimeNanos'] - gnss_start) * 1e-9\n\nimu_start = imu_acc['TimeNanos'].iloc[0]\nimu_acc['t_sec'] = (imu_acc['TimeNanos'] - imu_start) * 1e-9\nimu_gyro['t_sec'] = (imu_gyro['TimeNanos'] - imu_start) * 1e-9","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-08-20T15:12:11.06639Z","iopub.status.idle":"2025-08-20T15:12:11.066739Z","shell.execute_reply.started":"2025-08-20T15:12:11.066556Z","shell.execute_reply":"2025-08-20T15:12:11.06657Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"# --- Cell 4 ---\n# Initialize the state vector `x` with 12 elements:\n# [position_x, position_y, position_z, velocity_x, velocity_y, velocity_z,\n#  accel_bias_x, accel_bias_y, accel_bias_z, gyro_bias_x, gyro_bias_y, gyro_bias_z]\nx = np.zeros(12)\nx[0:3] = [x0, y0, z0]      # Initial position from ground truth\nx[3:6] = [0.0, 0.0, 0.0]   # Initial velocity is assumed to be zero\nx[6:9] = [0.0, 0.0, 0.0]   # Initial accelerometer biases assumed to be zero\nx[9:12] = [0.0, 0.0, 0.0]  # Initial gyroscope biases assumed to be zero\n\n# Initialize the covariance matrix `P`.\n# A large initial value (e.g., 1e2) indicates high uncertainty in our initial state estimate.\nP = np.eye(12) * 1e2\n\n# Define the process noise parameters. These are tuning variables.\n# They represent the expected noise in the IMU measurements and the random walk of biases.\naccel_noise_sigma = 0.1   # Standard deviation of acceleration noise\ngyro_noise_sigma = 0.01   # Standard deviation of gyroscope noise\nbias_accel_sigma = 0.01   # Standard deviation of accelerometer bias random walk\nbias_gyro_sigma = 0.001   # Standard deviation of gyroscope bias random walk\n\n# Precompute the continuous-time process noise covariance matrix `Q_cont`.\n# This matrix models how uncertainty grows over time from process noise.\nQ_cont = np.diag([0, 0, 0,\n                  accel_noise_sigma**2, accel_noise_sigma**2, accel_noise_sigma**2,\n                  bias_accel_sigma**2, bias_accel_sigma**2, bias_accel_sigma**2,\n                  gyro_noise_sigma**2, gyro_noise_sigma**2, gyro_noise_sigma**2])\n\n# Define the measurement noise covariance matrix `R`.\n# This represents the uncertainty of the GNSS position measurements.\n# A value of 25.0 corresponds to a 5-meter standard deviation.\nR = np.diag([25.0, 25.0, 25.0])","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-08-20T15:12:11.067839Z","iopub.status.idle":"2025-08-20T15:12:11.068079Z","shell.execute_reply.started":"2025-08-20T15:12:11.067968Z","shell.execute_reply":"2025-08-20T15:12:11.067981Z"}},"outputs":[],"execution_count":null},{"cell_type":"code","source":"# --- Cell 5 ---\n# Create an empty list to store the estimated positions at each time step.\nest_positions = []\n\n# Initialize an index to track the next GNSS measurement.\ngnss_idx = 0\n\n# Loop through the IMU accelerometer data.\n# The loop starts from the second data point to allow for time difference calculation.\nfor i in range(1, len(imu_acc)):\n    # --- Time Update (Predict) ---\n    t_prev = imu_acc['t_sec'].iloc[i-1]\n    t_now = imu_acc['t_sec'].iloc[i]\n    dt = t_now - t_prev\n    if dt <= 0:\n        continue # Skip if time hasn't advanced\n\n    # Get the accelerometer reading and correct it using the current bias estimate from the state.\n    ax = imu_acc['ax'].iloc[i] - x[6]\n    ay = imu_acc['ay'].iloc[i] - x[7]\n    az = imu_acc['az'].iloc[i] - x[8]\n    \n    # Predict the new state using a simple kinematic motion model.\n    # p_new = p_old + v*dt + 0.5*a*dt^2\n    # v_new = v_old + a*dt\n    x[0:3] += x[3:6] * dt + 0.5 * np.array([ax, ay, az]) * dt**2\n    x[3:6] += np.array([ax, ay, az]) * dt\n    # Biases are assumed constant during the prediction step.\n\n    # Build the state transition Jacobian matrix `F`.\n    # This matrix describes how the state changes from one time step to the next.\n    F = np.eye(12)\n    F[0:3, 3:6] = np.eye(3) * dt      # Position depends on velocity and dt (∂p/∂v = dt)\n    F[0:3, 6:9] = -0.5 * np.eye(3) * dt**2 # Position depends on accel bias (∂p/∂b_a = -0.5*dt^2)\n    F[3:6, 6:9] = -np.eye(3) * dt     # Velocity depends on accel bias (∂v/∂b_a = -dt)\n    \n    # Approximate the discrete process noise covariance matrix `Qk` by scaling `Q_cont`.\n    Qk = Q_cont * dt\n\n    # Predict the new covariance using the standard Kalman filter formula.\n    P = F @ P @ F.T + Qk\n\n    # --- Measurement Update (Correct) ---\n    # Check if a new GNSS measurement is available.\n    if gnss_idx < len(gnss_meas) and t_now >= gnss_meas['t_sec'].iloc[gnss_idx]:\n        # Get the new GNSS position measurement `z`.\n        z = gnss_meas[['px', 'py', 'pz']].iloc[gnss_idx].values\n        \n        # Define the measurement Jacobian matrix `H`.\n        # Since we are measuring position directly, H is an identity matrix for the position part of the state.\n        H = np.zeros((3, 12))\n        H[0, 0] = H[1, 1] = H[2, 2] = 1.0\n\n        # Calculate the innovation (the difference between the measurement and our prediction).\n        y = z - x[0:3]\n        \n        # Calculate the innovation covariance `S`.\n        S = H @ P @ H.T + R\n\n        # Calculate the Kalman Gain `K`.\n        K = P @ H.T @ np.linalg.inv(S)\n\n        # Update the state estimate using the Kalman Gain.\n        x = x + K @ y\n\n        # Update the covariance matrix using the Joseph form for numerical stability.\n        P = (np.eye(12) - K @ H) @ P @ (np.eye(12) - K @ H).T + K @ R @ K.T\n        \n        # Move to the next GNSS measurement.\n        gnss_idx += 1\n\n    # Save the estimated position at the current time step.\n    est_positions.append((t_now, x[0], x[1], x[2]))\n\n# Convert the list of positions into a pandas DataFrame for easy analysis and visualization.\nest_df = pd.DataFrame(est_positions, columns=['t_sec', 'est_x', 'est_y', 'est_z'])\nprint(est_df.head())","metadata":{"trusted":true,"execution":{"iopub.status.busy":"2025-08-20T15:12:11.069022Z","iopub.status.idle":"2025-08-20T15:12:11.069259Z","shell.execute_reply.started":"2025-08-20T15:12:11.06914Z","shell.execute_reply":"2025-08-20T15:12:11.069154Z"}},"outputs":[],"execution_count":null}]}