a simple linear correction on a_nav

because we require the device to be still
during intialization and ending phase

main.ipynb

@@ -48,7 +48,7 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "# pull data from phone\n",
+    "# Pull Data From Phone\n",
     "data order: gyroscorpe, accelerometer, magnetometer"
    ]
   },
@@ -72,7 +72,7 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "## Initialization"
+    "# Initialization"
    ]
   },
   {
@@ -120,15 +120,15 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "a_filtered, w_filtered = Filt_signal((a, w), dt=dt, wn=20, btype='lowpass')\n",
+    "a_filtered, w_filtered = Filt_signal((a, w), dt=dt, wn=10, btype='lowpass')\n",
     "plot_signal([a, a_filtered], [w, w_filtered], [m])"
    ]
   },
   {
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "## Kalman Filter"
+    "# Kalman Filter"
    ]
   },
   {
@@ -147,7 +147,11 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "metadata": {},
+   "metadata": {
+    "tags": [
+     "outputPrepend"
+    ]
+   },
    "outputs": [],
    "source": [
     "a_nav = []\n",
@@ -198,10 +202,10 @@
     "    q = Normalized(q)\n",
     "    P = 0.5 * (P + P.T)  # make sure P is symmertical\n",
     "    \n",
-    "    tmp = -I(4)\n",
-    "    tmp[0, 0] = 1\n",
-    "    an = Rotate(tmp @ q) @ at + gn\n",
-    "    ori = Rotate(tmp @ q) @ orin\n",
+    "    conj = -I(4)\n",
+    "    conj[0, 0] = 1\n",
+    "    an = Rotate(conj @ q) @ at + gn\n",
+    "    ori = Rotate(conj @ q) @ orin\n",
     "\n",
     "    a_nav.append(an.T[0])\n",
     "    orientations.append(ori.T[0])\n",
@@ -212,23 +216,96 @@
     "orientations = np.array(orientations)"
    ]
   },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Accelerometer Bias/Error Correction"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "a_threshold = 0.2"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {
+    "tags": [
+     "outputPrepend"
+    ]
+   },
+   "outputs": [],
+   "source": [
+    "t_start = 0\n",
+    "for t in range(sample_number):\n",
+    "    at = a_nav[t]\n",
+    "    if np.linalg.norm(at) > a_threshold:\n",
+    "        t_start = t\n",
+    "        break\n",
+    "\n",
+    "t_end = 0\n",
+    "for t in range(sample_number - 1, -1,-1):\n",
+    "    at = a_nav[t]\n",
+    "    if np.linalg.norm(at - a_nav[-1]) > a_threshold:\n",
+    "        t_end = t\n",
+    "        break"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "print('motion starts at: ', t_start)\n",
+    "print('motion ends at: ', t_end)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "an_drift = a_nav[t_end:].mean(axis=0)\n",
+    "an_drift_rate = an_drift / (t_end - t_start)\n",
+    "\n",
+    "for i in range(t_end - t_start):\n",
+    "    a_nav[t_start + i] -= (i+1) * an_drift_rate\n",
+    "\n",
+    "for i in range(sample_number - t_end):\n",
+    "    a_nav[t_end + i] -= an_drift"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": null,
    "metadata": {},
    "outputs": [],
    "source": [
-    "filtered_a_nav, = Filt_signal([a_nav], dt=dt, wn=(0.01, 20), btype='bandpass')\n",
-    "plot_3([a_nav, filtered_a_nav])"
+    "filtered_a_nav, = Filt_signal([a_nav], dt=dt, wn=(0.01, 15), btype='bandpass')\n",
+    "plot_3([a_nav, filtered_a_nav])\n",
+    "# plot_3([a_nav])"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Zero Velocity Update"
    ]
   },
   {
    "cell_type": "code",
    "execution_count": null,
    "metadata": {
     "tags": [
-     "outputPrepend",
-     "outputPrepend",
      "outputPrepend"
     ]
    },
@@ -238,23 +315,22 @@
     "prevt = -1\n",
     "still_phase = False\n",
     "\n",
-    "threshold = 0.01\n",
-    "\n",
     "v = np.zeros((3, 1))\n",
     "t = 0\n",
     "while t < sample_number:\n",
-    "    at = a_nav[t, np.newaxis].T\n",
+    "    at = filtered_a_nav[t, np.newaxis].T\n",
     "\n",
-    "    if np.linalg.norm(at) < threshold:\n",
+    "    if np.linalg.norm(at) < a_threshold:\n",
     "        if not still_phase:\n",
     "            predict_v = v + at * dt\n",
-    "            drift_rate = predict_v / (t - prevt)\n",
+    "\n",
+    "            v_drift_rate = predict_v / (t - prevt)\n",
     "            for i in range(t - prevt - 1):\n",
-    "                velocities[prevt + 1 + i] -= (i + 1) * drift_rate.T[0]\n",
+    "                velocities[prevt + 1 + i] -= (i + 1) * v_drift_rate.T[0]\n",
     "\n",
-    "            v = np.zeros((3, 1))\n",
-    "            prevt = t\n",
-    "            still_phase = True\n",
+    "        v = np.zeros((3, 1))\n",
+    "        prevt = t\n",
+    "        still_phase = True\n",
     "    else:\n",
     "        v = v + at * dt\n",
     "        still_phase = False\n",
@@ -273,6 +349,13 @@
     "plot_3([velocities])"
    ]
   },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Integration To Get Position"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": null,
@@ -287,8 +370,7 @@
     "    at = filtered_a_nav[t, np.newaxis].T\n",
     "    vt = velocities[t, np.newaxis].T\n",
     "\n",
-    "    # p = p + vt * dt + 0.5 * at * dt**2\n",
-    "    p = p + vt * dt\n",
+    "    p = p + vt * dt + 0.5 * at * dt**2\n",
     "    positions.append(p.T[0])\n",
     "\n",
     "    t += 1\n",
@@ -314,6 +396,13 @@
     "plot_3([positions])"
    ]
   },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Close All Graphs"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": null,