{"cells":[{"metadata":{},"cell_type":"markdown","source":"# Intro to Kalman Filters and IMU\n\nThis Notebook contains implementation of Extended Kalman Filter to generate Quaternions from the multi-sensor data of Accelerometer, Gyroscope and Magentometer.\n\nThings done in the Notebook so far:\n* Clean Data to handle the newline skip condition\n* Visualized 3D accelerometer and Magnetometer Data Temporaly\n* Double Integrate Accelerometer Data to produce (BAD) Position Result\n* Generate Quaternions from sensor data using [AHRS library][1]\n* Convert the Quaternions to Rotation Matrix\n* Get Clean Linear Acceleration from MATMUL on the Transposed Rotation Matrix and Orignal Acceleration\n* Double Integrate the Clean Linear Accelerometer Data to produce (Better) Position Result\n\n\n[1]:https://pypi.org/project/AHRS/"},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"import numpy as np # linear algebra\nimport pandas as pd # data processing, CSV file I/O (e.g. pd.read_csv)\n\nfrom dataclasses import dataclass\n\nimport matplotlib.pyplot as plt # visualization\nplt.rcParams.update({'font.size': 14})\nimport seaborn as sns # visualization\n\nimport warnings # Supress warnings \nwarnings.filterwarnings('ignore')","execution_count":null,"outputs":[]},{"metadata":{"_kg_hide-input":true,"_kg_hide-output":true,"trusted":true},"cell_type":"code","source":"!pip install AHRS","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"from tqdm import tqdm\nfrom pathlib import Path\nimport json\nimport plotly.graph_objs as go\nfrom PIL import Image\nfrom datetime import datetime\n\nfrom ahrs.filters import EKF\nfrom ahrs.common.orientation import acc2q\nfrom ahrs.utils import WMM\nfrom scipy.spatial.transform import Rotation as R\nfrom scipy import integrate","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Taking a Look at the text files"},{"metadata":{"trusted":true},"cell_type":"code","source":"!head -n 30 \"../input/indoor-location-navigation/train/5cd56c0ce2acfd2d33b6ab27/F2/5d09b234f03da60008286c44.txt\"","execution_count":null,"outputs":[]},{"metadata":{"trusted":true,"_kg_hide-input":true,"_kg_hide-output":true},"cell_type":"code","source":"train_folders = list(Path(\"../input/indoor-location-navigation/train/\").glob(\"*\"))\nlist(train_folders[0].glob(\"*\"))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"# copy from https://github.com/location-competition/indoor-location-competition-20/blob/master/io_f.py\n\n@dataclass\nclass ReadData:\n    acce: np.ndarray\n    acce_uncali: np.ndarray\n    gyro: np.ndarray\n    gyro_uncali: np.ndarray\n    magn: np.ndarray\n    magn_uncali: np.ndarray\n    ahrs: np.ndarray\n    wifi: np.ndarray\n    ibeacon: np.ndarray\n    waypoint: np.ndarray\n\ndef append_util(data_type, data_dict, line_data):\n    if data_type == \"TYPE_WIFI\":\n        sys_ts = data_type\n        ssid = line_data[1]\n        bssid = line_data[2]\n        rssi = line_data[3]\n        lastseen_ts = line_data[5]\n        data_dict[data_type].append([sys_ts, ssid, bssid, rssi, lastseen_ts])\n    elif data_type == \"TYPE_BEACON\":\n        ts = data_type\n        uuid = line_data[1]\n        major = line_data[2]\n        minor = line_data[3]\n        rssi = line_data[5]\n        data_dict[data_type].append([ts, '_'.join([uuid, major, minor]), rssi])\n    else:\n        data_dict[data_type].append([int(line_data[0]), *[float(item) for item in line_data[1:]]])\n        \ndef read_data_file(data_filename):\n    acce = []\n    acce_uncali = []\n    gyro = []\n    gyro_uncali = []\n    magn = []\n    magn_uncali = []\n    ahrs = []\n    wifi = []\n    ibeacon = []\n    waypoint = []\n    \n    data_dict = {\n        \"TYPE_WAYPOINT\": waypoint,\n        \"TYPE_ACCELEROMETER\": acce,\n        \"TYPE_ACCELEROMETER_UNCALIBRATED\": acce_uncali,\n        \"TYPE_GYROSCOPE\": gyro,\n        \"TYPE_GYROSCOPE_UNCALIBRATED\": gyro_uncali,\n        \"TYPE_MAGNETIC_FIELD\": magn,\n        \"TYPE_MAGNETIC_FIELD_UNCALIBRATED\": magn_uncali,\n        \"TYPE_ROTATION_VECTOR\": ahrs,\n        \"TYPE_WIFI\": wifi,\n        \"TYPE_BEACON\":ibeacon\n    }\n\n    with open(data_filename, 'r', encoding='utf-8') as file:\n        lines = file.readlines()\n\n    for line_data in lines:\n        line_data = line_data.strip()\n        if not line_data or line_data[0] == '#':\n            continue\n            \n        if line_data.count(\"TYPE_\") == 1:\n            line_data = line_data.split('\\t')\n            append_util(line_data.pop(1), data_dict, line_data)\n            \n        elif line_data.count(\"TYPE_\") == 2:\n            line_data_split = line_data.split('\\t')\n            type_index = []\n            total = 0\n            for item in line_data_split:\n                if \"TYPE_\" in item:\n                    type_index.append(total)\n                total+=len(item)+1\n                \n            line_data_split = line_data[:type_index[1]-14].split('\\t')\n            append_util(line_data_split.pop(1), data_dict, line_data_split)\n\n            line_data_split = line_data[type_index[1]-14:].split('\\t')\n            append_util(line_data_split.pop(1), data_dict, line_data_split)\n    \n    acce = np.array(acce)\n    acce_uncali = np.array(acce_uncali)\n    gyro = np.array(gyro)\n    gyro_uncali = np.array(gyro_uncali)\n    magn = np.array(magn)\n    magn_uncali = np.array(magn_uncali)\n    ahrs = np.array(ahrs)\n    wifi = np.array(wifi)\n    ibeacon = np.array(ibeacon)\n    waypoint = np.array(waypoint)\n    \n    return ReadData(acce, acce_uncali, gyro, gyro_uncali, magn, magn_uncali, ahrs, wifi, ibeacon, waypoint)","execution_count":null,"outputs":[]},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"def visualize_trajectory(trajectory, floor_plan_filename, width_meter, height_meter, title=None, mode='lines + markers + text', show=False):\n    \"\"\"\n    Copied from from https://github.com/location-competition/indoor-location-competition-20/blob/master/visualize_f.py\n\n    \"\"\"\n    fig = go.Figure()\n\n    # add trajectory\n    size_list = [6] * trajectory.shape[0]\n    size_list[0] = 10\n    size_list[-1] = 10\n\n    color_list = ['rgba(4, 174, 4, 0.5)'] * trajectory.shape[0]\n    color_list[0] = 'rgba(12, 5, 235, 1)'\n    color_list[-1] = 'rgba(235, 5, 5, 1)'\n\n    position_count = {}\n    text_list = []\n    for i in range(trajectory.shape[0]):\n        if str(trajectory[i]) in position_count:\n            position_count[str(trajectory[i])] += 1\n        else:\n            position_count[str(trajectory[i])] = 0\n        text_list.append('        ' * position_count[str(trajectory[i])] + f'{i}')\n    text_list[0] = 'Start 0'\n    text_list[-1] = f'End {trajectory.shape[0] - 1}'\n\n    fig.add_trace(\n        go.Scattergl(\n            x=trajectory[:, 0],\n            y=trajectory[:, 1],\n            mode=mode,\n            marker=dict(size=size_list, color=color_list),\n            line=dict(shape='linear', color='lightgrey', width=3, dash='dash'),\n            text=text_list,\n            textposition=\"top center\",\n            name='trajectory',\n        ))\n\n    # add floor plan\n    floor_plan = Image.open(floor_plan_filename)\n    fig.update_layout(images=[\n        go.layout.Image(\n            source=floor_plan,\n            xref=\"x\",\n            yref=\"y\",\n            x=0,\n            y=height_meter,\n            sizex=width_meter,\n            sizey=height_meter,\n            sizing=\"contain\",\n            opacity=1,\n            layer=\"below\",\n        )\n    ])\n\n    # configure\n    fig.update_xaxes(autorange=False, range=[0, width_meter])\n    fig.update_yaxes(autorange=False, range=[0, height_meter], scaleanchor=\"x\", scaleratio=1)\n    fig.update_layout(\n        title=go.layout.Title(\n            text=title or \"No title.\",\n            xref=\"paper\",\n            x=0,\n        ),\n        autosize=True,\n        width=800,\n        height=  800 * height_meter / width_meter,\n        template=\"plotly_white\",\n    )\n\n    if show:\n        fig.show()\n\n    return fig\n\ndef visualize_train_trajectory(txt_path):\n    \"\"\"\n    Edited from \n    https://www.kaggle.com/hrshtt/intro-to-indoor-location-navigation/\n    who Edited from \n    https://www.kaggle.com/ihelon/indoor-location-exploratory-data-analysis\n    \"\"\"\n#     _id, floor = path.split(\"/\")[:2]\n    if not isinstance(txt_path, Path):\n        path = Path(txt_path)\n    \n    _id, floor, filename = txt_path.parts[-3:]\n\n    \n    train_floor_data = read_data_file(txt_path)\n    with open(f\"../input/indoor-location-navigation/metadata/{_id}/{floor}/floor_info.json\") as f:\n        train_floor_info = json.load(f)\n\n    return visualize_trajectory(\n        train_floor_data.waypoint[:, 1:3], \n        f\"../input/indoor-location-navigation/metadata/{_id}/{floor}/floor_image.png\",\n        train_floor_info[\"map_info\"][\"width\"], \n        train_floor_info[\"map_info\"][\"height\"],\n        f\"Visualization of {_id}/{floor}/{filename}\"\n    )\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"sample_txt_file_path = list(list(train_folders[0].glob(\"*\"))[2].glob(\"*\"))[5]\n# sample_txt_file_path = Path(\"../input/indoor-location-navigation/train/5a0546857ecc773753327266/F2/5dccf516c04f060006e6e3c9.txt\")\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"sample_data = read_data_file(sample_txt_file_path)\nprint('acce shape:', sample_data.acce.shape)\nprint('acce_uncali shape:', sample_data.acce_uncali.shape)\nprint('gyro shape:', sample_data.gyro.shape)\nprint('gyro_uncali shape:', sample_data.gyro_uncali.shape)\nprint('magn shape:', sample_data.magn.shape)\nprint('magn_uncali shape:',sample_data.magn_uncali.shape)\nprint('ahrs shape:', sample_data.ahrs.shape)\nprint('wifi shape:', sample_data.wifi.shape)\nprint('ibeacon shape:', sample_data.ibeacon.shape)\nprint('waypoint shape:', sample_data.waypoint.shape)\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"# temp = np.concatenate([sample_data.acce, \n#                        sample_data.acce_uncali[:, 1:],\n#                        sample_data.gyro[:, 1:],\n#                        sample_data.gyro_uncali[:, 1:],\n#                        sample_data.magn[:, 1:],\n#                        sample_data.magn_uncali[:, 1:],\n#                        sample_data.ahrs[:, 1:],\n#                       ], axis=1)\n\n# imu_df = pd.DataFrame(temp)\n\n# del temp\n\n# imu_df.columns = ['timestamp', 'acce_x','acce_y', 'acce_z','acce_uncali_x','acce_uncali_y', 'acce_uncali_z',\n#               'gyro_x','gyro_y', 'gyro_z','gyro_uncali_x','gyro_uncali_y', 'gyro_uncali_z',\n#               'magn_x','magn_y', 'magn_z','magn_uncali_x','magn_uncali_y', 'magn_uncali_z',\n#               'ahrs_x','ahrs_y', 'ahrs_z']\n\n# display(imu_df.head(8).style.set_caption('IMU Data'))","execution_count":null,"outputs":[]},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"waypoint_df = pd.DataFrame(sample_data.waypoint)\nwaypoint_df.columns = ['timestamp', 'waypoint_x','waypoint_y']\ndisplay(waypoint_df.style.set_caption('Waypoint'))","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Trajectory of the Waypoints"},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"visualize_train_trajectory(sample_txt_file_path)\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true,"_kg_hide-input":true,"_kg_hide-output":false},"cell_type":"code","source":"from mpl_toolkits.mplot3d import Axes3D\ndef plot_3d_graph(x,y,z, c=None):\n    sns.set(style = \"darkgrid\")\n\n    fig = plt.figure(figsize=(15,15))\n    ax = fig.add_subplot(111, projection = '3d')\n\n    ax.set_xlabel(\"x\")\n    ax.set_ylabel(\"y\")\n    ax.set_zlabel(\"z\")\n\n    ax.scatter(x, y, z, c=c)\n    ax.set_title(\"acce\")\n    plt.show()","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Plotting cumulative sum of acceleration vector moving in time.\n\nThis should give us a general idea of which direction the change in motion is happening."},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"plot_3d_graph(\n    np.cumsum(sample_data.acce[:,1]),\n    np.cumsum(sample_data.acce[:,2]),\n    np.cumsum(sample_data.acce[:,3]),\n    sample_data.acce[:,0]    \n)\n","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Double Integrating the Orignal Acceleration to Generate \"BAD\" Position Vectors (Noisy Data Makes the Integration Drift)"},{"metadata":{"trusted":true},"cell_type":"code","source":"time_ = sample_data.acce[:,0]\n\nvel_x = integrate.cumtrapz(sample_data.acce[:,1], time_, initial=0)\nvel_y = integrate.cumtrapz(sample_data.acce[:,2], time_, initial=0)\nvel_z = integrate.cumtrapz(sample_data.acce[:,3], time_, initial=0)\n\npos_x = integrate.cumtrapz(vel_x, time_, initial=0)\npos_y = integrate.cumtrapz(vel_y, time_, initial=0)\npos_z = integrate.cumtrapz(vel_z, time_, initial=0)","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### BAD Position Plot"},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"plot_3d_graph(\n    pos_x,\n    pos_y,\n    pos_z,\n    sample_data.acce[:,0]    \n)\n","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Plotting cumulative sum of magnetometer vector moving in time.\n\nI dont know what to do from magnetometer readings but it still looks like there is a strong directional output to the sensor"},{"metadata":{"trusted":true,"_kg_hide-input":true},"cell_type":"code","source":"plot_3d_graph(\n    np.cumsum(sample_data.magn[:,1]),\n    np.cumsum(sample_data.magn[:,2]),\n    np.cumsum(sample_data.magn[:,3]),\n    sample_data.magn[:,0]    \n)","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Using EKF from ahrs library, \nNote: the magnetometer readings are supposed to be added with a correction value to handle the True North vs Magnetic North seperation, I have not implemented this yet.\n"},{"metadata":{"trusted":true},"cell_type":"code","source":"ekf_estimator = EKF(gyr=sample_data.gyro[:,1:4], acc=sample_data.acce[:,1:4], mag=sample_data.magn[:,1:4], frequency=50.0)\n# Initializing the class with the sensor data generates our Quaternion in the Q Variable\n\nQ = ekf_estimator.Q\nn_samples = sample_data.acce.shape[0]\nnew_acce = np.zeros((n_samples, 3))\n# Initializing Array to hold the Linear acceleration Vector\n\nfor t in range(n_samples):\n    r = R.from_quat(Q[t])\n    # Getting a Rotation Matrix from the Quaternions\n    new_acce[t] = np.matmul(r.as_matrix().T,sample_data.acce[t][1:4])\n    # matmul Rotation Matrix Transpose to orignal Acceleration to produce the clean linear acceleration\n","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Double Integrating the Clean Linear Acceleration to Generate \"Better\" Position Vectors"},{"metadata":{"trusted":true},"cell_type":"code","source":"vel_x = integrate.cumtrapz(new_acce[:,0], time_, initial=0)\nvel_y = integrate.cumtrapz(new_acce[:,1], time_, initial=0)\nvel_z = integrate.cumtrapz(new_acce[:,2], time_, initial=0)\n\npos_x = integrate.cumtrapz(vel_x, time_, initial=0)\npos_y = integrate.cumtrapz(vel_y, time_, initial=0)\npos_z = integrate.cumtrapz(vel_z, time_, initial=0)","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"### Better Position Plot"},{"metadata":{"_kg_hide-input":true,"trusted":true},"cell_type":"code","source":"plot_3d_graph(\n    pos_y,\n    pos_x,\n    pos_z,\n    sample_data.magn[:,0]    \n)\n","execution_count":null,"outputs":[]},{"metadata":{},"cell_type":"markdown","source":"I read up somewhere that the axis may get switched up during the calculations, so to confirm our result, am confirming to see which axis has the least effect on the plot (It should be the Z axis as there is not much movement in that space.)"},{"metadata":{"trusted":true},"cell_type":"code","source":"plot_3d_graph(\n    pos_y,\n    pos_x,\n    np.zeros_like(pos_z),\n    sample_data.magn[:,0]    \n)\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"plot_3d_graph(\n    pos_x,\n    np.ones_like(pos_y),\n    pos_z,\n    sample_data.magn[:,0]    \n)\n","execution_count":null,"outputs":[]},{"metadata":{"trusted":true},"cell_type":"code","source":"plot_3d_graph(\n    np.ones_like(pos_x),\n    pos_y,\n    pos_z,\n    sample_data.magn[:,0]    \n)\n","execution_count":null,"outputs":[]}],"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":4,"nbformat_minor":4}