Started section on Runge Kutta.

Mostly a placeholder for now. There is also some experimental code
for quaternions, which I will be needing to introduce later.

experiements/quaternion.py

@@ -0,0 +1,76 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Thu Jan 29 18:36:36 2015
+
+@author: rlabbe
+"""
+
+import numpy as np
+from math import sin, cos, atan2, asin
+
+'''
+psi = yaw
+phi = roll
+theta = pitch
+'''
+
+
+def e2q(vector):
+    
+    roll    = vector[0]
+    pitch   = vector[1]
+    heading = vector[2]
+    sinhdg = sin(heading/2)
+    coshdg = cos(heading/2)
+    
+    sinroll = sin(roll/2)
+    cosroll = cos(roll/2)
+    
+    sinpitch = sin(pitch/2)
+    cospitch = cos(pitch/2)
+    
+    q0 = cosroll*cospitch*coshdg + sinroll*sinpitch*sinhdg
+    q1 = sinroll*cospitch*coshdg - cosroll*sinpitch*sinhdg
+    q2 = cosroll*sinpitch*coshdg + sinroll*cospitch*sinhdg
+    q3 = cosroll*cospitch*sinhdg - sinroll*sinpitch*coshdg
+    
+    return np.array([q0, q1, q2, q3])
+    
+    
+def q2e(q):
+    roll = atan2(2*(q[0]*q[1] + q[2]*q[3]), 1 - 2*(q[1]**2 + q[2]**2))
+    pitch = asin(2*(q[0]*q[2] - q[3]*q[1]))
+    hdg = atan2(2*(q[0]*q[3] + q[1]*q[2]), 1-2*(q[2]**2 + q[3]**2))
+    
+    return np.array([roll, pitch, hdg])
+   
+   
+def e2d(e):
+    return np.degrees(e)
+def e2r(e):
+    return np.radians(e)
+    
+def add(q1,q2):
+    return np.multiply(q1,q2)
+
+
+
+def add2(q1, q2):
+    Q1 = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]
+    Q2 = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]
+    Q3 = q1[0]*q2[2] + q1[2]*q2[0] + q1[3]*q2[1] - q1[1]*q2[3]
+    Q4 = q1[0]*q2[3] + q1[3]*q2[0] + q1[1]*q2[2] - q1[2]*q2[1]
+    
+    return np.array([Q1, Q2, Q3, Q4])
+
+
+e = e2r([10, 0, 0])
+q = e2q(e)
+print(q)
+print(e2d(q2e(q)))
+q2 = add2(q,q)
+print(q2)
+e2 = q2e(q2)
+print(e2d(e2))
+
+