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/RungeKutta.py

@@ -76,11 +76,8 @@ def rk4(y, x, dx, f):
     
     return y + (k1 + 2*k2 + 2*k3 + k4) / 6.
 
-def rk2 (y,x,dx,f):
-    """computes the 2nd order Runge-kutta for dy/dx"""
-    
-    
     
+        
     
 def fx(x,t):
     return fx.vel
@@ -126,7 +123,44 @@ def ball_scipy(y0, vel, omega, dt):
         ys.append(solver.integrate(t))
         
         
+def RK4(f):
+    return lambda t, y, dt: (
+            lambda dy1: (
+            lambda dy2: (
+            lambda dy3: (
+            lambda dy4: (dy1 + 2*dy2 + 2*dy3 + dy4)/6
+            )( dt * f( t + dt  , y + dy3   ) )
+	    )( dt * f( t + dt/2, y + dy2/2 ) )
+	    )( dt * f( t + dt/2, y + dy1/2 ) )
+	    )( dt * f( t       , y         ) )
+ 
+def theory(t): return (t**2 + 4)**2 /16
+ 
+from math import sqrt
+dy = RK4(lambda t, y: t*sqrt(y))
+ 
+t, y, dt = 0., 1., .1
+while t <= 10:
+    if abs(round(t) - t) < 1e-5:
+        print("y(%2.1f)\t= %4.6f \t error: %4.6g" % (t, y, abs(y - theory(t))))
+ 
+    t, y = t + dt, y + dy(t, y, dt)        
+        
+t = 0.
+y=1.
+
+def test(y, t):
+    return t*sqrt(y)
+    
+while t <= 10:
+    if abs(round(t) - t) < 1e-5:
+        print("y(%2.1f)\t= %4.6f \t error: %4.6g" % (t, y, abs(y - theory(t))))
+ 
+    y = rk4(y, t, dt, test)
+    t += dt     
+    
 if __name__ == "__main__":
+    1/0
 
     dt = 1./30
     y0 = 15.

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))
+
+