{"metadata":{"kernelspec":{"name":"python3","display_name":"Python 3","language":"python"},"language_info":{"name":"python","version":"3.10.13","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":30698,"isInternetEnabled":true,"language":"python","sourceType":"notebook","isGpuEnabled":false}},"nbformat_minor":4,"nbformat":4,"cells":[{"cell_type":"markdown","source":"This notebook investigates IMU data . Initially inspired by an [old notebook for the same 2021 challenge](https://www.kaggle.com/code/museas/estimating-the-direction-with-a-magnetic-sensor)","metadata":{}},{"cell_type":"code","source":"#needed libraries\nimport os\nimport numpy as np\nimport pandas as pd\nfrom math import * \nfrom pathlib import Path\nfrom matplotlib import pyplot as plt\nimport warnings\nwarnings.simplefilter('ignore')","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"\ndef find_csv_paths(root_dir=\"/kaggle/input/smartphone-decimeter-2023/sdc2023/train\",subfolder=\"pixel4\",file_to_find=\"device_imu.csv\"):\n   \"\"\"Finds full paths of files named 'device_imu.csv' within a subdirectory named 'subfolder'.\n\n   Args:\n       root_dir (str, optional): The root directory to start searching from. Defaults to \".\" (current directory).\n       subfolder (str, optional):subfolder name . Defaults to \"pixel4\"\n       file_to_find (str, optional):\n   Returns:\n       list: A list of full paths to the found files.\n   \"\"\"\n\n   csv_paths = []\n   for root, _, files in os.walk(root_dir):\n       if subfolder in root:\n           \n           for file in files:\n               if file == file_to_find:\n                   csv_paths.append(os.path.join(root, file))\n   return csv_paths\n\n# Example usage:\ncsv_files = find_csv_paths('../data/train/','pixel7pro')\nfor file in csv_files:\n    print(file)\n ","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"# lowpass filter to be used with the readings\n\nfrom scipy.signal import butter, lfilter\n\ndef butter_lowpass(cutoff, fs, order=5):\n    nyq = 0.5 * fs\n    normal_cutoff = cutoff / nyq\n    b, a = butter(order, normal_cutoff, btype='low', analog=False)\n    return b, a\n\ndef butter_lowpass_filter(data, cutoff, fs, order=5):\n    b, a = butter_lowpass(cutoff, fs, order=order)\n    y = lfilter(b, a, data)\n    return y\n\norder = 5\nfs = 200.0\ncutoff = 0.7","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"\n\n# A sample trip\nThe data/train/2021-12-07-19-22-us-ca-lax-d trip is choosen to explore IMU readings and do some processing. The trip has data from samsung,pixel,mi8 phones and their IMU readings can be compared. EAch phone IMU readings are read and plot under each other phone. mean and sd are shown in the plots titles","metadata":{}},{"cell_type":"code","source":"\n\nlogg_files = find_csv_paths('/kaggle/input/smartphone-decimeter-2023/sdc2023/train/','2021-12-07-19-22-us-ca-lax-d')\nlogg_paths = [str(p) for p in logg_files]\n# draw 3 subplots for each phone\nfig,ax = plt.subplots(len(logg_paths)*3, 1, figsize=(25,6*3*len(logg_paths)))\n\nno_phones=len(logg_paths)\n\nfor n, logg_path in enumerate(logg_paths):\n                      \n\n    imu_path = logg_path\n    gt_path = os.path.split(imu_path)[0].replace('supplemental','')+\"/ground_truth.csv\"\n    gt_df = pd.read_csv(gt_path)\n    imu_logs_df = pd.read_csv(imu_path)\n    mag_df = imu_logs_df.loc[imu_logs_df['MessageType']=='UncalMag']\n    acce_df = imu_logs_df.loc[imu_logs_df['MessageType']=='UncalAccel']\n    gyro_df = imu_logs_df.loc[imu_logs_df['MessageType']=='UncalGyro']\n    gt_df[\"utcTimeMillis\"]=gt_df[\"UnixTimeMillis\"]\n\n\n\n   \n    #draw raw acceleromoter readings of each phone first at sub plots 0,1,2..no_phones-1\n    #calculate mean and sd of each reading and display it in the title for comparison\n    ax[n].plot(acce_df[\"utcTimeMillis\"],acce_df[\"MeasurementX\"] ,label=\"MeasurementX\")\n    ax[n].plot(acce_df[\"utcTimeMillis\"],acce_df[\"MeasurementY\"], label=\"MeasurementY\")\n    ax[n].plot(acce_df[\"utcTimeMillis\"],acce_df[\"MeasurementZ\"], label=\"MeasurementZ\")\n    ax[n].legend()\n    title = logg_path + ' accel mean and std:x ' + str(round(acce_df['MeasurementX'].mean(), 2)) + ', ' + str(round(acce_df['MeasurementX'].std(), 2))\n    title = title + ' y ' + str(round(acce_df['MeasurementY'].mean(), 2)) + ', ' + str(round(acce_df['MeasurementY'].std(), 2))\n    title = title + ' z ' + str(round(acce_df['MeasurementZ'].mean(), 2)) + ', ' + str(round(acce_df['MeasurementZ'].std(), 2))\n    ax[n].set_title(title)\n \n    #draw raw gyrometer readings of each phone at sub plots n,n+1..2*no_phones-1\n    ax[n+no_phones].plot(gyro_df[\"utcTimeMillis\"],gyro_df[\"MeasurementX\"] ,label=\"MeasurementX\")\n    ax[n+no_phones].plot(gyro_df[\"utcTimeMillis\"],gyro_df[\"MeasurementY\"], label=\"MeasurementY\")\n    ax[n+no_phones].plot(gyro_df[\"utcTimeMillis\"],gyro_df[\"MeasurementZ\"], label=\"MeasurementZ\")\n    ax[n+no_phones].legend()\n    title = logg_path + ' gyro mean and std:x ' + str(round(gyro_df['MeasurementX'].mean(), 2)) + ', ' + str(round(gyro_df['MeasurementX'].std(), 2))\n    title = title + ' y ' + str(round(gyro_df['MeasurementY'].mean(), 2)) + ', ' + str(round(gyro_df['MeasurementY'].std(), 2))\n    title = title + ' z ' + str(round(gyro_df['MeasurementZ'].mean(), 2)) + ', ' + str(round(gyro_df['MeasurementZ'].std(), 2))\n    ax[n+no_phones].set_title(title)\n    \n\n    #draw raw mag readings of each phone at the last sub plots \n    ax[n+2*no_phones].plot(mag_df[\"utcTimeMillis\"],mag_df[\"MeasurementX\"] ,label=\"MeasurementX\")\n    ax[n+2*no_phones].plot(mag_df[\"utcTimeMillis\"],mag_df[\"MeasurementY\"], label=\"MeasurementY\")\n    ax[n+2*no_phones].plot(mag_df[\"utcTimeMillis\"],mag_df[\"MeasurementZ\"], label=\"MeasurementZ\")\n    ax[n+2*no_phones].legend()\n    title = logg_path + ' mag mean and std:x ' + str(round(mag_df['MeasurementX'].mean(), 2)) + ', ' + str(round(mag_df['MeasurementX'].std(), 2))\n    title = title + ' y ' + str(round(mag_df['MeasurementY'].mean(), 2)) + ', ' + str(round(mag_df['MeasurementY'].std(), 2))\n    title = title + ' z ' + str(round(mag_df['MeasurementZ'].mean(), 2)) + ', ' + str(round(mag_df['MeasurementZ'].std(), 2))\n    ax[n+2*no_phones].set_title(title)\n    \n  \n\n\n\n","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Basic signal processing\n1. **Time syncronization**\nIn some phones, gyro and accelerometer readings are not provided in a consistant time difference.In addition, it is better to sync both to report readings at the same timestamps. I upsampled both to be at 200HZ","metadata":{}},{"cell_type":"code","source":"x_axis='utcTimeMillis'\n\nacce_df_essential=acce_df[['MeasurementX','MeasurementY','MeasurementZ',x_axis]]\nacce_df_essential.rename(columns={\"MeasurementX\": \"ax\", \"MeasurementY\": \"ay\",\"MeasurementZ\": \"az\"},inplace=True)\nacce_df_essential[x_axis] = pd.to_datetime(acce_df[x_axis], unit='ms')\n\nacce_df_res=acce_df_essential.resample('5ms',on=x_axis).mean()\nacce_df_res.interpolate(inplace=True)\nacce_df_res['utcTimeResampled'] = acce_df_res.index.view('int64') // 10**6\n\ngyro_df_essential=gyro_df[['MeasurementX','MeasurementY','MeasurementZ',x_axis]]\ngyro_df_essential.rename(columns={\"MeasurementX\": \"gx\", \"MeasurementY\": \"gy\",\"MeasurementZ\": \"gz\"},inplace=True)\ngyro_df_essential[x_axis] = pd.to_datetime(gyro_df[x_axis], unit='ms')\n\ngyro_df_res=gyro_df_essential.resample('5ms',on=x_axis).mean()\ngyro_df_res.interpolate(inplace=True)\ngyro_df_res['utcTimeResampled'] = gyro_df_res.index.view('int64') // 10**6\n\n\n\n\ncombined_df = pd.merge_asof(acce_df_res, gyro_df_res, on='utcTimeResampled')\ncombined_df.sort_values(by='utcTimeResampled',inplace=True)\n# fill NaN due to difference in start of logging between gyrp and accele\n#         these are tens of milliseconds\ncombined_df.fillna(method='bfill',inplace=True)\ncombined_df.set_index('utcTimeResampled',inplace=True)\ncombined_df.head(10) \n","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"**2. Apply low pass filter** \nGyro and Accelerometer readings are very noisy specially when car runs at high speed. Will apply lpf and compare readings with gt","metadata":{}},{"cell_type":"code","source":"#apply low pass filter with order 5, fs=200 (1/5ms) and cutoff freq = 70 Hz (by trial and  error for now)\norder=5\nfs=200\ncutoff=0.7\n\ncombined_df[\"ax_lpf\"] = butter_lowpass_filter(combined_df[\"ax\"], cutoff, fs, order)\ncombined_df[\"ay_lpf\"] = butter_lowpass_filter(combined_df[\"ay\"], cutoff, fs, order)\ncombined_df[\"az_lpf\"] = butter_lowpass_filter(combined_df[\"az\"], cutoff, fs, order)\n    \ncombined_df[\"gx_lpf\"] = butter_lowpass_filter(combined_df[\"gx\"], cutoff, fs, order)\ncombined_df[\"gy_lpf\"] = butter_lowpass_filter(combined_df[\"gy\"], cutoff, fs, order)\ncombined_df[\"gz_lpf\"] = butter_lowpass_filter(combined_df[\"gz\"], cutoff, fs, order)","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"#A-mitigate delay effect of lpf on accelerometer  readings by setting first 400 values (2 secends) to \n    #a fixed value, usually the first few seconds the vehicle is not moving but this neds to be verified\n\nstart_ms = combined_df.index[0]  # Get the first timestamp\nend_ms = start_ms + 499*5  # Add 499*5 ms to get the 399th timestamp\n\nfor col in ['ax_lpf','ay_lpf','az_lpf']:    \n    starting_value=combined_df.loc[end_ms+5,col] # get the value of the 400th timestamp\n    combined_df.loc[start_ms:end_ms, col] = starting_value\n    \nfor col in ['ax_lpf','az_lpf']: \n    combined_df[col]-=starting_value #use it as a bias and subtract it from all readings except y axis as it measures gravity\n\n\n","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"","metadata":{}},{"cell_type":"markdown","source":"**Comparing IMU readings with ground truth.**\n\nThe ground truth has bearing angles and speed in m/s which I will use to to compare gt with IMU readings in specific axes.\nrecorded speed is of course relative to the car and hence change (acceleration) should be mainly recorded in the phone's -z axis of the accelerometer as per Android convension.\nSimilarly, recorded change of bearing angle should be indicated by the gyro reading in the y axis (rotation around y axis)\n\n","metadata":{}},{"cell_type":"code","source":"#calculate ground truth acceleration and rotation velocity to compare it with the accelerometer and gyro readings respectively\n\ngt_df['gt_acc'] = (gt_df['SpeedMps'] - gt_df['SpeedMps'].shift(1)) #/ (df['timeelapsed'] - df['timeelapsed'].shift(1))\n\ndef limit_rad(row):\n    rad = row['rad']\n    if rad < -np.pi:\n        rad += 2*np.pi\n    elif rad > np.pi:\n        rad -= 2*np.pi\n    \n    return rad\n\ndef limit_omega(row):\n    rad = row['gt_rot_vel']\n    if rad < -np.pi:\n        rad += 2*np.pi\n    elif rad > np.pi:\n        rad -= 2*np.pi\n    \n    return rad\n\ngt_df['rad']=gt_df['BearingDegrees']*np.pi/180 # convert to rad/s\ngt_df['rad']=gt_df.apply(limit_rad, axis=1)\n\ngt_df['gt_rot_vel'] = (gt_df['rad'] - gt_df['rad'].shift(1)) \ngt_df['gt_rot_vel'] =gt_df.apply(limit_omega, axis=1)\n","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"First plot readings before low pass filter and notice effect of speed on readings noise","metadata":{}},{"cell_type":"code","source":"# plot resampled gyro and accelerometer readings before low pss filter\ngyro_df_res.plot(x='utcTimeResampled',figsize=(25,6))\nacce_df_res.plot(x='utcTimeResampled',figsize=(25,6))\nplt.figure(3)\nplt.figure(figsize=(25,6))\nplt.plot(gt_df[\"UnixTimeMillis\"],gt_df['SpeedMps'],label=\"Speed\",color='blueviolet')\nplt.legend('gt_speed')\n","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# Comparing gyro readings in y axis of the mobile to the gt change in bearing angles\n\n**Both show very close sd and mean in addition to shape. Please note that gt readings are 1Hz while IMU were processed to be at 200Hz**","metadata":{}},{"cell_type":"markdown","source":"","metadata":{}},{"cell_type":"code","source":"#comparing gyro  in y axis of the mobile to the gt change in bearing angles\ngyro_yaxis=-1*combined_df['gy_lpf']\n\ngyro_yaxis.plot(y='gy_lpf',figsize=(25,6))\nplt.figure(2,figsize=(25,6))\nplt.plot(gt_df[\"UnixTimeMillis\"],gt_df['gt_rot_vel'],label=\"gt_rot_vel\",color='red')\n","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"code","source":"#print mean and sd of each \n\nprint(gyro_yaxis.describe() )\nprint(gt_df['gt_rot_vel'].describe())","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# **Comparing accel. readins  in z axis of the mobile to the gt change in speed**\n\nAgain, both show very close sd and mean in addition to shape. Please note that gt readings are 1Hz while IMU were processed to be at 200Hz","metadata":{}},{"cell_type":"code","source":"#comparing acceleration in -z axis of the mobile to the gt acceleration\naccel_zaxis=-1*combined_df['az_lpf']\n#combined_df.plot(y='az_lpf',figsize=(25,6))\naccel_zaxis.plot(y='az_lpf',figsize=(25,6))\n#combined_df.plot(y='MeasurementZ_x',figsize=(25,6))\nplt.plot(gt_df[\"UnixTimeMillis\"],gt_df['gt_acc'],label=\"ab\",color='red')\nplt.legend('gt_accel')\n","metadata":{"trusted":true},"execution_count":null,"outputs":[]},{"cell_type":"markdown","source":"# **Where to go from here and request to collaborate**","metadata":{}},{"cell_type":"markdown","source":"**Kalman filter**\nMy current score is solely dependant on the great rtk library .\nI think it worth investigating using IMU readings in a Kalman filter. I have many ideas on how this should be implemented. we can add bias and scale factor for IMU in the state matrix. we can use only certain axis in the IMU readings, ready made unscented and extended kalman filter can be used. Please search for the kalman filter, you will find many resources.\nStill reading how to evaluate acuracy of each calculated gps coordinate. this can be helpful in the kalman filter or any interpolation technique.","metadata":{}},{"cell_type":"markdown","source":"Notes: \n\n1-This is a version I made keeping in mind that we are short in time. I will try to ellaborate more on many areas.\n\n2-Part of the code provided were created after request from and discussion with Gemini and chatgpt.\n\n3- if you want to collaborate or form a team, please contact at ahmedemad04 @ hotmail.com.","metadata":{}},{"cell_type":"code","source":"","metadata":{"trusted":true},"execution_count":null,"outputs":[]}]}