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.
This commit is contained in:
Roger Labbe committed
1 parent
cfd4e3485a
commit
a294a51807
4 files changed
+296
-7
No files matched your search
+182
-3
@@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:27a5ed2ddd67c228610a6ba471f1ec1e67b9a62114cc2be9574fab84b11cb41b"
|
||||
"signature": "sha256:5f83a9ad789c1e39b3baf7784f5a29653a668d597037e30e671943b708366e53"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@@ -259,7 +259,7 @@
|
||||
"output_type": "pyout",
|
||||
"prompt_number": 1,
|
||||
"text": [
|
||||
"<IPython.core.display.HTML at 0x7f620295c9e8>"
|
||||
"<IPython.core.display.HTML at 0x7f842d6f86a0>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -737,7 +737,7 @@
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAxYAAAGaCAYAAACSU9UtAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7f61e81c98d0>"
|
||||
"<matplotlib.figure.Figure at 0x7f0a48014898>"
|
||||
]
|
||||
}
|
||||
],
|
||||
@@ -1337,6 +1337,185 @@
|
||||
"> For reference, the Appendix **Symbols and Notations** lists the symbology used by the major authors in the field."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Numeric Integration of Differential Equations"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"** author's note: this is just notes to a section. If you need to know this in depth, \n",
|
||||
"*Computational Physics in Python * by Dr. Eric Ayars is excellent, and available here.\n",
|
||||
"http://phys.csuchico.edu/ayars/312/Handouts/comp-phys-python.pdf **\n",
|
||||
"\n",
|
||||
"So far in this book we have been working with systems that can be expressed with simple linear differential equations such as\n",
|
||||
"\n",
|
||||
"$$v = \\dot{x} = \\frac{dx}{dt}$$\n",
|
||||
"\n",
|
||||
"which we can integrate into a closed form solution, in this case $x(t) =vt + x_0$. This equation is then put into the system matrix $\\mathbf{F}$, which allows the Kalman filter equations to predict the system state in the future. For example, our constant velocity filters use\n",
|
||||
"\n",
|
||||
"$$\\mathbf{F} = \\begin{bmatrix}\n",
|
||||
"1 & \\Delta t \\\\ 0 & 1\\end{bmatrix}$$.\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"The Kalman filter predict equation is $\\mathbf{x}^- = \\mathbf{Fx} + \\mathbf{Bu}$. Hence the prediction is\n",
|
||||
"\n",
|
||||
"$$\\mathbf{x}^- = \\begin{bmatrix}\n",
|
||||
"1 & \\Delta t \\\\ 0 & 1\\end{bmatrix}\\begin{bmatrix}\n",
|
||||
"x\\\\ \\dot{x}\\end{bmatrix}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"which multiplies out to \n",
|
||||
"$$\\begin{aligned}x^- &= x + v\\Delta t \\\\\n",
|
||||
"\\dot{x}^- &= \\dot{x}\\end{aligned}$$."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"This works for linear ordinary differential equations (ODEs), but does not work (well) for nonlinear equations. For example, consider trying to predict the position of a rapidly turning car. Cars turn by pivoting the front wheels, which cause the car to pivot around the rear axle. Therefore the path will be continuously varying and a linear prediction will necessarily produce an incorrect value. If the change in the system is small enough relative to $\\Delta t$ this can often produce adequate results, but that will rarely be the case with the nonlinear Kalman filters we will be studying in subsequent chapters. Another problem is that even trivial systems produce differential equations for which finding closed form solutions is difficult or impossible. \n",
|
||||
"\n",
|
||||
"For these reasons we need to know how to numerically integrate differential equations. This can be a vast topic, and scipy provides integration routines such as `scipy.integrate.ode`. These routines are robust, but "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"** material about Euler here**\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 3,
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Runge Kutta Methods"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Runge Kutta integration is the workhorse of numerical integration. As mentioned earlier there are a vast number of methods and literature on the subject. In practice, using the Runge Kutta algorithm that I present here will solve most any problem you will face. It offers a very good balance of speed, precision, and stability, and it is used in vast amounts of scientific software. \n",
|
||||
"\n",
|
||||
"Let's just dive in. We start with some differential equation\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\ddot{y} = \\frac{d}{dt}\\dot{y}$$.\n",
|
||||
"\n",
|
||||
"We can substitute the derivative of y with a function f, like so\n",
|
||||
"\n",
|
||||
"$$\\ddot{y} = \\frac{d}{dt}f(y,t)$$."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"$$t(t+\\Delta t) = y(t) + \\frac{1}{6}(k_1 + 2k_2 + 2k_3 + k_4) + O(\\Delta t^4)$$\n",
|
||||
"\n",
|
||||
"$$\\begin{aligned}\n",
|
||||
"k_1 &= f(y,t)\\Delta t \\\\\n",
|
||||
"k_2 &= f(y+\\frac{1}{2}k_1, t+\\frac{1}{2}\\Delta t)\\Delta t \\\\\n",
|
||||
"k_3 &= f(y+\\frac{1}{2}k_2, t+\\frac{1}{2}\\Delta t)\\Delta t \\\\\n",
|
||||
"k_4 &= f(y+k_3, t+\\Delta t)\\Delta t\n",
|
||||
"\\end{aligned}\n",
|
||||
"$$\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"def runge_kutta4(y, x, dx, f):\n",
|
||||
" \"\"\"computes 4th order Runge-Kutta for dy/dx.\n",
|
||||
" y is the initial value for y\n",
|
||||
" x is the initial value for x\n",
|
||||
" dx is the difference in x (e.g. the time step)\n",
|
||||
" f is a callable function (y, x) that you supply to compute dy/dx for\n",
|
||||
" the specified values.\n",
|
||||
" \"\"\"\n",
|
||||
" \n",
|
||||
" k1 = dx * f(y, x)\n",
|
||||
" k2 = dx * f(y + 0.5*k1, x + 0.5*dx)\n",
|
||||
" k3 = dx * f(y + 0.5*k2, x + 0.5*dx)\n",
|
||||
" k4 = dx * f(y + k3, x + dx)\n",
|
||||
" \n",
|
||||
" return y + (k1 + 2*k2 + 2*k3 + k4) / 6."
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"prompt_number": 15
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Let's use this for a simple example. Let\n",
|
||||
"\n",
|
||||
"$$\\dot{y} = t\\sqrt{y(t)}$$,\n",
|
||||
"\n",
|
||||
"with the initial values\n",
|
||||
"\n",
|
||||
"$$\\begin{aligned}t_0 &= 0\\\\y_0 &= y(t_0) = 1\\end{aligned}$$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"import math\n",
|
||||
"t = 0.\n",
|
||||
"y = 1.\n",
|
||||
"dt = .1\n",
|
||||
"\n",
|
||||
"ys = []\n",
|
||||
"ts = []\n",
|
||||
"\n",
|
||||
"def func(y,t):\n",
|
||||
" return t*math.sqrt(y)\n",
|
||||
"\n",
|
||||
"while t <= 10:\n",
|
||||
" y = runge_kutta4(y, t, dt, func)\n",
|
||||
" t += dt\n",
|
||||
"\n",
|
||||
" ys.append(y)\n",
|
||||
" ts.append(t)\n",
|
||||
"\n",
|
||||
"exact = [(t**2+4)**2 / 16. for t in ts]\n",
|
||||
"plt.plot(ts, ys)\n",
|
||||
"plt.plot(ts, exact)\n",
|
||||
"plt.show()\n",
|
||||
"\n",
|
||||
"error = np.array(exact) - np.array(ys)\n",
|
||||
"print(\"max error {})"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"metadata": {},
|
||||
"output_type": "display_data",
|
||||
"png": "iVBORw0KGgoAAAANSUhEUgAAAyIAAAGNCAYAAADtmMVMAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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 truncated
|
||||
"text": [
|
||||
"<matplotlib.figure.Figure at 0x7f0a3ce895f8>"
|
||||
]
|
||||
}
|
||||
],
|
||||
"prompt_number": 27
|
||||
},
|
||||
{
|
||||
"cell_type": "heading",
|
||||
"level": 2,
|
||||
|
||||
@@ -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.
|
||||
|
||||
Whitespace-only changes.
@@ -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))
|
||||
|
||||
|
||||
Reference in new issue
Block a user