complimentary-filter.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "pitch=0.00, roll=9.78 degrees\n",
      "pitch=0.00, roll=15.64 degrees\n",
      "pitch=0.00, roll=19.16 degrees\n",
      "pitch=0.00, roll=21.28 degrees\n",
      "pitch=0.00, roll=22.54 degrees\n",
      "pitch=0.00, roll=23.30 degrees\n",
      "pitch=0.00, roll=23.76 degrees\n",
      "pitch=0.00, roll=24.03 degrees\n",
      "pitch=0.00, roll=24.20 degrees\n",
      "pitch=0.00, roll=24.30 degrees\n",
      "pitch=0.00, roll=24.36 degrees\n",
      "pitch=0.00, roll=24.39 degrees\n",
      "pitch=0.00, roll=24.41 degrees\n",
      "pitch=0.00, roll=24.42 degrees\n",
      "pitch=0.00, roll=24.43 degrees\n",
      "pitch=0.00, roll=24.44 degrees\n",
      "pitch=0.00, roll=24.44 degrees\n",
      "pitch=0.00, roll=24.44 degrees\n",
      "pitch=0.00, roll=24.44 degrees\n",
      "pitch=0.00, roll=24.44 degrees\n",
      "X=0.00, Y=4.06, Z=8.93, length=9.81\n",
      "Gyro updates\n",
      "pitch=0.30, roll=24.74 degrees\n",
      "X=0.05, Y=4.11, Z=8.91, length=9.81\n",
      "pitch=0.48, roll=24.92 degrees\n",
      "X=0.08, Y=4.13, Z=8.90, length=9.81\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import math\n",
    "\n",
    "X = 0\n",
    "Y = 1\n",
    "Z = 2\n",
    "\n",
    "class CompFilter:\n",
    "    \n",
    "    pitch = 0\n",
    "    roll = 0\n",
    "    yaw = 0\n",
    "    \n",
    "    alpha = 0.6\n",
    "    \n",
    "    def update(self, acc_vec, gyro_vec, dt):\n",
    "        \n",
    "        # determine angles just from accelerometer\n",
    "        roll_acc = math.atan2(acc_vec[Y], acc_vec[Z]) * 180.0 / math.pi;\n",
    "        pitch_acc = math.atan2(-acc_vec[X], math.sqrt(acc_vec[Y]**2 + acc_vec[Z]**2)) * 180.0 / math.pi;\n",
    "        \n",
    "        #integrate gyro using previous angles\n",
    "        roll_gyro = self.roll + gyro_vec[X] * dt\n",
    "        pitch_gyro = self.pitch + gyro_vec[Y] * dt\n",
    "        \n",
    "        \n",
    "        self.pitch = pitch_gyro * self.alpha + pitch_acc * (1.0 - self.alpha)\n",
    "        self.roll = roll_gyro * self.alpha + roll_acc * (1.0 - self.alpha)\n",
    "        \n",
    "    def gravity_vect(self):\n",
    "        x = math.sin(math.radians(self.pitch))\n",
    "        z = math.cos(math.radians(self.roll)) * math.cos(math.radians(self.pitch))\n",
    "        y = math.sin(math.radians(self.roll)) * math.cos(math.radians(self.pitch))\n",
    "        \n",
    "        return np.array([x, y, z])  * 9.81\n",
    "    \n",
    "    \n",
    "\n",
    "f = CompFilter()\n",
    "\n",
    "\n",
    "acc_reading = np.array([0, 4.0, 8.8])\n",
    "gyro_reading = np.array([0, 0, 0])\n",
    "DT = 0.1\n",
    "\n",
    "# demonstrate successive updates without any change in gyro (showing LPF nature of accel data)\n",
    "for i in range(20):\n",
    "    f.update(acc_reading, gyro_reading, DT)\n",
    "\n",
    "    print(\"pitch=%.2f, roll=%.2f degrees\"  % (f.pitch, f.roll))\n",
    "\n",
    "# print final gravity vector\n",
    "gv = f.gravity_vect()\n",
    "print(\"X=%.2f, Y=%.2f, Z=%.2f, length=%.2f\" % (gv[X], gv[Y], gv[Z], np.linalg.norm(gv)))\n",
    "\n",
    "print(\"Gyro updates\")\n",
    "\n",
    "# demonstrate gyro updates\n",
    "for x in range(2):\n",
    "    gyro_reading = np.array([5, 5, 0])\n",
    "    f.update(acc_reading, gyro_reading, DT)\n",
    "    print(\"pitch=%.2f, roll=%.2f degrees\"  % (f.pitch, f.roll))\n",
    "    gv = f.gravity_vect()\n",
    "    print(\"X=%.2f, Y=%.2f, Z=%.2f, length=%.2f\" % (gv[X], gv[Y], gv[Z], np.linalg.norm(gv)))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "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.6.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}