quadcopter_dynamics.ipynb

{
 "metadata": {
  "name": ""
 },
 "nbformat": 3,
 "nbformat_minor": 0,
 "worksheets": [
  {
   "cells": [
    {
     "cell_type": "heading",
     "level": 1,
     "metadata": {},
     "source": [
      "Quadcopter dynamics"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Based on internet research, I've tried to develop an understanding of the dynamics of a quadcopter. This should be useful as I learn how to build and fly my own, with a view to achieving some sort of autonomous sensor platform."
     ]
    },
    {
     "cell_type": "heading",
     "level": 2,
     "metadata": {},
     "source": [
      "References"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "* <https://engineering.purdue.edu/~bethel/rot2.pdf>\n",
      "* <http://www.chrobotics.com/library/understanding-euler-angles>\n",
      "* <http://andrew.gibiansky.com/blog/physics/quadcopter-dynamics/>"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "from IPython.display import SVG\n",
      "from sympy import *\n",
      "\n",
      "init_printing()  # Enable LaTeX formatting"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 1
    },
    {
     "cell_type": "heading",
     "level": 2,
     "metadata": {},
     "source": [
      "Kinematics"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "SVG(filename=\"quad_body.svg\")"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 2,
       "svg": [
        "<svg height=\"512.21594\" id=\"svg2\" inkscape:version=\"0.48.4 r9939\" sodipodi:docname=\"quad_body.svg\" version=\"1.1\" width=\"540.2655\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:cc=\"http://creativecommons.org/ns#\" xmlns:dc=\"http://purl.org/dc/elements/1.1/\" xmlns:inkscape=\"http://www.inkscape.org/namespaces/inkscape\" xmlns:rdf=\"http://www.w3.org/1999/02/22-rdf-syntax-ns#\" xmlns:sodipodi=\"http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd\" xmlns:svg=\"http://www.w3.org/2000/svg\">\n",
        "  <sodipodi:namedview bordercolor=\"#666666\" borderopacity=\"1.0\" fit-margin-bottom=\"10\" fit-margin-left=\"10\" fit-margin-right=\"10\" fit-margin-top=\"10\" id=\"base\" inkscape:bbox-nodes=\"false\" inkscape:current-layer=\"g5673\" inkscape:cx=\"120.03653\" inkscape:cy=\"88.920701\" inkscape:document-units=\"px\" inkscape:object-nodes=\"true\" inkscape:pageopacity=\"0.0\" inkscape:pageshadow=\"2\" inkscape:snap-bbox=\"false\" inkscape:snap-center=\"true\" inkscape:snap-grids=\"true\" inkscape:window-height=\"1000\" inkscape:window-maximized=\"1\" inkscape:window-width=\"1215\" inkscape:window-x=\"65\" inkscape:window-y=\"24\" inkscape:zoom=\"4\" pagecolor=\"#ffffff\" showgrid=\"true\">\n",
        "    <inkscape:grid empspacing=\"5\" enabled=\"false\" id=\"grid3004\" originx=\"-52.473473px\" originy=\"-296.1853px\" snapvisiblegridlinesonly=\"true\" type=\"xygrid\" visible=\"true\"/>\n",
        "    <inkscape:grid empspacing=\"5\" enabled=\"true\" id=\"grid3006\" originx=\"-14.80918mm\" originy=\"-83.590074mm\" snapvisiblegridlinesonly=\"true\" type=\"axonomgrid\" units=\"mm\" visible=\"true\"/>\n",
        "  </sodipodi:namedview>\n",
        "  <defs id=\"defs4\">\n",
        "    <marker id=\"Arrow1Mend\" inkscape:stockid=\"Arrow1Mend\" orient=\"auto\" refX=\"0\" refY=\"0\" style=\"overflow:visible\">\n",
        "      <path d=\"M 0,0 5,-5 -12.5,0 5,5 0,0 z\" id=\"path3801\" inkscape:connector-curvature=\"0\" style=\"fill-rule:evenodd;stroke:#000000;stroke-width:1pt\" transform=\"matrix(-0.4,0,0,-0.4,-4,0)\"/>\n",
        "    </marker>\n",
        "    <marker id=\"Arrow1Lstart\" inkscape:stockid=\"Arrow1Lstart\" orient=\"auto\" refX=\"0\" refY=\"0\" style=\"overflow:visible\">\n",
        "      <path d=\"M 0,0 5,-5 -12.5,0 5,5 0,0 z\" id=\"path3792\" inkscape:connector-curvature=\"0\" style=\"fill-rule:evenodd;stroke:#000000;stroke-width:1pt\" transform=\"matrix(0.8,0,0,0.8,10,0)\"/>\n",
        "    </marker>\n",
        "    <marker id=\"Arrow1Lend\" inkscape:stockid=\"Arrow1Lend\" orient=\"auto\" refX=\"0\" refY=\"0\" style=\"overflow:visible\">\n",
        "      <path d=\"M 0,0 5,-5 -12.5,0 5,5 0,0 z\" id=\"path3795\" inkscape:connector-curvature=\"0\" style=\"fill-rule:evenodd;stroke:#000000;stroke-width:1pt\" transform=\"matrix(-0.8,0,0,-0.8,-10,0)\"/>\n",
        "    </marker>\n",
        "    <marker id=\"Arrow1Mend5\" inkscape:stockid=\"Arrow1Mend5\" orient=\"auto\" refX=\"0\" refY=\"0\" style=\"overflow:visible\">\n",
        "      <path d=\"M 0,0 5,-5 -12.5,0 5,5 0,0 z\" id=\"path5541\" inkscape:connector-curvature=\"0\" style=\"fill:#ff0000;fill-rule:evenodd;stroke:#ff0000;stroke-width:1pt\" transform=\"matrix(-0.4,0,0,-0.4,-4,0)\"/>\n",
        "    </marker>\n",
        "    <marker id=\"Arrow1Mend6\" inkscape:stockid=\"Arrow1Mend6\" orient=\"auto\" refX=\"0\" refY=\"0\" style=\"overflow:visible\">\n",
        "      <path d=\"M 0,0 5,-5 -12.5,0 5,5 0,0 z\" id=\"path5585\" inkscape:connector-curvature=\"0\" style=\"fill:#0000ff;fill-rule:evenodd;stroke:#0000ff;stroke-width:1pt\" transform=\"matrix(-0.4,0,0,-0.4,-4,0)\"/>\n",
        "    </marker>\n",
        "    <marker id=\"Arrow1MendK\" inkscape:stockid=\"Arrow1MendK\" orient=\"auto\" refX=\"0\" refY=\"0\" style=\"overflow:visible\">\n",
        "      <path d=\"M 0,0 5,-5 -12.5,0 5,5 0,0 z\" id=\"path5588\" inkscape:connector-curvature=\"0\" style=\"fill:#008000;fill-rule:evenodd;stroke:#008000;stroke-width:1pt\" transform=\"matrix(-0.4,0,0,-0.4,-4,0)\"/>\n",
        "    </marker>\n",
        "  </defs>\n",
        "  <metadata id=\"metadata7\">\n",
        "    <rdf:RDF>\n",
        "      <cc:Work rdf:about=\"\">\n",
        "        <dc:format>image/svg+xml</dc:format>\n",
        "        <dc:type rdf:resource=\"http://purl.org/dc/dcmitype/StillImage\"/>\n",
        "        <dc:title/>\n",
        "      </cc:Work>\n",
        "    </rdf:RDF>\n",
        "  </metadata>\n",
        "  <g id=\"layer1\" inkscape:groupmode=\"layer\" inkscape:label=\"Layer 1\" transform=\"translate(-52.473473,-243.96092)\">\n",
        "    <path d=\"M 153.4297,574.01573 490.97503,379.13384\" id=\"path3008\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cc\" style=\"fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none\"/>\n",
        "    <path d=\"M 490.97503,574.01573 322.20236,476.57478 153.4297,379.13384\" id=\"path3010\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"ccc\" style=\"fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none\"/>\n",
        "    <g id=\"g5673\" transform=\"translate(-92.163729,88.893424)\">\n",
        "      <path d=\"m 245.48752,609.4488 0,35.43307\" id=\"path4784\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cc\" style=\"fill:none;stroke:#0000ff;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend6)\"/>\n",
        "      <path d=\"m 245.48752,609.4488 30.68594,-17.71654\" id=\"path4786\" inkscape:connector-curvature=\"0\" style=\"fill:#ff0000;stroke:#ff0000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend5)\"/>\n",
        "      <path d=\"m 245.48752,609.4488 30.79185,17.40577\" id=\"path4788\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cc\" style=\"fill:none;stroke:#008000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1MendK)\"/>\n",
        "      <text id=\"text5619\" sodipodi:linespacing=\"125%\" style=\"font-size:28px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Crimson;-inkscape-font-specification:Crimson Italic\" x=\"278.45779\" xml:space=\"preserve\" y=\"596.3559\"><tspan id=\"tspan5621\" sodipodi:role=\"line\" x=\"278.45779\" y=\"596.3559\">x</tspan></text>\n",
        "      <text id=\"text5623\" sodipodi:linespacing=\"125%\" style=\"font-size:28px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Crimson;-inkscape-font-specification:Crimson Italic\" x=\"280.90506\" xml:space=\"preserve\" y=\"631.77759\"><tspan id=\"tspan5625\" sodipodi:role=\"line\" x=\"280.90506\" y=\"631.77759\">y</tspan></text>\n",
        "      <text id=\"text5627\" sodipodi:linespacing=\"125%\" style=\"font-size:28px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Crimson;-inkscape-font-specification:Crimson Italic\" x=\"249.05019\" xml:space=\"preserve\" y=\"649.58069\"><tspan id=\"tspan5629\" sodipodi:role=\"line\" x=\"249.05019\" y=\"649.58069\">z</tspan></text>\n",
        "    </g>\n",
        "    <g id=\"g5644\" transform=\"matrix(0.44019464,0,0,0.44019464,275.65818,227.62535)\">\n",
        "      <path d=\"m 486.54409,343.00517 c 0,0 26.56819,32.4973 97.44095,76.71485 78.10345,48.72878 114.37247,48.13122 106.29921,30.68594 -8.07326,-17.44529 -71.966,-59.28595 -113.38582,-79.78345 -24.80315,-12.27437 -55.17444,-27.76351 -81.49607,-42.96031 -17.71653,30.68594 8.85827,-15.34298 -8.85827,15.34297 z\" id=\"path5631\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cczscc\" style=\"fill:#000000;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "      <path d=\"m 493.57104,327.16428 c 0,0 -26.56819,-32.4973 -97.44095,-76.71485 -78.10345,-48.72878 -114.37247,-48.13122 -106.29921,-30.68594 8.07326,17.44529 71.966,59.28595 113.38582,79.78345 24.80315,12.27437 55.17444,27.76351 81.49607,42.96031 17.71653,-30.68594 -8.85827,15.34298 8.85827,-15.34297 z\" id=\"path5633\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cczscc\" style=\"fill:#000000;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "      <path d=\"m 484.89802,286.94111 c -14.59791,-0.157 -33.23541,32.88539 -33.23541,54.91366 0,3.3685 14.72132,6.09921 32.88099,6.09921 18.15967,0 33.58983,-2.41671 33.58983,-5.78521 0,-22.02827 -18.6375,-55.07066 -33.23541,-55.22766 z\" id=\"path5639\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"ccscc\" style=\"fill:#ff0000;fill-opacity:1;stroke:#000000;stroke-width:0.91601712px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "    </g>\n",
        "    <g id=\"g5649\" transform=\"matrix(0.44019464,0,0,0.44019464,-61.88715,430.30597)\">\n",
        "      <path d=\"m 486.54409,343.00517 c 0,0 26.56819,32.4973 97.44095,76.71485 78.10345,48.72878 114.37247,48.13122 106.29921,30.68594 -8.07326,-17.44529 -71.966,-59.28595 -113.38582,-79.78345 -24.80315,-12.27437 -55.17444,-27.76351 -81.49607,-42.96031 -17.71653,30.68594 8.85827,-15.34298 -8.85827,15.34297 z\" id=\"path5651\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cczscc\" style=\"fill:#000000;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "      <path d=\"m 493.57104,327.16428 c 0,0 -26.56819,-32.4973 -97.44095,-76.71485 -78.10345,-48.72878 -114.37247,-48.13122 -106.29921,-30.68594 8.07326,17.44529 71.966,59.28595 113.38582,79.78345 24.80315,12.27437 55.17444,27.76351 81.49607,42.96031 17.71653,-30.68594 -8.85827,15.34298 8.85827,-15.34297 z\" id=\"path5653\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cczscc\" style=\"fill:#000000;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "      <path d=\"m 484.89802,286.94111 c -14.59791,-0.157 -33.23541,32.88539 -33.23541,54.91366 0,3.3685 14.72132,6.09921 32.88099,6.09921 18.15967,0 33.58983,-2.41671 33.58983,-5.78521 0,-22.02827 -18.6375,-55.07066 -33.23541,-55.22766 z\" id=\"path5655\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"ccscc\" style=\"fill:#ff0000;fill-opacity:1;stroke:#000000;stroke-width:0.91601712px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "    </g>\n",
        "    <g id=\"g5657\" transform=\"matrix(0.44019464,0,0,0.44019464,275.65818,430.30597)\">\n",
        "      <path d=\"m 495.16014,342.08771 c 0,0 41.42757,-6.76008 115.15747,-46.02892 81.25209,-43.27518 98.86911,-74.98385 79.72442,-76.71485 -19.14469,-1.73099 -87.32614,32.68141 -125.78741,58.30329 -23.03149,15.34297 -51.63112,33.9007 -77.95275,49.0975 17.71653,30.68594 -8.85827,-15.34297 8.85827,15.34298 z\" id=\"path5659\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cczscc\" style=\"fill:#000000;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "      <path d=\"m 484.95499,328.08174 c 0,0 -41.42757,6.76008 -115.15747,46.02892 -81.25209,43.27518 -98.86911,74.98385 -79.72442,76.71485 19.14469,1.73099 87.32614,-32.68141 125.78741,-58.30329 23.03149,-15.34297 51.63112,-33.9007 77.95275,-49.0975 -17.71653,-30.68594 8.85827,15.34297 -8.85827,-15.34298 z\" id=\"path5661\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cczscc\" style=\"fill:#000000;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "      <path d=\"m 484.89802,286.94111 c -14.59791,-0.157 -33.23541,32.88539 -33.23541,54.91366 0,3.3685 14.72132,6.09921 32.88099,6.09921 18.15967,0 33.58983,-2.41671 33.58983,-5.78521 0,-22.02827 -18.6375,-55.07066 -33.23541,-55.22766 z\" id=\"path5663\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"ccscc\" style=\"fill:#ff0000;fill-opacity:1;stroke:#000000;stroke-width:0.91601712px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "    </g>\n",
        "    <g id=\"g5687\">\n",
        "      <path d=\"m 156.07969,386.00926 c 0,0 18.23619,-2.97576 50.6917,-20.26169 35.76673,-19.0495 43.52165,-33.00749 35.09426,-33.76946 -8.42739,-0.76198 -38.4405,14.38618 -55.37094,25.66478 -10.13834,6.7539 -22.72774,14.92292 -34.31438,21.61247 7.79872,13.50778 -3.89937,-6.7539 3.89936,6.7539 z\" id=\"path5667\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cczscc\" style=\"fill:#000000;stroke:#000000;stroke-width:0.44019464px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "      <path d=\"m 151.58744,379.8439 c 0,0 -18.23619,2.97575 -50.69171,20.26168 -35.766728,19.04951 -43.521645,33.00749 -35.094253,33.76947 8.427387,0.76198 38.440503,-14.38618 55.370943,-25.66479 10.13834,-6.7539 22.72774,-14.92291 34.31438,-21.61246 -7.79872,-13.50779 3.89936,6.75389 -3.89936,-6.7539 z\" id=\"path5669\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"cczscc\" style=\"fill:#000000;stroke:#000000;stroke-width:0.44019464px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "      <path d=\"m 151.56236,361.73402 c -6.42592,-0.0691 -14.63005,14.47597 -14.63005,24.1727 0,1.48279 6.48025,2.68484 14.47404,2.68484 7.99378,0 14.78606,-1.06383 14.78606,-2.54662 0,-9.69673 -8.20413,-24.24181 -14.63005,-24.31092 z\" id=\"path5671\" inkscape:connector-curvature=\"0\" sodipodi:nodetypes=\"ccscc\" style=\"fill:#ff0000;fill-opacity:1;stroke:#000000;stroke-width:0.40322581px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1\"/>\n",
        "    </g>\n",
        "    <path d=\"m 336.42858,476.64789 c 0,8.67876 -7.03553,15.71428 -15.71429,15.71428 -8.67876,0 -15.71428,-7.03552 -15.71428,-15.71428 0,-8.67876 7.03552,-15.71429 15.71428,-15.71429 8.67876,0 15.71429,7.03553 15.71429,15.71429 z\" id=\"path5692\" sodipodi:cx=\"320.71429\" sodipodi:cy=\"476.64789\" sodipodi:rx=\"15.714286\" sodipodi:ry=\"15.714286\" sodipodi:type=\"arc\" style=\"fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:2;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none\" transform=\"matrix(0.89743589,0,0,0.89743589,31.17949,50.60126)\"/>\n",
        "    <path d=\"m 319.00001,478.36218 0,56.14286\" id=\"path5694\" inkscape:connector-curvature=\"0\" style=\"fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend)\"/>\n",
        "    <text id=\"text5888\" sodipodi:linespacing=\"125%\" style=\"font-size:40px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Crimson;-inkscape-font-specification:Crimson Italic\" x=\"295.71429\" xml:space=\"preserve\" y=\"563.79071\"><tspan id=\"tspan5890\" sodipodi:role=\"line\" x=\"295.71429\" y=\"563.79071\">mg</tspan></text>\n",
        "    <g id=\"g5933\" transform=\"translate(-6.4938718e-7,3.9285686)\">\n",
        "      <path d=\"m 489.10769,350.00672 0,-56.14286\" id=\"path5892\" inkscape:connector-curvature=\"0\" style=\"fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend)\"/>\n",
        "      <text id=\"text5894\" sodipodi:linespacing=\"125%\" style=\"font-size:40px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Crimson;-inkscape-font-specification:Crimson Italic\" x=\"465\" xml:space=\"preserve\" y=\"279.505\"><tspan id=\"tspan5896\" sodipodi:role=\"line\" x=\"465\" y=\"279.505\">L<tspan id=\"tspan5913\" style=\"font-size:65.00091553%;baseline-shift:sub\">1</tspan></tspan></text>\n",
        "    </g>\n",
        "    <g id=\"g5939\" transform=\"translate(-6.4938718e-7,206.60919)\">\n",
        "      <path d=\"m 489.10769,350.00672 0,-56.14286\" id=\"path5941\" inkscape:connector-curvature=\"0\" style=\"fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend)\"/>\n",
        "      <text id=\"text5943\" sodipodi:linespacing=\"125%\" style=\"font-size:40px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Crimson;-inkscape-font-specification:Crimson Italic\" x=\"465\" xml:space=\"preserve\" y=\"279.505\"><tspan id=\"tspan5945\" sodipodi:role=\"line\" x=\"465\" y=\"279.505\">L<tspan id=\"tspan5975\" style=\"font-size:65.00091553%;baseline-shift:sub\">2</tspan></tspan></text>\n",
        "    </g>\n",
        "    <g id=\"g5949\" transform=\"translate(-337.54533,206.60919)\">\n",
        "      <path d=\"m 489.10769,350.00672 0,-56.14286\" id=\"path5951\" inkscape:connector-curvature=\"0\" style=\"fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend)\"/>\n",
        "      <text id=\"text5953\" sodipodi:linespacing=\"125%\" style=\"font-size:40px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Crimson;-inkscape-font-specification:Crimson Italic\" x=\"465\" xml:space=\"preserve\" y=\"279.505\"><tspan id=\"tspan5955\" sodipodi:role=\"line\" x=\"465\" y=\"279.505\">L<tspan id=\"tspan5973\" style=\"font-size:65.00091553%;baseline-shift:sub\">3</tspan></tspan></text>\n",
        "    </g>\n",
        "    <g id=\"g5959\" transform=\"translate(-337.54533,11.7273)\">\n",
        "      <path d=\"m 489.10769,350.00672 0,-56.14286\" id=\"path5961\" inkscape:connector-curvature=\"0\" style=\"fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend)\"/>\n",
        "      <text id=\"text5963\" sodipodi:linespacing=\"125%\" style=\"font-size:40px;font-style:italic;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Crimson;-inkscape-font-specification:Crimson Italic\" x=\"465\" xml:space=\"preserve\" y=\"279.505\"><tspan id=\"tspan5965\" sodipodi:role=\"line\" x=\"465\" y=\"279.505\">L<tspan id=\"tspan5971\" style=\"font-size:65.00091553%;baseline-shift:sub\">4</tspan></tspan></text>\n",
        "    </g>\n",
        "  </g>\n",
        "</svg>"
       ],
       "text": [
        "<IPython.core.display.SVG at 0x67f2588>"
       ]
      }
     ],
     "prompt_number": 2
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Here we have a basic representation of a quadcopter: a simple body with a mass $m$ and four propellers/rotors, each producing a lift force $L_n$.\n",
      "\n",
      "We have used the aircraft principal axes for flight: $x$ is positive forwards, $y$ is positive right and $z$ is positive down."
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The quadcopter has its own frame of reference, which we will call the body frame. The position of the quadcopter is measured relative to a world frame of reference, known as the inertial frame.\n",
      "\n",
      "The position of the quadcopter in the inertial frame can be defined as a vector $X$:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "x, y, z = symbols(\"x, y, z\")\n",
      "X = Matrix([x, y, z])\n",
      "X"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}x\\\\y\\\\z\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 3,
       "text": [
        "\u23a1x\u23a4\n",
        "\u23a2 \u23a5\n",
        "\u23a2y\u23a5\n",
        "\u23a2 \u23a5\n",
        "\u23a3z\u23a6"
       ]
      }
     ],
     "prompt_number": 3
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The velocity of the quadcopter can also be defined, as $\\dot X$:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "x_dot, y_dot, z_dot = symbols(\"\\dot{x}, \\dot{y}, \\dot{z}\")\n",
      "X_dot = Matrix([x_dot, y_dot, z_dot])\n",
      "X_dot"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}\\dot{x}\\\\\\dot{y}\\\\\\dot{z}\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 4,
       "text": [
        "\u23a1\\dot{x}\u23a4\n",
        "\u23a2       \u23a5\n",
        "\u23a2\\dot{y}\u23a5\n",
        "\u23a2       \u23a5\n",
        "\u23a3\\dot{z}\u23a6"
       ]
      }
     ],
     "prompt_number": 4
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Roll, pitch and yaw angles of the quadcopter *in the body frame* can be defined as $\\theta$:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "phi, theta, psi = symbols(\"phi, theta, psi\")\n",
      "Theta = Matrix([phi, theta, psi])\n",
      "Theta"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}\\phi\\\\\\theta\\\\\\psi\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 5,
       "text": [
        "\u23a1\u03c6\u23a4\n",
        "\u23a2 \u23a5\n",
        "\u23a2\u03b8\u23a5\n",
        "\u23a2 \u23a5\n",
        "\u23a3\u03c8\u23a6"
       ]
      }
     ],
     "prompt_number": 5
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The corresponding angular velocities are equal to $\\dot\\theta$:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "phi_dot, theta_dot, psi_dot = symbols(\"\\dot{\\phi}, \\dot{\\\\theta}, \\dot{\\psi}\")\n",
      "Theta_dot = Matrix([phi_dot, theta_dot, psi_dot])\n",
      "Theta_dot"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}\\dot{\\phi}\\\\\\dot{\\theta}\\\\\\dot{\\psi}\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 6,
       "text": [
        "\u23a1 \\dot{\\phi} \u23a4\n",
        "\u23a2            \u23a5\n",
        "\u23a2\\dot{\\theta}\u23a5\n",
        "\u23a2            \u23a5\n",
        "\u23a3 \\dot{\\psi} \u23a6"
       ]
      }
     ],
     "prompt_number": 6
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "An angular velocity vector, $\\omega$, exists for the quadcopter, but $\\omega \\ne \\dot\\theta$. An angular velocity vector points along the axis of rotation, as discussed on Wikipedia:\n",
      "\n",
      "<http://en.wikipedia.org/wiki/Angular_velocity>\n",
      "\n",
      "The $\\theta$ vector contains time derivatives of roll, pitch and yaw angles, and these can be converted to an angular velocity vector using a transformation matrix:\n",
      "\n",
      "$$\n",
      "\\omega = \\begin{bmatrix}\n",
      "1 & 0 & -\\sin\\theta \\\\\n",
      "0 & \\cos\\phi & \\cos\\theta \\sin\\phi \\\\\n",
      "0 & -\\sin\\phi & \\cos\\theta\\cos\\phi \\\\\n",
      "\\end{bmatrix}\n",
      "\\dot{\\theta}\n",
      "$$\n",
      "\n",
      "In Python we can expand this:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "T = Matrix([[1, 0, -sin(theta)],\n",
      "            [0, cos(phi), cos(theta) * sin(phi)],\n",
      "            [0, -sin(phi), cos(theta) * cos(phi)]])\n",
      "T"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}1 & 0 & - \\sin{\\left (\\theta \\right )}\\\\0 & \\cos{\\left (\\phi \\right )} & \\sin{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )}\\\\0 & - \\sin{\\left (\\phi \\right )} & \\cos{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )}\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 7,
       "text": [
        "\u23a11     0        -sin(\u03b8)   \u23a4\n",
        "\u23a2                         \u23a5\n",
        "\u23a20  cos(\u03c6)   sin(\u03c6)\u22c5cos(\u03b8)\u23a5\n",
        "\u23a2                         \u23a5\n",
        "\u23a30  -sin(\u03c6)  cos(\u03c6)\u22c5cos(\u03b8)\u23a6"
       ]
      }
     ],
     "prompt_number": 7
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "omega = T * Theta_dot\n",
      "omega"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}\\dot{\\phi} - \\dot{\\psi} \\sin{\\left (\\theta \\right )}\\\\\\dot{\\psi} \\sin{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )} + \\dot{\\theta} \\cos{\\left (\\phi \\right )}\\\\\\dot{\\psi} \\cos{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )} - \\dot{\\theta} \\sin{\\left (\\phi \\right )}\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 8,
       "text": [
        "\u23a1        \\dot{\\phi} - \\dot{\\psi}\u22c5sin(\u03b8)        \u23a4\n",
        "\u23a2                                              \u23a5\n",
        "\u23a2\\dot{\\psi}\u22c5sin(\u03c6)\u22c5cos(\u03b8) + \\dot{\\theta}\u22c5cos(\u03c6)\u23a5\n",
        "\u23a2                                              \u23a5\n",
        "\u23a3\\dot{\\psi}\u22c5cos(\u03c6)\u22c5cos(\u03b8) - \\dot{\\theta}\u22c5sin(\u03c6)\u23a6"
       ]
      }
     ],
     "prompt_number": 8
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "In a similar way, the rotation of the body can be related to its equivalent in the inertial frame using a rotation matrix defined below."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "R = Matrix([[cos(phi) * cos(psi) - cos(theta) * sin(phi) * sin(psi), -cos(psi) * sin(phi) - cos(phi) * cos(theta) * sin(psi), sin(theta) * sin(psi)],\n",
      "            [cos(theta) * cos(psi) * sin(phi) + cos(phi) * sin(psi), cos(phi) * cos(theta) * cos(psi) - sin(phi) * sin(psi), -cos(psi) * sin(theta)],\n",
      "            [sin(phi) * sin(theta), cos(phi) * sin(theta), cos(theta)]])\n",
      "R"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}- \\sin{\\left (\\phi \\right )} \\sin{\\left (\\psi \\right )} \\cos{\\left (\\theta \\right )} + \\cos{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} & - \\sin{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} - \\sin{\\left (\\psi \\right )} \\cos{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )} & \\sin{\\left (\\psi \\right )} \\sin{\\left (\\theta \\right )}\\\\\\sin{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} \\cos{\\left (\\theta \\right )} + \\sin{\\left (\\psi \\right )} \\cos{\\left (\\phi \\right )} & - \\sin{\\left (\\phi \\right )} \\sin{\\left (\\psi \\right )} + \\cos{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} \\cos{\\left (\\theta \\right )} & - \\sin{\\left (\\theta \\right )} \\cos{\\left (\\psi \\right )}\\\\\\sin{\\left (\\phi \\right )} \\sin{\\left (\\theta \\right )} & \\sin{\\left (\\theta \\right )} \\cos{\\left (\\phi \\right )} & \\cos{\\left (\\theta \\right )}\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 9,
       "text": [
        "\u23a1-sin(\u03c6)\u22c5sin(\u03c8)\u22c5cos(\u03b8) + cos(\u03c6)\u22c5cos(\u03c8)  -sin(\u03c6)\u22c5cos(\u03c8) - sin(\u03c8)\u22c5cos(\u03c6)\u22c5cos(\u03b8) \n",
        "\u23a2                                                                             \n",
        "\u23a2sin(\u03c6)\u22c5cos(\u03c8)\u22c5cos(\u03b8) + sin(\u03c8)\u22c5cos(\u03c6)   -sin(\u03c6)\u22c5sin(\u03c8) + cos(\u03c6)\u22c5cos(\u03c8)\u22c5cos(\u03b8) \n",
        "\u23a2                                                                             \n",
        "\u23a3            sin(\u03c6)\u22c5sin(\u03b8)                          sin(\u03b8)\u22c5cos(\u03c6)             \n",
        "\n",
        " sin(\u03c8)\u22c5sin(\u03b8) \u23a4\n",
        "               \u23a5\n",
        " -sin(\u03b8)\u22c5cos(\u03c8)\u23a5\n",
        "               \u23a5\n",
        "     cos(\u03b8)    \u23a6"
       ]
      }
     ],
     "prompt_number": 9
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "This looks messy and I don't yet understand it all. It apparently uses Z-Y-Z Euler angle conventions, but I think I will have to learn how to derive it to fully understand it."
     ]
    },
    {
     "cell_type": "heading",
     "level": 3,
     "metadata": {},
     "source": [
      "Understanding Euler Angles"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "I found a resource that explains Euler angles in this sort of application: \n",
      "\n",
      "<http://www.chrobotics.com/library/understanding-euler-angles>\n",
      "\n",
      "It is probably easier to think of a rotation that goes from the inertial frame to the body frame. If the body is in an arbitrary orientation to the inertial frame, the inertial frame can be rotated about its axes until it is aligned with the body frame. So the first rotation could be a yawing about the Z axis. In the article referenced above this first rotation generates the \"vehicle-1 frame\" by using the following rotation matrix:\n",
      "\n",
      "$$\n",
      "R^{v1}_{I}\\left(\\psi\\right) = \\begin{bmatrix}\n",
      "\\cos \\psi & \\sin \\psi & 0 \\\\\n",
      "-\\sin \\psi & \\cos \\psi & 0 \\\\\n",
      "0 & 0 & 1\n",
      "\\end{bmatrix}\n",
      "$$\n",
      "\n",
      "I reckon I should be able to make sense of this; all the angles are $\\psi$, which makes sense as it is the only rotation that is applied at this stage. It is in 3D rather than 2D though.\n",
      "\n",
      "I think I'll go back to the 2D rotation matrix and work from there. [This PDF](files/rot2.pdf) will help (available at <https://engineering.purdue.edu/~bethel/rot2.pdf).>"
     ]
    },
    {
     "cell_type": "heading",
     "level": 4,
     "metadata": {},
     "source": [
      "2D Rotation"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "For a point $P$ in a coordinate sytem $x^\\prime, y^\\prime$ that is rotated by an angle $\\theta$ from the $x, y$ coordinate system, the coordinates are related by the following equations:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "x &= x^{\\prime} \\cos\\theta - y^{\\prime} \\sin\\theta \\\\\n",
      "y &= x^{\\prime} \\sin\\theta + y^{\\prime} \\cos\\theta\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "This is easily written in matrix form:\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "x \\\\\n",
      "y\n",
      "\\end{bmatrix}\n",
      "=\n",
      "\\begin{bmatrix}\n",
      "\\cos\\theta & -\\sin\\theta \\\\\n",
      "\\sin\\theta & \\cos\\theta\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "x^{\\prime} \\\\\n",
      "y^{\\prime}\n",
      "\\end{bmatrix}\n",
      "$$"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "x, y, z, x_prime, y_prime, z_prime, theta, psi, phi = symbols(\"x, y, z, x^\\prime, y^\\prime, z^\\prime, theta, psi, phi\")\n",
      "X = Matrix([x, y])\n",
      "X_prime = Matrix([x_prime, y_prime])\n",
      "R = Matrix([[cos(theta), -sin(theta)],[sin(theta), cos(theta)]])\n",
      "R"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}\\cos{\\left (\\theta \\right )} & - \\sin{\\left (\\theta \\right )}\\\\\\sin{\\left (\\theta \\right )} & \\cos{\\left (\\theta \\right )}\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 10,
       "text": [
        "\u23a1cos(\u03b8)  -sin(\u03b8)\u23a4\n",
        "\u23a2               \u23a5\n",
        "\u23a3sin(\u03b8)  cos(\u03b8) \u23a6"
       ]
      }
     ],
     "prompt_number": 10
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The equation can be rearranged using the inverse of the matrix (which in this case is equal to its transpose):\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "x^{\\prime} \\\\\n",
      "y^{\\prime} \\\\\n",
      "\\end{bmatrix}\n",
      "=\\begin{bmatrix}\n",
      "\\cos \\theta & \\sin \\theta \\\\\n",
      "-\\sin \\theta & \\cos \\theta\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "x \\\\\n",
      "y\n",
      "\\end{bmatrix}\n",
      "$$\n"
     ]
    },
    {
     "cell_type": "heading",
     "level": 4,
     "metadata": {},
     "source": [
      "3D Rotation"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "We can add the $z$ axis without any additional work. Note the change from $\\theta$ to $\\psi$; this is to maintain the convention for aircraft roll, pitch and yaw symbols:\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "x^{\\prime} \\\\\n",
      "y^{\\prime} \\\\\n",
      "z^{\\prime} \\\\\n",
      "\\end{bmatrix}\n",
      "= \\begin{bmatrix}\n",
      "\\cos \\psi & \\sin \\psi & 0 \\\\\n",
      "-\\sin \\psi & \\cos \\psi & 0 \\\\\n",
      "0 & 0 & 1\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "x \\\\\n",
      "y \\\\\n",
      "z\n",
      "\\end{bmatrix}\n",
      "$$\n",
      "\n",
      "Rotating first about the $z$ axis is just about the same as the 2D rotation. Note that when the multiplication is done, $z$ remains unchanged.\n",
      "\n",
      "This rotation matrix can be represented as `R_psi` and used to find the result of the single axis rotation:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "X = Matrix([x, y, z])\n",
      "X_prime = Matrix([x_prime, y_prime, z_prime])\n",
      "R_psi = Matrix([[cos(psi), sin(psi), 0],[-sin(psi), cos(psi), 0], [0, 0, 1]])\n",
      "R_psi"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}\\cos{\\left (\\psi \\right )} & \\sin{\\left (\\psi \\right )} & 0\\\\- \\sin{\\left (\\psi \\right )} & \\cos{\\left (\\psi \\right )} & 0\\\\0 & 0 & 1\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 11,
       "text": [
        "\u23a1cos(\u03c8)   sin(\u03c8)  0\u23a4\n",
        "\u23a2                  \u23a5\n",
        "\u23a2-sin(\u03c8)  cos(\u03c8)  0\u23a5\n",
        "\u23a2                  \u23a5\n",
        "\u23a3   0       0     1\u23a6"
       ]
      }
     ],
     "prompt_number": 11
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "R_psi * X"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}x \\cos{\\left (\\psi \\right )} + y \\sin{\\left (\\psi \\right )}\\\\- x \\sin{\\left (\\psi \\right )} + y \\cos{\\left (\\psi \\right )}\\\\z\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 12,
       "text": [
        "\u23a1x\u22c5cos(\u03c8) + y\u22c5sin(\u03c8) \u23a4\n",
        "\u23a2                    \u23a5\n",
        "\u23a2-x\u22c5sin(\u03c8) + y\u22c5cos(\u03c8)\u23a5\n",
        "\u23a2                    \u23a5\n",
        "\u23a3         z          \u23a6"
       ]
      }
     ],
     "prompt_number": 12
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Next we can rotate about the $x$ axis (which we'll call a rotation by $\\phi$). Without deriving anything further, we can substitute the $x$ and $y$ axes of the 2D rotation for the $y$ and $z$ axes of the 3D rotation respectively. This leaves the $x$ axis (the axis about which we are rotating) in the place of the $z$ axis in our first 3D rotation above.\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "y^{\\prime} \\\\\n",
      "z^{\\prime} \\\\\n",
      "x^{\\prime} \\\\\n",
      "\\end{bmatrix}\n",
      "= \\begin{bmatrix}\n",
      "\\cos \\phi & \\sin \\phi & 0 \\\\\n",
      "-\\sin \\phi & \\cos \\phi & 0 \\\\\n",
      "0 & 0 & 1\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "y \\\\\n",
      "z \\\\\n",
      "x\n",
      "\\end{bmatrix}\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The right hand side of the equation can be rearranged so that it resembles the original $X$ vector. To do this we need to shift the first two elements down by one and the last element up by two. The adjacent matrix is adjusted by moving the first two columns to the right by one, and the last column to the left by two:\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "y^{\\prime} \\\\\n",
      "z^{\\prime} \\\\\n",
      "x^{\\prime} \\\\\n",
      "\\end{bmatrix}\n",
      "= \\begin{bmatrix}\n",
      "0 & \\cos \\phi  & \\sin \\phi \\\\\n",
      "0 & -\\sin \\phi & \\cos \\phi \\\\\n",
      "1 & 0 & 0\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "x \\\\\n",
      "y \\\\\n",
      "z\n",
      "\\end{bmatrix}\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Now we need to rearrange the left hand side. The first two elements of the vector need to move down by one and the last element needs to move up by two. This corresponds to moving the matrix rows the same way:\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "x^{\\prime} \\\\\n",
      "y^{\\prime} \\\\\n",
      "z^{\\prime} \\\\\n",
      "\\end{bmatrix}\n",
      "= \\begin{bmatrix}\n",
      "1 & 0 & 0 \\\\\n",
      "0 & \\cos \\phi  & \\sin \\phi \\\\\n",
      "0 & -\\sin \\phi & \\cos \\phi\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "x \\\\\n",
      "y \\\\\n",
      "z\n",
      "\\end{bmatrix}\n",
      "$$\n",
      "\n",
      "Let's define this rotation matrix as `R_phi`:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "R_phi = Matrix([[1, 0, 0], [0, cos(phi), sin(phi)], [0, -sin(phi), cos(phi)]])\n",
      "R_phi"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}1 & 0 & 0\\\\0 & \\cos{\\left (\\phi \\right )} & \\sin{\\left (\\phi \\right )}\\\\0 & - \\sin{\\left (\\phi \\right )} & \\cos{\\left (\\phi \\right )}\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 13,
       "text": [
        "\u23a11     0       0   \u23a4\n",
        "\u23a2                  \u23a5\n",
        "\u23a20  cos(\u03c6)   sin(\u03c6)\u23a5\n",
        "\u23a2                  \u23a5\n",
        "\u23a30  -sin(\u03c6)  cos(\u03c6)\u23a6"
       ]
      }
     ],
     "prompt_number": 13
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "We have currently rotated around the $z$ and $x$ axes, leaving only the $y$ axis to do. We'll use $\\theta$ for this rotation. As before, we substitute the $y$ axis with our initial $z$ axis, likewise for $z$ we substitute $x$ and for $x$ we substitute $y$:\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "z^{\\prime} \\\\\n",
      "x^{\\prime} \\\\\n",
      "y^{\\prime} \\\\\n",
      "\\end{bmatrix}\n",
      "= \\begin{bmatrix}\n",
      "\\cos \\theta & \\sin \\theta & 0 \\\\\n",
      "-\\sin \\theta & \\cos \\theta & 0 \\\\\n",
      "0 & 0 & 1\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "z \\\\\n",
      "x \\\\\n",
      "y\n",
      "\\end{bmatrix}\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Rearranging the right hand side in the same way as above yields:\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "z^{\\prime} \\\\\n",
      "x^{\\prime} \\\\\n",
      "y^{\\prime} \\\\\n",
      "\\end{bmatrix}\n",
      "= \\begin{bmatrix}\n",
      "\\sin \\theta & 0 & \\cos \\theta \\\\\n",
      "\\cos \\theta & 0 & -\\sin \\theta \\\\\n",
      "0 & 1 & 0 \n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "x \\\\\n",
      "y \\\\\n",
      "z\n",
      "\\end{bmatrix}\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Rearranging the left hand side:\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "x^{\\prime} \\\\\n",
      "y^{\\prime} \\\\\n",
      "z^{\\prime} \\\\\n",
      "\\end{bmatrix}\n",
      "= \\begin{bmatrix}\n",
      "\\cos \\theta & 0 & -\\sin \\theta \\\\\n",
      "0 & 1 & 0 \\\\\n",
      "\\sin \\theta & 0 & \\cos \\theta\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "x \\\\\n",
      "y \\\\\n",
      "z\n",
      "\\end{bmatrix}\n",
      "$$\n",
      "\n",
      "Again, a variable can be defined called `R_theta`:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "R_theta = Matrix([[cos(theta), 0, -sin(theta)], [0, 1, 0], [sin(theta), 0, cos(theta)]])\n",
      "R_theta"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}\\cos{\\left (\\theta \\right )} & 0 & - \\sin{\\left (\\theta \\right )}\\\\0 & 1 & 0\\\\\\sin{\\left (\\theta \\right )} & 0 & \\cos{\\left (\\theta \\right )}\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 14,
       "text": [
        "\u23a1cos(\u03b8)  0  -sin(\u03b8)\u23a4\n",
        "\u23a2                  \u23a5\n",
        "\u23a2  0     1     0   \u23a5\n",
        "\u23a2                  \u23a5\n",
        "\u23a3sin(\u03b8)  0  cos(\u03b8) \u23a6"
       ]
      }
     ],
     "prompt_number": 14
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Now we can apply a series of three rotations in any order. The PDF I followed for this rotates around $x$ first, then $y$ and $z$, or $\\phi$, $\\theta$ and $\\psi$ using our defined angles. This can be applied as a single matrix (note that the right-most matrix is applied first):"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "M = R_psi * R_theta * R_phi\n",
      "M"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left[\\begin{smallmatrix}{}\\cos{\\left (\\psi \\right )} \\cos{\\left (\\theta \\right )} & \\sin{\\left (\\phi \\right )} \\sin{\\left (\\theta \\right )} \\cos{\\left (\\psi \\right )} + \\sin{\\left (\\psi \\right )} \\cos{\\left (\\phi \\right )} & \\sin{\\left (\\phi \\right )} \\sin{\\left (\\psi \\right )} - \\sin{\\left (\\theta \\right )} \\cos{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )}\\\\- \\sin{\\left (\\psi \\right )} \\cos{\\left (\\theta \\right )} & - \\sin{\\left (\\phi \\right )} \\sin{\\left (\\psi \\right )} \\sin{\\left (\\theta \\right )} + \\cos{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} & \\sin{\\left (\\phi \\right )} \\cos{\\left (\\psi \\right )} + \\sin{\\left (\\psi \\right )} \\sin{\\left (\\theta \\right )} \\cos{\\left (\\phi \\right )}\\\\\\sin{\\left (\\theta \\right )} & - \\sin{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )} & \\cos{\\left (\\phi \\right )} \\cos{\\left (\\theta \\right )}\\end{smallmatrix}\\right]$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 15,
       "text": [
        "\u23a1cos(\u03c8)\u22c5cos(\u03b8)   sin(\u03c6)\u22c5sin(\u03b8)\u22c5cos(\u03c8) + sin(\u03c8)\u22c5cos(\u03c6)   sin(\u03c6)\u22c5sin(\u03c8) - sin(\u03b8)\n",
        "\u23a2                                                                             \n",
        "\u23a2-sin(\u03c8)\u22c5cos(\u03b8)  -sin(\u03c6)\u22c5sin(\u03c8)\u22c5sin(\u03b8) + cos(\u03c6)\u22c5cos(\u03c8)  sin(\u03c6)\u22c5cos(\u03c8) + sin(\u03c8)\n",
        "\u23a2                                                                             \n",
        "\u23a3    sin(\u03b8)                 -sin(\u03c6)\u22c5cos(\u03b8)                         cos(\u03c6)\u22c5cos(\n",
        "\n",
        "\u22c5cos(\u03c6)\u22c5cos(\u03c8)\u23a4\n",
        "              \u23a5\n",
        "\u22c5sin(\u03b8)\u22c5cos(\u03c6)\u23a5\n",
        "              \u23a5\n",
        "\u03b8)            \u23a6"
       ]
      }
     ],
     "prompt_number": 15
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Note that this is not the same as the matrix presented above, which may have been produced using a different order of rotation, but as of now I am unsure."
     ]
    },
    {
     "cell_type": "heading",
     "level": 2,
     "metadata": {},
     "source": [
      "Physics"
     ]
    },
    {
     "cell_type": "heading",
     "level": 3,
     "metadata": {},
     "source": [
      "Motors"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Torque in an electric motor is given by:\n",
      "\n",
      "$$\n",
      "\\tau = K_t \\left(I - I_0 \\right)\n",
      "$$\n",
      "\n",
      "where $\\tau$ is torque, $K_t$ is a torque proportionality constant, $I$ is input current and $I_0$ is the no-load current.\n",
      "\n",
      "Voltage drop across the motor, $V$, is defined as:\n",
      "\n",
      "$$\n",
      "V = IR_m + K_v \\omega\n",
      "$$\n",
      "\n",
      "$R_m$ is the motor resistance, $K_v$ is a proportionality constant (back-EMF generated per revolution) and $\\omega$ is the motor angular velocity.\n",
      "\n",
      "Power is simply $P = IR$, but let's use SymPy to do the algebra for us:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "tau, Kt, I, I0, V, Rm, Kv, omega_m = symbols(\"tau, K_t, I, I_0, V, R_m, K_v, omega\")"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 16
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "First, our equation for $V$:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "eq_V = Eq(V, I * Rm + Kv * omega_m)\n",
      "eq_V"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$V = I R_{m} + K_{v} \\omega$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 17,
       "text": [
        "V = I\u22c5R_m + K\u1d65\u22c5\u03c9"
       ]
      }
     ],
     "prompt_number": 17
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "And now for $\\tau$:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "eq_tau = Eq(tau, Kt * (I - I0))\n",
      "eq_tau"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\tau = K_{t} \\left(I - I_{0}\\right)$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 18,
       "text": [
        "\u03c4 = K_t\u22c5(I - I\u2080)"
       ]
      }
     ],
     "prompt_number": 18
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "which we can rearrange in terms of $I$:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "eq_I = Eq(I, solve(eq_tau, I)[0])\n",
      "eq_I"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$I = I_{0} + \\frac{\\tau}{K_{t}}$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 19,
       "text": [
        "          \u03c4 \n",
        "I = I\u2080 + \u2500\u2500\u2500\n",
        "         K_t"
       ]
      }
     ],
     "prompt_number": 19
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "And now we can write our power equation:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "P = eq_V.rhs * eq_I.rhs\n",
      "P"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\left(I_{0} + \\frac{\\tau}{K_{t}}\\right) \\left(I R_{m} + K_{v} \\omega\\right)$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 20,
       "text": [
        "\u239b      \u03c4 \u239e               \n",
        "\u239cI\u2080 + \u2500\u2500\u2500\u239f\u22c5(I\u22c5R_m + K\u1d65\u22c5\u03c9)\n",
        "\u239d     K_t\u23a0               "
       ]
      }
     ],
     "prompt_number": 20
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "P.simplify()"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\frac{1}{K_{t}} \\left(I R_{m} + K_{v} \\omega\\right) \\left(I_{0} K_{t} + \\tau\\right)$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 21,
       "text": [
        "(I\u22c5R_m + K\u1d65\u22c5\u03c9)\u22c5(I\u2080\u22c5K_t + \u03c4)\n",
        "\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\n",
        "            K_t            "
       ]
      }
     ],
     "prompt_number": 21
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "If there is negligible motor resistance, the expression can be simplified:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "P = 1 / Kt * (Kv * omega_m) * (I0 * Kt + tau)\n",
      "P"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\frac{K_{v} \\omega}{K_{t}} \\left(I_{0} K_{t} + \\tau\\right)$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 22,
       "text": [
        "K\u1d65\u22c5\u03c9\u22c5(I\u2080\u22c5K_t + \u03c4)\n",
        "\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\u2500\n",
        "       K_t       "
       ]
      }
     ],
     "prompt_number": 22
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Finally, it can be assumed that $K_t I_0 \\ll \\tau$:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "P = Kv / Kt * tau * omega_m\n",
      "P"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "latex": [
        "$$\\frac{K_{v} \\omega}{K_{t}} \\tau$$"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 23,
       "text": [
        "K\u1d65\u22c5\u03c9\u22c5\u03c4\n",
        "\u2500\u2500\u2500\u2500\u2500\u2500\n",
        " K_t  "
       ]
      }
     ],
     "prompt_number": 23
    },
    {
     "cell_type": "heading",
     "level": 3,
     "metadata": {},
     "source": [
      "Forces"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The basic equations of motion can be used to start with:\n",
      "\n",
      "* Conservation of energy\n",
      "* Conservation of mass\n",
      "* Conservation of momentum\n",
      "\n",
      "The energy used by the motor over a given period of time (i.e. its power, the rate of energy consumption, over a particular amount of time, which is equal to work done) is equal to the force generated on the propeller times multiplied by the distance moved by the air it displaces (also work):\n",
      "\n",
      "$$\n",
      "P\\ \\mathrm{d}t = F\\ \\mathrm{d}z\n",
      "$$\n",
      "\n",
      "If we rearrange the above, we can equate power to the product of the thrust of the propeller and the velocity of the air:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "P &= F\\frac{\\mathrm{d}z}{\\mathrm{d}t} \\\\\\\\\n",
      "P &= Tv_h\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "$v_h$ is used to represent the air velocity when the quadcopter is hovering or when vehicle speeds are suitably low. For completeness we also assume that the freestream velocity, $v_\\infty=0$, meaning that the body of air in which the quadcopter is hovering is still; i.e. there is no wind.\n",
      "\n",
      "Conservation of momentum can be used to relate propeller thrust to $v_h$. Assuming a closed system, the mass flow of air (i.e. kilograms of air per second) entering the propeller disk must equal the mass flow of air leaving it. The force exerted on the air to change its speed is:\n",
      "\n",
      "$$\n",
      "F = \\dot{m}\\left(v_h - v_\\infty\\right)\n",
      "$$\n",
      "\n",
      "Mass flow is determined by the area swept out by the propeller:\n",
      "\n",
      "$$\n",
      "\\dot{m} = \\rho A v\n",
      "$$\n",
      "\n",
      "Checking the dimensional equivalence of the units, this makes sense:\n",
      "\n",
      "$$\n",
      "\\frac{kg}{s} = \\frac{kg}{m^3}\\times m^2 \\times \\frac{m}{s}\n",
      "$$\n",
      "\n",
      "What is not clear, however, is which velocity should be used. Should it be the velocity of the propeller? Maybe NASA [has the answer](https://www.grc.nasa.gov/www/Wright/airplane/propth.html)."
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "It is suggested that $v$ above should be $v_p$, the propeller velocity. But what does that mean? It could either be the velocity of the rotating propeller itself, or the velocity of air through the propeller.\n",
      "\n",
      "Substituting the new equation for mass flow into the thrust equation yields:\n",
      "\n",
      "$$\n",
      "F = \\rho A v_p \\left(v_h - v_\\infty\\right)\n",
      "$$\n",
      "\n",
      "Bernoulli's equation can be applied to relate pressure and velocity entering the propeller and exiting it, but not through it, since it there that work is done to the air. The total pressure at entry and exit are defined below.\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "p_{t_\\infty} &= p_\\infty + \\frac{1}{2}\\rho v_\\infty ^2 \\\\\\\\\n",
      "p_{t_h} &= p_h + \\frac{1}{2}\\rho v_h^2\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "Static pressure is the same at entry and exit, so $p_\\infty = p_h$. The work done by the propeller disk results in a change in total pressure:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "\\Delta p &= p_{t_h} - p_{t_\\infty} \\\\\\\\\n",
      "\\Delta p &= \\left(p_h+ \\frac{1}{2}\\rho v_h^2\\right) - \\left(p_\\infty + \\frac{1}{2}\\rho v_\\infty ^2\\right) \\\\\\\\\n",
      "\\Delta p &= \\frac{1}{2} \\rho \\left( v_h^2 - v_\\infty^2\\right)\n",
      "\\end{align}\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The change in pressure exerts a force on the propeller, and this force is equal to the thrust generated:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "F &= \\Delta p A \\\\\n",
      "F &= \\frac{1}{2} \\rho A \\left( v_h^2 - v_\\infty^2\\right)\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "The two equations for thrust can now be combined and solved for $v_p$:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "\\frac{1}{2} \\rho A \\left( v_h^2 - v_\\infty^2\\right) &= \\rho A v_p \\left(v_h - v_\\infty\\right) \\\\\n",
      "\\frac{1}{2} \\left( v_h^2 - v_\\infty^2\\right) &= v_p \\left(v_h - v_\\infty\\right) \\\\\n",
      "v_p \\left(v_h - v_\\infty\\right) &= \\frac{1}{2} \\frac{\\left( v_h + v_\\infty \\right)\\left( v_h - v_\\infty \\right)}{\\left(v_h - v_\\infty\\right)} \\\\\n",
      "v_p &= \\frac{1}{2} \\left( v_h + v_\\infty\\right)\n",
      "\\end{align}\n",
      "$$\n"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "So far, we do not appear to be anywhere near the equation presented in our reference material:\n",
      "\n",
      "$$\n",
      "v_h = \\sqrt{\\frac{T}{2\\rho A}}\n",
      "$$\n",
      "\n",
      "However, if we assume that $v_\\infty$ is negligible, then we could try the following:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "F = T &= \\frac{1}{2}\\rho A \\left( v_h^2 - 0 \\right) \\\\\n",
      "2T &= \\rho A v_h^2 \\\\\n",
      "\\frac{2T}{\\rho A} &= v_h^2 \\\\\n",
      "v_h &= \\sqrt{\\frac{2T}{\\rho A}}\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "Similar to our reference material, but not the same. I think we'll use this for now and see what happens."
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Taking the simplified equation for power derived above:\n",
      "\n",
      "$$\n",
      "P = \\frac{K_v \\tau \\omega}{K_t}\n",
      "$$\n",
      "\n",
      "we can substitude the torque variable for thrust multiplied by a constant $K_\\tau$ that is determined by the configuration of the quadcopter:\n",
      "\n",
      "$$\n",
      "P = \\frac{K_v K_\\tau T \\omega}{K_t} = \\frac{T^\\frac{3}{2}} {\\sqrt{2\\rho A}}\n",
      "$$\n",
      "\n",
      "Well that's quite a jump isn't it! Let's go a bit more slowly and see what is happening. We know that torque is proportional to the *cross* product of force and radius ($\\tau = \\overrightarrow{r} \\times \\overrightarrow{F}$) and in the equation above we have declared that $\\tau = TK_\\tau$. However, the right hand side of that equation is a mystery. Let's do some algebra and see if that helps."
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "$$\n",
      "\\begin{align}\n",
      "P &= \\frac{T^\\frac{3}{2}}{\\sqrt{2\\rho A}} \\\\\n",
      "&= \\frac{T\\sqrt{T}}{\\sqrt{2\\rho A}} \\\\\n",
      "&= T \\sqrt{\\frac{T}{2\\rho A}}\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "Now at the beginning of this section we wrote the following equation:\n",
      "\n",
      "$$\n",
      "P = Tv_h\n",
      "$$\n",
      "\n",
      "and it can be seen that the two are equal if we use the original stated expression for $v_h$. If we use the expression that I derived, then the resulting equation would be:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "P &= Tv_h \\\\\n",
      "&= T \\sqrt{\\frac{2T}{\\rho A}} \\\\\n",
      "&= T^\\frac{3}{2}\\sqrt{\\frac{2}{\\rho A}} = \\frac{K_v K_\\tau T \\omega}{K_t}\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "Solving for $T$,\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "T \\frac{K_v K_\\tau \\omega}{K_t} &= T^\\frac{3}{2}\\sqrt{\\frac{2}{\\rho A}} \\\\\n",
      "\\frac{T}{T^{\\frac{3}{2}}} &= \\frac{K_t\\sqrt{\\frac{2}{\\rho A}}}{K_v K_\\tau \\omega} \\\\\n",
      "T^{-\\frac{1}{2}} &= \\frac{K_t\\sqrt{\\frac{2}{\\rho A}}}{K_v K_\\tau \\omega} \\\\\n",
      "\\sqrt{T} &= \\frac{K_v K_\\tau \\omega}{K_t\\sqrt{\\frac{2}{\\rho A}}} \\\\\n",
      "T &= \\left(\\frac{K_v K_\\tau \\omega}{K_t\\sqrt{\\frac{2}{\\rho A}}}\\right)^2 \\\\\n",
      "T &= \\left(\\frac{K_v K_\\tau \\sqrt{\\frac{\\rho A}{2}} \\omega}{K_t}\\right)^2 \\\\\n",
      "T &= \\left(\\frac{K_v K_\\tau \\sqrt{\\rho A}}{K_t\\sqrt{2}}\\omega\\right)^2 \\\\\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "And if we choose an appropriate constant $k$, \n",
      "\n",
      "$$\n",
      "T = k\\omega^2\n",
      "$$\n",
      "\n",
      "*Note the $\\sqrt{2}$ term in the denominator is the result of my derivation; in the reference material this term appears in the numerator.*"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "There are four motors on a quadcopter, and so the total thrust in the body frame needs to be found.\n",
      "\n",
      "$$\n",
      "T_B = \\sum^4_{i=1} T_i = k \n",
      "\\begin{bmatrix}\n",
      "0 \\\\\n",
      "0 \\\\\n",
      "\\sum \\omega_i^2\n",
      "\\end{bmatrix}\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The air exerts a friction force on the quadcopter. A simple assumption is that this force is proportional to each velocity component. The fluid friction results in the drag experienced by the quadcopter:\n",
      "\n",
      "$$\n",
      "F_D = \\begin{bmatrix}\n",
      "-k_d \\dot x \\\\\n",
      "-k_d \\dot y \\\\\n",
      "-k_d \\dot z\n",
      "\\end{bmatrix}\n",
      "$$\n",
      "\n",
      "Again, $k_d$ is a constant."
     ]
    },
    {
     "cell_type": "heading",
     "level": 3,
     "metadata": {},
     "source": [
      "Torque/Moments"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "With the forces acting on the quadcopter defined, we can take moments."
     ]
    },
    {
     "cell_type": "heading",
     "level": 4,
     "metadata": {},
     "source": [
      "Motor Torque"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Each motor turning a propeller generates a moment in the quadcopter body's $z$-axis. The reaction against this moment is required to maintain the rotation of the propeller as it overcomes friction and generates thrust. Since there is a force, there is also an acceleration, angular acceleration in this case.\n",
      "\n",
      "The force due to drag is:\n",
      "\n",
      "$$\n",
      "F_D = \\frac{1}{2}\\rho C_D A v^2\n",
      "$$\n",
      "\n",
      "In this case, $A$ is not the propeller's swept area. The reference material states that it is the cross-sectional area of the propeller, although in which axis I am not sure. If the propeller was treated as a wing, its reference area would be its plan area, i.e. looking down on the wing from above, or in this case the area of the propeller as viewed from the front. Hopefully I will be able to clarify this later. "
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The torque produced by the propeller due to drag can be calculated by remembering that $\\tau = Fr$ and $v=\\omega r$, where $r$ is a radius (or length of the lever arm):\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "\\tau_D &= \\frac{1}{2}\\rho C_D Av^2r \\\\\n",
      "&= \\frac{1}{2}\\rho C_D A \\left(\\omega r\\right)^2 r \\\\\n",
      "&= b\\omega^2\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "Again, $b$ is just an appropriately dimensioned constant."
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "For each motor, the total torque about the $z$-axis is defined below.\n",
      "\n",
      "$$\n",
      "\\tau_z = b\\omega^2 + I_M\\dot\\omega\n",
      "$$\n",
      "\n",
      "$I_M$ is the moment of inertia about the motor's $z$-axis and $\\dot\\omega$ is the angular acceleration of the propeller. When in steady flight we can assume that $\\dot \\omega = 0$ as each motor will be producing constant thrust. The expression for the $i$th motor is:\n",
      "\n",
      "$$\n",
      "\\tau_z = \\left(-1\\right)^{i+1}b\\omega_i^2\n",
      "$$\n",
      "\n",
      "where torque is negative for a counterclockwise-rotating propeller and positive for one spinning clockwise."
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Taking all four motors, the total torque becomes:\n",
      "\n",
      "$$\n",
      "\\tau_\\psi = b\\left(\\omega_1^2 - \\omega_2^2 + \\omega_3^2 - \\omega_4^2 \\right)\n",
      "$$"
     ]
    },
    {
     "cell_type": "heading",
     "level": 4,
     "metadata": {},
     "source": [
      "Roll and Pitch Moments"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "In the diagram at the top of this notebook, we have assumed that the quadcopter flies in \"plus\" configuration (rather than the more common \"cross\" configuration). This means that motors 1 and 3 lie on the roll axis and motors 2 and 4 lie on the pitch axis.\n",
      "\n",
      "The rolling moment can be derived simply as:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "\\tau_\\phi &= \\sum r \\times T \\\\\n",
      "&= lk\\omega_4^2 - lk\\omega_2^2 \\\\\n",
      "&= lk\\left(\\omega_4^2 - \\omega_2^2\\right)\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "where $l$ is the length of the lever arm, in this case the distance from the motor to the centre of the quadcopter, and $k$ is the constant applied to our derived thrust equation ($T = k\\omega^2$).\n",
      "\n",
      "Similarly for pitch:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "\\tau_\\theta &= lk\\left(\\omega_1^2 - \\omega_3^2\\right)\n",
      "\\end{align}\n",
      "$$"
     ]
    },
    {
     "cell_type": "heading",
     "level": 4,
     "metadata": {},
     "source": [
      "Total Body Moments"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "All torques or bending moments have now been defined:\n",
      "\n",
      "$$\n",
      "\\tau_B = \n",
      "\\begin{bmatrix}\n",
      "lk\\left(\\omega_4^2 - \\omega_2^2\\right) \\\\\n",
      "lk\\left(\\omega_1^2 - \\omega_3^2\\right) \\\\\n",
      "b\\left(\\omega_1^2 - \\omega_2^2 + \\omega_3^2 - \\omega_4^2 \\right)\n",
      "\\end{bmatrix}\n",
      "$$"
     ]
    },
    {
     "cell_type": "heading",
     "level": 3,
     "metadata": {},
     "source": [
      "Equations of Motion"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Previously we have been working in the body frame. If we now turn our attention to the inertial frame, the quadcopter experiences the following forces:\n",
      "\n",
      "* Thrust\n",
      "* Gravity\n",
      "* Drag\n",
      "\n",
      "Each force causes an acceleration on the quadcopter body. By using Newton's second law ($F=ma$) we can calculate the total acceleration of the quadcopter in the inertial frame. \n",
      "\n",
      "$$\n",
      "m\\ddot x = \\begin{bmatrix}\n",
      "0 \\\\\n",
      "0 \\\\\n",
      "mg\n",
      "\\end{bmatrix} + RT_B + F_D\n",
      "$$\n",
      "\n",
      "Note that the thrust vector is transformed from the body frame to the inertial frame by the rotation matrix $R$."
     ]
    },
    {
     "cell_type": "heading",
     "level": 4,
     "metadata": {},
     "source": [
      "Euler's Equations for Rigid Body Dynamics"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "[Euler's second law][1] states that the rate of change of angular momentum $\\mathbf L$ is equal to the sum of moments of force acting on the body:\n",
      "\n",
      "$$\n",
      "\\frac{\\mathrm{d}\\mathbf L}{\\mathrm{d}t} = \\mathbf M\n",
      "$$\n",
      "\n",
      "Angular momentum of a fixed mass rotating about a fixed symmetry axis is defined as:\n",
      "\n",
      "$$\n",
      "\\mathbf L = I\\omega\n",
      "$$\n",
      "\n",
      "where $I$ is the body's moment of inertia. In these equations we are dealing with the inertial frame of reference, but it is useful to convert to the body frame of reference.\n",
      "\n",
      "As [apparently defined][2], angular momentum can be written\n",
      "\n",
      "$$\n",
      "\\mathbf L \\equiv L_1 \\mathbf{e}_1 + L_2\\mathbf{e}_2 +L_3\\mathbf{e}_3 = I_1\\omega_1\\mathbf{e}_1 + I_2\\omega_2\\mathbf{e}_2 +I_3\\omega_3\\mathbf{e}_3\n",
      "$$\n",
      "\n",
      "where $\\mathbf{e}_n$ is a unit vector for each axis. There is more information on Wikipedia about [standard basis vectors][3]. The following is equivalent and makes more sense to me:\n",
      "\n",
      "$$\n",
      "\\mathbf L \\equiv L_x \\mathbf{e}_x + L_y\\mathbf{e}_y +L_z\\mathbf{e}_z = I_x\\omega_x\\mathbf{e}_x + I_y\\omega_y\\mathbf{e}_y +I_z\\omega_z\\mathbf{e}_z\n",
      "$$\n",
      "\n",
      "In the body frame of reference, we can define a new set of unit vectors $\\hat{\\boldsymbol{\\imath}}, \\hat{\\boldsymbol{\\jmath}}, \\hat{\\boldsymbol{k}}$. These vectors remain normalised during rotation of the body. If a unit vector $\\hat{\\boldsymbol{u}}$ rotates with  a speed $\\Omega$ about an axis $\\boldsymbol{\\Omega}$, the following equation will be true:\n",
      "\n",
      "$$\n",
      "\\frac{\\mathrm{d}}{\\mathrm{d}t} \\hat{\\boldsymbol{u}} = \\boldsymbol{\\Omega} \\times \\hat{\\boldsymbol{u}}\n",
      "$$\n",
      "\n",
      "What is this actually saying? The rate of change of the unit vector with respect to time is equal to the cross product of the axis and the unit vector. It represents the rate of rotation of the body axes with respect to the inertial axes. [This section][4] in Wikipedia provides more information, and it is also being developed in my reference notebook.\n",
      "\n",
      "More work should be done to understand unit vectors and their application once again.\n",
      "\n",
      "Now suppose there is a vector function $\\boldsymbol{f}$ such that\n",
      "\n",
      "$$\n",
      "\\boldsymbol{f} \\left(t\\right) = f_x \\left(t\\right)\\hat{\\boldsymbol{\\imath}} + f_y \\left(t\\right)\\hat{\\boldsymbol{\\jmath}} + f_z \\left(t\\right)\\hat{\\boldsymbol{k}}\n",
      "$$\n",
      "\n",
      "and we take the first derivative with respect to $t$, using the product rule and making use of $\\frac{\\mathrm{d}}{\\mathrm{d}t} \\hat{\\boldsymbol{u}} = \\boldsymbol{\\Omega} \\times \\hat{\\boldsymbol{u}}$:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "\\frac{\\mathrm{d}}{\\mathrm{d}t}\\boldsymbol{f} &= \\frac{\\mathrm{d}\\hat{\\boldsymbol{\\imath}}}{\\mathrm{d}t}f_x + \\frac{\\mathrm{d}f_x}{\\mathrm{d}t}\\hat{\\boldsymbol{\\imath}} + \\frac{\\mathrm{d}\\hat{\\boldsymbol{\\jmath}}}{\\mathrm{d}t}f_y + \\frac{\\mathrm{d}f_y}{\\mathrm{d}t}\\hat{\\boldsymbol{\\jmath}} + \\frac{\\mathrm{d}\\hat{\\boldsymbol{k}}}{\\mathrm{d}t}f_z + \\frac{\\mathrm{d}f_z}{\\mathrm{d}t}\\hat{\\boldsymbol{k}}\\\\\n",
      "&= \\frac{\\mathrm{d}f_x}{\\mathrm{d}t}\\hat{\\boldsymbol{\\imath}} + \n",
      "   \\frac{\\mathrm{d}f_y}{\\mathrm{d}t}\\hat{\\boldsymbol{\\jmath}} + \n",
      "   \\frac{\\mathrm{d}f_z}{\\mathrm{d}t}\\hat{\\boldsymbol{k}} + \\frac{\\mathrm{d}\\hat{\\boldsymbol{\\imath}}}{\\mathrm{d}t}f_x + \\frac{\\mathrm{d}\\hat{\\boldsymbol{\\jmath}}}{\\mathrm{d}t}f_y + \\frac{\\mathrm{d}\\hat{\\boldsymbol{k}}}{\\mathrm{d}t}f_z \\\\\n",
      "&= \\frac{\\mathrm{d}f_x}{\\mathrm{d}t}\\hat{\\boldsymbol{\\imath}} +\n",
      "   \\frac{\\mathrm{d}f_y}{\\mathrm{d}t}\\hat{\\boldsymbol{\\jmath}} +\n",
      "   \\frac{\\mathrm{d}f_z}{\\mathrm{d}t}\\hat{\\boldsymbol{k}} +\n",
      "\\left(\\boldsymbol{\\Omega} \\times \\hat{\\boldsymbol{\\imath}}f_x \\right) +\n",
      "\\left(\\boldsymbol{\\Omega} \\times \\hat{\\boldsymbol{\\jmath}}f_y \\right) +\n",
      "\\left(\\boldsymbol{\\Omega} \\times \\hat{\\boldsymbol{k}}f_z \\right) \\\\\n",
      "&= \\frac{\\mathrm{d}f_x}{\\mathrm{d}t}\\hat{\\boldsymbol{\\imath}} +\n",
      "   \\frac{\\mathrm{d}f_y}{\\mathrm{d}t}\\hat{\\boldsymbol{\\jmath}} +\n",
      "   \\frac{\\mathrm{d}f_z}{\\mathrm{d}t}\\hat{\\boldsymbol{k}} +\n",
      "\\left[\\boldsymbol{\\Omega} \\times \\left(f_x\\hat{\\boldsymbol{\\imath}} +                              f_y\\hat{\\boldsymbol{\\jmath}} +\n",
      "   f_z\\hat{\\boldsymbol{k}} \\right)\\right] \\\\\n",
      "&= \\left( \\frac{\\mathrm{d}\\boldsymbol{f}}{\\mathrm{d}t}\\right)_r + \\boldsymbol{\\Omega} \\times \\boldsymbol{f}\\left(t\\right)\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "[1]: http://en.wikipedia.org/wiki/Euler%27s_second_law\n",
      "[2]: http://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics)\n",
      "[3]: http://en.wikipedia.org/wiki/Standard_basis\n",
      "[4]: http://en.wikipedia.org/wiki/Rotating_reference_frame#Relating_rotating_frames_to_stationary_frames"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "We can take the result above and substitue the momentum vector and rotational velocity of the quadcopter body:\n",
      "\n",
      "$$\n",
      "\\left( \\frac{\\mathrm{d}\\mathbf{L}}{\\mathrm{d}t} \\right)_r + \\omega \\times \\mathbf{L} = \\mathbf{M}\n",
      "$$\n",
      "\n",
      "where the subscript $r$ indicates the change with respect to time of the momentum vector is taking place in the rotating (body) frame of reference.\n",
      "\n",
      "Eventually we need to get to here:\n",
      "\n",
      "$$\n",
      "\\mathbf M = \\mathbf I\\cdot\\dot\\omega + \\omega \\times \\left( \\mathbf I\\cdot \\omega \\right)\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Rearranged in terms of the angular velocity vector:\n",
      "\n",
      "$$\n",
      "\\dot \\omega = \\mathbf I^{-1} \\left(\\tau-\\omega \\times \\left(\\mathbf I \\cdot \\omega \\right)\\right)\n",
      "$$\n",
      "\n",
      "This can also be written as:\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "\\dot \\omega_x \\\\\n",
      "\\dot \\omega_y \\\\\n",
      "\\dot \\omega_z\n",
      "\\end{bmatrix}\n",
      "= \\mathbf I^{-1} \\left(\\tau-\\omega \\times \\left(\\mathbf I \\cdot \\omega \\right)\\right)\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The quadcopter can be modelled as two thin rods crossed at the body's origin, with point masses representing motors at the ends of each rod. This can apparently then imply that a diagonal inertia matrix exists of the form\n",
      "\n",
      "$$\n",
      "\\mathbf I = \\begin{bmatrix}\n",
      "I_{xx} & 0 & 0 \\\\\n",
      "0 & I_{yy} & 0 \\\\\n",
      "0 & 0 & I_{zz}\n",
      "\\end{bmatrix}\n",
      "$$\n",
      "\n",
      "*Note that further relearning about inertia is required and will result in further expansion here*\n",
      "\n",
      "We need the inverse of the matrix so let's use SymPy to help."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "Ixx, Iyy, Izz = symbols(\"I_{xx}, I_{yy}, I_{zz}\")"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 24
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "I = Matrix([[Ixx, 0, 0], [0, Iyy, 0], [0, 0, Izz]])\n",
      "print(I)\n",
      "I_inv = I.inv()\n",
      "print(I_inv)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "Matrix([[I_{xx}, 0, 0], [0, I_{yy}, 0], [0, 0, I_{zz}]])\n",
        "Matrix([[1/I_{xx}, 0, 0], [0, 1/I_{yy}, 0], [0, 0, 1/I_{zz}]])\n"
       ]
      }
     ],
     "prompt_number": 27
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "So the inverse $\\mathbf I$ matrix is\n",
      "\n",
      "$$\n",
      "\\begin{bmatrix}\n",
      "\\frac{1}{I_{xx}} & 0 & 0 \\\\\n",
      "0 & \\frac{1}{I_{yy}} & 0 \\\\\n",
      "0 & 0 & \\frac{1}{I_{zz}}\n",
      "\\end{bmatrix}\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "And we can now substitute $\\mathbf I$ into our rotational equation of motion:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "\\dot \\omega &= \\mathbf I^{-1} \\left(\\tau-\\omega \\times \\left(\\mathbf I \\cdot \\omega \\right)\\right) \\\\\n",
      "\\dot \\omega &= \\begin{bmatrix}\n",
      "\\frac{1}{I_{xx}} & 0 & 0 \\\\\n",
      "0 & \\frac{1}{I_{yy}} & 0 \\\\\n",
      "0 & 0 & \\frac{1}{I_{zz}}\n",
      "\\end{bmatrix}\n",
      "\\left(\n",
      "\\begin{bmatrix}\n",
      "\\tau_{\\phi} \\\\\n",
      "\\tau_{\\theta} \\\\\n",
      "\\tau_{\\psi}\n",
      "\\end{bmatrix}\n",
      "- \\begin{bmatrix}\n",
      "\\omega_x \\\\\n",
      "\\omega_y \\\\\n",
      "\\omega_z\n",
      "\\end{bmatrix}\n",
      "\\times \\left(\\begin{bmatrix}\n",
      "I_{xx} & 0 & 0 \\\\\n",
      "0 & I_{yy} & 0 \\\\\n",
      "0 & 0 & I_{zz}\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "\\omega_x \\\\\n",
      "\\omega_y \\\\\n",
      "\\omega_z\n",
      "\\end{bmatrix}\n",
      "\\right)\\right) \\\\\n",
      "&= \\begin{bmatrix}\n",
      "\\frac{1}{I_{xx}} & 0 & 0 \\\\\n",
      "0 & \\frac{1}{I_{yy}} & 0 \\\\\n",
      "0 & 0 & \\frac{1}{I_{zz}}\n",
      "\\end{bmatrix}\n",
      "\\left(\n",
      "\\begin{bmatrix}\n",
      "\\tau_{\\phi} \\\\\n",
      "\\tau_{\\theta} \\\\\n",
      "\\tau_{\\psi}\n",
      "\\end{bmatrix}\n",
      "- \\begin{bmatrix}\n",
      "\\omega_x \\\\\n",
      "\\omega_y \\\\\n",
      "\\omega_z\n",
      "\\end{bmatrix}\n",
      "\\times \\begin{bmatrix}\n",
      "I_{xx}\\omega_x \\\\\n",
      "I_{yy}\\omega_y \\\\\n",
      "I_{zz}\\omega_z\n",
      "\\end{bmatrix}\n",
      "\\right) \\\\\n",
      "&= \\begin{bmatrix}\n",
      "\\frac{1}{I_{xx}} & 0 & 0 \\\\\n",
      "0 & \\frac{1}{I_{yy}} & 0 \\\\\n",
      "0 & 0 & \\frac{1}{I_{zz}}\n",
      "\\end{bmatrix}\n",
      "\\left(\n",
      "\\begin{bmatrix}\n",
      "\\tau_{\\phi} \\\\\n",
      "\\tau_{\\theta} \\\\\n",
      "\\tau_{\\psi}\n",
      "\\end{bmatrix}\n",
      "- \\begin{bmatrix}\n",
      "I_{zz}\\omega_y\\omega_z - I_{yy}\\omega_y\\omega_z \\\\\n",
      "I_{xx}\\omega_x\\omega_z - I_{zz}\\omega_x\\omega_z \\\\\n",
      "I_{yy}\\omega_x\\omega_y - I_{xx}\\omega_x\\omega_y\n",
      "\\end{bmatrix}\n",
      "\\right) \\\\\n",
      "&= \\begin{bmatrix}\n",
      "\\frac{1}{I_{xx}} & 0 & 0 \\\\\n",
      "0 & \\frac{1}{I_{yy}} & 0 \\\\\n",
      "0 & 0 & \\frac{1}{I_{zz}}\n",
      "\\end{bmatrix}\n",
      "\\begin{bmatrix}\n",
      "\\tau_\\phi - I_{zz}\\omega_y\\omega_z + I_{yy}\\omega_y\\omega_z \\\\\n",
      "\\tau_\\theta - I_{xx}\\omega_x\\omega_z + I_{zz}\\omega_x\\omega_z \\\\\n",
      "\\tau_\\psi - I_{yy}\\omega_x\\omega_y + I_{xx}\\omega_x\\omega_y\n",
      "\\end{bmatrix}\\\\\n",
      "&= \\begin{bmatrix}\n",
      "\\frac{\\tau_\\phi}{I_{xx}} - \\frac{I_{zz}\\omega_y\\omega_z}{I_{xx}} + \\frac{I_{yy}\\omega_y\\omega_z}{I_{xx}} \\\\\n",
      "\\frac{\\tau_\\theta}{I_{yy}} - \\frac{I_{xx}\\omega_x\\omega_z}{I_{yy}} + \\frac{I_{zz}\\omega_x\\omega_z}{I_{yy}} \\\\\n",
      "\\frac{\\tau_\\psi}{I_{zz}} - \\frac{I_{yy}\\omega_x\\omega_y}{I_{zz}} + \\frac{I_{xx}\\omega_x\\omega_y}{I_{zz}}\n",
      "\\end{bmatrix}\n",
      "\\end{align}\n",
      "$$"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "This matrix can be simplified a bit as follows:\n",
      "\n",
      "$$\n",
      "\\begin{align}\n",
      "\\dot{\\omega} &= \\begin{bmatrix}\n",
      "\\frac{\\tau_\\phi}{I_{xx}} + \\frac{I_{yy} - I_{zz}}{I_{xx}}\\omega_y\\omega_z\\\\\n",
      "\\frac{\\tau_\\theta}{I_{yy}} + \\frac{I_{zz} - I_{xx}}{I_{yy}}\\omega_x\\omega_z \\\\\n",
      "\\frac{\\tau_\\psi}{I_{zz}} + \\frac{I_{xx} - I_{yy}}{I_{zz}}\\omega_x\\omega_y\n",
      "\\end{bmatrix}\n",
      "\\end{align}\n",
      "$$\n",
      "\n",
      "*The signs of the inertial terms should be checked as they appear to be incorrect.*"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Should torque be in terms of $\\phi$, $\\theta$ and $\\psi$ and angular velocity be in terms of $x$, $y$ and $z$? I don't know yet."
     ]
    },
    {
     "cell_type": "heading",
     "level": 2,
     "metadata": {},
     "source": [
      "Simulation"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "With the equations of motion defined, let's try to simulate the system. The reference material used MATLAB, but we will try using Python and the control systems library.\n",
      "\n",
      "This is supposed to use [Euler's method](http://en.wikipedia.org/wiki/Euler_method), so perhaps studying that will help explain what is going on."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# Simulation time in seconds.\n",
      "start_time = 0\n",
      "end_time = 10\n",
      "step = 0.005\n",
      "# Generate a NumPy array. A list could be used instead, but generating \n",
      "# using range does not permit floats. An alternative could be used: \n",
      "# [i * step for i in range(int(start_time / 0.005), int(end_time / 0.005))]\n",
      "times = arange(start_time, end_time, step)\n",
      "\n",
      "# Number of points in the simulation.\n",
      "n = len(times)\n",
      "\n",
      "# Initial simulation state.\n",
      "x = array([[0], [0], [10]])\n",
      "xdot = zeros((3, 1))\n",
      "theta = zeros((3, 1))"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 1
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# Simulate a disturbance in the angular velocity. \n",
      "#\u00a0Magnitude is in radians per second.\n",
      "deviation = 100\n",
      "thetadot = deg2rad(2 * deviation * rand(3, 1) - deviation)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 2
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# Step through the simulation, updating the state.\n",
      "for t in times:\n",
      "    # Take input from our controller. Assuming the MATLAB function is\n",
      "    # similar to the Python one, why would we want a user input for each \n",
      "    # iteration?\n",
      "    i = input(t)\n",
      "    \n",
      "    # Undefined functions.\n",
      "    omega = thetadot2omega(thetadot, theta)\n",
      "    a = acceleration(i, theta, xdot, m, g, k, kd)\n",
      "    omegadot = angular_acceleration(i, omega, I, L, b, k)\n",
      "    \n",
      "    omega = omega + dt * omegadot\n",
      "    thetadot = omega2thetadot(omega, theta)  # Undefined.\n",
      "    theta = theta + dt * thetadot\n",
      "    xdot = xdot + dt * a\n",
      "    x = x + dt * xdot"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "ename": "KeyboardInterrupt",
       "evalue": "",
       "output_type": "pyerr",
       "traceback": [
        "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m\n\u001b[1;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
        "\u001b[1;32m<ipython-input-6-1fa36e9d7573>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[0;32m      4\u001b[0m     \u001b[1;31m# similar to the Python one, why would we want a user input for each\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m     \u001b[1;31m# iteration?\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 6\u001b[1;33m     \u001b[0mi\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0minput\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mt\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      7\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      8\u001b[0m     \u001b[1;31m# Undefined functions.\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
        "\u001b[1;32m/home/paul/.virtualenvs/general/lib/python3.3/site-packages/IPython/kernel/zmq/ipkernel.py\u001b[0m in \u001b[0;36m<lambda>\u001b[1;34m(prompt)\u001b[0m\n\u001b[0;32m    352\u001b[0m         \u001b[1;31m# raw_input in the user namespace.\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    353\u001b[0m         \u001b[1;32mif\u001b[0m \u001b[0mcontent\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mget\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'allow_stdin'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;32mFalse\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m--> 354\u001b[1;33m             \u001b[0mraw_input\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;32mlambda\u001b[0m \u001b[0mprompt\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;34m''\u001b[0m\u001b[1;33m:\u001b[0m \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m_raw_input\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mprompt\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mident\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mparent\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    355\u001b[0m         \u001b[1;32melse\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    356\u001b[0m             \u001b[0mraw_input\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;32mlambda\u001b[0m \u001b[0mprompt\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;34m''\u001b[0m \u001b[1;33m:\u001b[0m \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m_no_raw_input\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
        "\u001b[1;32m/home/paul/.virtualenvs/general/lib/python3.3/site-packages/IPython/kernel/zmq/ipkernel.py\u001b[0m in \u001b[0;36m_raw_input\u001b[1;34m(self, prompt, ident, parent)\u001b[0m\n\u001b[0;32m    769\u001b[0m             \u001b[1;32mexcept\u001b[0m \u001b[0mKeyboardInterrupt\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    770\u001b[0m                 \u001b[1;31m# re-raise KeyboardInterrupt, to truncate traceback\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m--> 771\u001b[1;33m                 \u001b[1;32mraise\u001b[0m \u001b[0mKeyboardInterrupt\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    772\u001b[0m             \u001b[1;32melse\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    773\u001b[0m                 \u001b[1;32mbreak\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
        "\u001b[1;31mKeyboardInterrupt\u001b[0m: "
       ]
      }
     ],
     "prompt_number": 6
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "from IPython.core.display import HTML\n",
      "def css_styling():\n",
      "    styles = open(\"styles/custom.css\", \"r\").read()\n",
      "    return HTML(styles)\n",
      "css_styling()"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "html": [
        "<style>\n",
        "    @font-face {\n",
        "        font-family: \"Computer Modern\";\n",
        "        src: url('http://mirrors.ctan.org/fonts/cm-unicode/fonts/otf/cmunss.otf');\n",
        "    }\n",
        "    div.cell{\n",
        "        width:800px;\n",
        "        margin-left:16% !important;\n",
        "        margin-right:auto;\n",
        "    }\n",
        "    h1 {\n",
        "        font-family: Helvetica, serif;\n",
        "    }\n",
        "    h2 {\n",
        "        font-family: Helvetica, serif;\n",
        "    }\n",
        "    h4{\n",
        "        margin-top:12px;\n",
        "        margin-bottom: 3px;\n",
        "       }\n",
        "    div.text_cell_render{\n",
        "        font-family: \"Open Sans\", Computer Modern, \"Helvetica Neue\", Arial, Helvetica, Geneva, sans-serif;\n",
        "        line-height: 135%;\n",
        "        font-size: 120%;\n",
        "        width:600px;\n",
        "        margin-left:auto;\n",
        "        margin-right:auto;\n",
        "    }\n",
        "    .CodeMirror{\n",
        "            font-family: \"Source Code Pro\", \"Ubuntu Mono\", source-code-pro,Consolas, monospace;\n",
        "    }\n",
        "/*    .prompt{\n",
        "        display: None;\n",
        "    }*/\n",
        "    .text_cell_render h5 {\n",
        "        font-weight: 300;\n",
        "        font-size: 16pt;\n",
        "        color: #4057A1;\n",
        "        font-style: italic;\n",
        "        margin-bottom: .5em;\n",
        "        margin-top: 0.5em;\n",
        "        display: block;\n",
        "    }\n",
        "\n",
        "    .warning{\n",
        "        color: rgb( 240, 20, 20 )\n",
        "        }\n",
        "</style>\n",
        "<script>\n",
        "    MathJax.Hub.Config({\n",
        "                        TeX: {\n",
        "                           extensions: [\"AMSmath.js\"]\n",
        "                           },\n",
        "                tex2jax: {\n",
        "                    inlineMath: [ ['$','$'], [\"\\\\(\",\"\\\\)\"] ],\n",
        "                    displayMath: [ ['$$','$$'], [\"\\\\[\",\"\\\\]\"] ]\n",
        "                },\n",
        "                displayAlign: 'center', // Change this to 'center' to center equations.\n",
        "                \"HTML-CSS\": {\n",
        "                    styles: {'.MathJax_Display': {\"margin\": 4}}\n",
        "                }\n",
        "        });\n",
        "</script>"
       ],
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 3,
       "text": [
        "<IPython.core.display.HTML at 0x30d3530>"
       ]
      }
     ],
     "prompt_number": 3
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [],
     "language": "python",
     "metadata": {},
     "outputs": []
    }
   ],
   "metadata": {}
  }
 ]
}