kalman.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'ahrs'",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-6-c4a778285916>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m      2\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mscipy\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      3\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mscipy\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mio\u001b[0m \u001b[1;32mas\u001b[0m \u001b[0msio\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 4\u001b[1;33m \u001b[1;32mimport\u001b[0m \u001b[0mahrs\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      5\u001b[0m \u001b[1;32mfrom\u001b[0m \u001b[0mahrs\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mfilters\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mEKF\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      6\u001b[0m \u001b[1;32mfrom\u001b[0m \u001b[0mahrs\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mcommon\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0morientation\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0macc2q\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mModuleNotFoundError\u001b[0m: No module named 'ahrs'"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import scipy\n",
    "import scipy.io as sio\n",
    "import ahrs\n",
    "from ahrs.filters import EKF\n",
    "from ahrs.common.orientation import acc2q\n",
    "import pandas as pd\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "acc = pd.read_csv('../data/acc.csv')\n",
    "gyro = pd.read_csv('../data/ang_vel.csv')\n",
    "mag = pd.read_csv('../data/mag.csv')\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(448, 4) (448, 4) (438, 4)\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "438"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "print(acc.shape, gyro.shape, mag.shape)\n",
    "num_samples = min(acc.shape[0], gyro.shape[0], mag.shape[0])\n",
    "num_samples"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(438, 3) (438, 3) (438, 3)\n"
     ]
    }
   ],
   "source": [
    "# drop timestamp column\n",
    "acc.drop(['Timestamp'], axis=1, inplace=True)\n",
    "gyro.drop(['Timestamp'], axis=1, inplace=True)\n",
    "mag.drop(['Timestamp'], axis=1, inplace=True)\n",
    "\n",
    "acc.drop(acc.index[num_samples:], inplace=True)\n",
    "gyro.drop(gyro.index[num_samples:], inplace=True)\n",
    "mag.drop(mag.index[num_samples:], inplace=True)\n",
    "\n",
    "print(acc.shape, gyro.shape, mag.shape)\n",
    "\n",
    "assert acc.shape == gyro.shape == mag.shape\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "acc_list = acc.to_numpy()\n",
    "gyro_list = gyro.to_numpy()\n",
    "mag_list = mag.to_numpy()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style scoped>\n",
       "    .dataframe tbody tr th:only-of-type {\n",
       "        vertical-align: middle;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: right;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>X</th>\n",
       "      <th>Y</th>\n",
       "      <th>Z</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <td>0</td>\n",
       "      <td>-0.488417</td>\n",
       "      <td>9.890448</td>\n",
       "      <td>1.053449</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>1</td>\n",
       "      <td>-0.715866</td>\n",
       "      <td>9.919178</td>\n",
       "      <td>1.553837</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>2</td>\n",
       "      <td>-0.519542</td>\n",
       "      <td>10.012551</td>\n",
       "      <td>1.881842</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>3</td>\n",
       "      <td>0.234632</td>\n",
       "      <td>9.890448</td>\n",
       "      <td>1.544260</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>4</td>\n",
       "      <td>0.646434</td>\n",
       "      <td>9.713276</td>\n",
       "      <td>0.847547</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>433</td>\n",
       "      <td>-0.821211</td>\n",
       "      <td>9.564836</td>\n",
       "      <td>2.868254</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>434</td>\n",
       "      <td>-0.720655</td>\n",
       "      <td>9.667787</td>\n",
       "      <td>3.179500</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>435</td>\n",
       "      <td>-0.701501</td>\n",
       "      <td>9.471462</td>\n",
       "      <td>2.995146</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>436</td>\n",
       "      <td>-0.885855</td>\n",
       "      <td>9.478644</td>\n",
       "      <td>2.971204</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <td>437</td>\n",
       "      <td>-1.091756</td>\n",
       "      <td>9.696517</td>\n",
       "      <td>2.817975</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "<p>438 rows × 3 columns</p>\n",
       "</div>"
      ],
      "text/plain": [
       "            X          Y         Z\n",
       "0   -0.488417   9.890448  1.053449\n",
       "1   -0.715866   9.919178  1.553837\n",
       "2   -0.519542  10.012551  1.881842\n",
       "3    0.234632   9.890448  1.544260\n",
       "4    0.646434   9.713276  0.847547\n",
       "..        ...        ...       ...\n",
       "433 -0.821211   9.564836  2.868254\n",
       "434 -0.720655   9.667787  3.179500\n",
       "435 -0.701501   9.471462  2.995146\n",
       "436 -0.885855   9.478644  2.971204\n",
       "437 -1.091756   9.696517  2.817975\n",
       "\n",
       "[438 rows x 3 columns]"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "acc"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# from ahrs.common import to_angles\n",
    "from ahrs.common.quaternion import QuaternionArray\n",
    "Q = QuaternionArray(ekf.Q)\n",
    "Q.to_angles()\n",
    "\n",
    "# array for quaternions\n",
    "# each index represents estimated quaternion for each sensor measurement  \n",
    "Q = np.zeros((num_samples, 4))\n",
    "# set initial position from acceleration alonr\n",
    "Q[0] = acc2q(acc_list[0])\n",
    "for t in range(1, num_samples):\n",
    "\tQ[t] = ekf.update(Q[t-1], gyro_list[t], acc_list[t])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ekf = EKF(gyr=gyro_list, acc=acc_list, mag=mag_list)\n",
    "ekf.Q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "tmp = pd.read_json('../logs/log.json') "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "<class 'pandas.core.frame.DataFrame'>\n",
      "RangeIndex: 68 entries, 0 to 67\n",
      "Data columns (total 3 columns):\n",
      "acc     68 non-null object\n",
      "gyro    68 non-null object\n",
      "mag     68 non-null object\n",
      "dtypes: object(3)\n",
      "memory usage: 1.7+ KB\n"
     ]
    }
   ],
   "source": [
    "tmp.info()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "acc = tmp['acc']\n",
    "gyro = tmp['gyro']\n",
    "mag = tmp['mag']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "def json_to_list(df):\n",
    "    tmp = []\n",
    "    for row in df.index:\n",
    "        msmt = df.loc[row]\n",
    "        tmp.append([msmt['x'], msmt['y'], msmt['z']])\n",
    "    return tmp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "acc_list = json_to_list(acc)\n",
    "gyro_list = json_to_list(gyro)\n",
    "mag_list = json_to_list(mag)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'EKF' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-24-e9f0f5eb1e90>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[1;32m----> 1\u001b[1;33m \u001b[0mekf\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mEKF\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mgyr\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mgyro_list\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0macc\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0macc_list\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mmag\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mmag_list\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      2\u001b[0m \u001b[0mekf\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mQ\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mNameError\u001b[0m: name 'EKF' is not defined"
     ]
    }
   ],
   "source": [
    "ekf = EKF(gyr=gyro_list, acc=acc_list, mag=mag_list)\n",
    "ekf.Q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'ahrs'",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-26-81d71d03fcfd>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[1;32m----> 1\u001b[1;33m \u001b[1;32mfrom\u001b[0m \u001b[0mahrs\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mfilters\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mekf\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mEKF\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m",
      "\u001b[1;31mModuleNotFoundError\u001b[0m: No module named 'ahrs'"
     ]
    }
   ],
   "source": [
    "from ahrs.filters.ekf import EKF "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "310202ad9400812d9967a37e4ee3420306bd685a92e2f5ee43836a94c78f6e55"
  },
  "kernelspec": {
   "display_name": "Python 3.7.0 64-bit",
   "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.9.5"
  },
  "orig_nbformat": 2
 },
 "nbformat": 4,
 "nbformat_minor": 2
}