notebooks/imu-velocity.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "import seaborn as sns\n",
    "sns.set(rc={'figure.figsize':(11, 4)})\n",
    "from ipca import IncrementalPCA\n",
    "from sklearn.decomposition import PCA\n",
    "from scipy.integrate import cumtrapz, trapz\n",
    "import matplotlib.pyplot as plt\n",
    "from scipy.signal import find_peaks\n",
    "import numpy as np\n",
    "from datetime import timedelta\n",
    "import gpxpy\n",
    "from xml.etree import ElementTree as ET"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Data import"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "data_file_path = \"../data/\"\n",
    "df = pd.read_csv(data_file_path)\n",
    "# df.columns = [\"time\", \"aX\", \"aY\", \"aZ\", \"gX\", \"gY\", \"gZ\", \"pca0\"]\n",
    "df.time = df.time - df.time.min()\n",
    "df"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "df.loc[:,[\"aX\", \"aY\", \"aZ\"]] = df.loc[:,[\"aX\", \"aY\", \"aZ\"]] * 9.8"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since we do not know the direction of movement (the device can be placed in different orientation), we perform Principal component analysis (PCA). The 0th component should ideally lie in the direction of movement, assuming there's a greater variation in acceleration in this direction."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "df.loc[:,[\"pca0\", \"pca1\", \"pca2\"]] = PCA().fit_transform(df.loc[:,[\"aX\", \"aY\", \"aZ\"]].values)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_peaks(data):\n",
    "    pos_kwargs={\n",
    "        \"distance\": 20,\n",
    "        \"height\": (35, None)\n",
    "    }\n",
    "    peaks, _ = find_peaks(data, **pos_kwargs)\n",
    "    return peaks # np.hstack([[0],peaks,[len(data)-1]])\n",
    "\n",
    "def get_step_ranges(data):\n",
    "    peaks = get_peaks(data)\n",
    "    print(peaks)\n",
    "    midpoints = (peaks[1:] + peaks[:-1]) / 2\n",
    "    return np.vstack([midpoints[:-1], midpoints[1:]]).T.round()\n",
    "\n",
    "steps = get_step_ranges(df.pca0)\n",
    "steps.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "df.loc[:,[\"pca0\", \"pca1\", \"pca2\"]].plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "step = steps[-1]\n",
    "a, b = step.astype(int)\n",
    "velocity = cumtrapz(df.pca0.iloc[a:b], x=df.time.iloc[a:b] / 1000) * 3.6\n",
    "display(pd.Series(velocity).abs().max())\n",
    "df.pca0.iloc[a:b].plot()\n",
    "plt.show()\n",
    "sns.lineplot(x=range(len(velocity)), y=velocity)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_speed(data, time, step):\n",
    "    a, b = step.astype(int)\n",
    "    velocity = cumtrapz(data.loc[a:b], x=time.loc[a:b] / 1000) * 3.6\n",
    "    return np.max(np.abs(velocity))\n",
    "\n",
    "velocity = []\n",
    "window_size = 1000\n",
    "step_size = 100\n",
    "for i in np.arange(-window_size+step_size, len(df), step_size):\n",
    "    _df = df.loc[i:i+window_size]\n",
    "    _steps = [x for x in steps if (int(x[0]) in _df.index) and (x[1] in _df.index)]\n",
    "    velocity += [{\n",
    "        \"time\": _df.time.iloc[-1],\n",
    "        \"velocity\": np.mean([get_speed(_df.pca0, _df.time, x) for x in _steps])\n",
    "    }]\n",
    "velocity = pd.DataFrame(velocity).fillna(0.0)\n",
    "sns.lineplot(x=velocity.time/1000, y=velocity.velocity)\n",
    "velocity.to_csv(\"../data/wmf-sports-field-lap_velocity.csv\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "time_steps = df.time.iloc[steps[:,1]]\n",
    "distance = pd.Series(cumtrapz(velocity.velocity / 3.6, velocity.time / 1000), index=velocity.time.iloc[:-1] / 1000)\n",
    "distance.plot()\n",
    "distance.iloc[-1]\n",
    "# velocity"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def round_to_seconds(timestamp):\n",
    "    return timestamp - timedelta(microseconds=timestamp.microsecond)\n",
    "\n",
    "timestamp = pd.to_datetime(\"2020-09-22T16:02:36.516Z\")\n",
    "timestamps = [round_to_seconds(timestamp + timedelta(seconds=x)) for x in time_steps / 1000]\n",
    "out_df = pd.DataFrame([timestamps, velocity, distance]).T.dropna()\n",
    "out_df.columns = [\"time\", \"speed\", \"course\"]\n",
    "out_df.speed = out_df.speed.astype(float)\n",
    "out_df.course = out_df.course.astype(float)\n",
    "out_df = out_df.groupby(\"time\").mean().reset_index()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_speed_extension(speed, distance):\n",
    "    prefix = \"gpxtpx:\"\n",
    "    element = ET.Element(f\"{prefix}TrackPointExtension\")\n",
    "    speed_element = ET.SubElement(element, f\"{prefix}speed\")\n",
    "    course_element = ET.SubElement(element, f\"{prefix}course\")\n",
    "    cadence_element = ET.SubElement(element, f\"{prefix}cad\")\n",
    "    # Schema only permits integers up to 254\n",
    "    cadence_element.text = str(30)\n",
    "    speed_element.text = str(speed)\n",
    "    course_element.text = str(distance)\n",
    "    return element\n",
    "\n",
    "def get_nmea_speed(speed):\n",
    "    prefix = \"nmea:\"\n",
    "    element = ET.Element(f\"{prefix}speed\")\n",
    "    element.text = str(speed)\n",
    "    return element\n",
    "\n",
    "def get_point(time, speed, distance):\n",
    "    extensions = [get_nmea_speed(speed), get_speed_extension(speed, distance)]\n",
    "    point = gpxpy.gpx.GPXTrackPoint()\n",
    "    point.extensions = extensions\n",
    "    point.time = time\n",
    "    return point\n",
    "\n",
    "def get_gpx(timestamps, speed, distance):\n",
    "    \"\"\"\n",
    "    data -- pandas DataFrame with time and cadence fields\n",
    "    \"\"\"\n",
    "    gpx = gpxpy.gpx.GPX()\n",
    "    gpx.nsmap[\"gpxtpx\"] = 'http://www.garmin.com/xmlschemas/TrackPointExtension/v2'\n",
    "    gpx.nsmap[\"nmea\"] = 'http://trekbuddy.net/2009/01/gpx/nmea'\n",
    "    track = gpxpy.gpx.GPXTrack()\n",
    "    gpx.tracks.append(track)\n",
    "    segment = gpxpy.gpx.GPXTrackSegment()\n",
    "    track.segments.append(segment)\n",
    "    segment.points = [get_point(t, v, s) for t, v, s in zip(timestamps, speed, distance)]\n",
    "    return gpx\n",
    "    \n",
    "output_file_path = \"../data/wmf-up-n-down.gpx\"\n",
    "# timestamp = pd.to_datetime(\"2020-09-22T16:02:36.516Z\")\n",
    "# timestamps = [timestamp + timedelta(seconds=x) for x in steps[:,1] / 1000]\n",
    "\n",
    "with open(output_file_path, 'w+') as f:\n",
    "    f.write(get_gpx(out_df.time, out_df.speed.values / 3.6, out_df.course.values).to_xml())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}