daviddoji/Kalman-and-Bayesian-Filters-in-Python
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added warnings about unfinished EKF chapter.

Browse Source 
The EKF chapter is a very early draft/dump of material, and
yet people are trying to use it. They really shouldn't.
This commit is contained in:
Roger Labbe 2015-01-22 08:56:26 -08:00
parent a4bcbd0f14
commit ccf79aab09
4 changed files with 400 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
09_Extended_Kalman_Filters/Extended_Kalman_Filters.ipynb
View File

File diff suppressed because one or more lines are too long

179
exp/Untitled1.ipynb Normal file
View File

@ -0,0 +1,179 @@
{
"metadata": {
"name": "",
"signature": "sha256:fc6923e502800422d1ca66394e71d567efb82c4dcd09c273561149f8d3b00fe5"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "heading",
"level": 1,
"metadata": {},
"source": [
"Complementary Filter"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Convert continuous transfer function into discrete\n",
"\n"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"import scipy.signal as sig\n",
"from math import sqrt\n",
"\n",
"dt = 0.01\n",
"ki = 0.1**2\n",
"kp = sqrt(2)*0.1\n",
"\n",
"num = [kp, ki]\n",
"den = [1, 0.]\n",
"sig.cont2discrete(sys=(num,den), dt=0.1)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 8,
"text": [
"(array([[ 0.14142136, -0.14042136]]), array([ 1., -1.]), 0.1)"
]
}
],
"prompt_number": 8
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"a = 5\n",
"num = [a, 0]\n",
"den = [a, 1]\n",
"sig.cont2discrete(sys=(num,den), dt=0.1)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 11,
"text": [
"(array([[ 1., -1.]]), array([ 1. , -0.98019867]), 0.1)"
]
}
],
"prompt_number": 11
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from sympy import *\n",
"from sympy.abc import *\n",
"a = Symbol('a', positive=True)\n",
"foo = (s/(s+a)).rewrite()\n",
"print(foo)\n",
"inverse_laplace_transform(1/((s+a)), s, t)"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"s/(a + s)\n"
]
},
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 39,
"text": [
"exp(-a*t)*Heaviside(t)"
]
}
],
"prompt_number": 39
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
}
],
"metadata": {}
}
]
}

54
exp/compute_q.ipynb Normal file
View File

@ -0,0 +1,54 @@
{
"metadata": {
"name": "",
"signature": "sha256:72ca7769e63a40b2f7f89ae8f816dcd0df3658bb19519d8c95b2f6a5e99413ec"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"from sympy import Symbol, Matrix\n",
"from sympy.interactive import printing\n",
"printing.init_printing()\n",
"\n",
"dt = Symbol('\\Delta t')\n",
"\n",
"Q = Matrix([[.5*dt**2], [.5*dt**2], [dt], [dt]])\n",
"Q*Q.T\n",
"\n",
"\n",
"Q = Matrix([[.5*dt**2], [dt]])\n",
"Q*Q.T"
],
"language": "python",
"metadata": {},
"outputs": [
{
"latex": [
"$$\\left[\\begin{matrix}0.25 \\Delta t^{4} & 0.5 \\Delta t^{3}\\\\0.5 \\Delta t^{3} & \\Delta t^{2}\\end{matrix}\\right]$$"
],
"metadata": {},
"output_type": "pyout",
"png": "iVBORw0KGgoAAAANSUhEUgAAAKQAAAAzBAMAAADr+L6JAAAAMFBMVEX///8AAAAAAAAAAAAAAAAA\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAv3aB7AAAAD3RSTlMA74lUMhCZds3dIma7\nq0Ru0ZIZAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAEHUlEQVRYCe2XTWgTQRTH/0m6m68mFqwgHszi\nJSKURhQRFJJqLd6ailg8JRQtlVYbC34giEHBXks9KEXpnnoQoelFDx7MQREP4oKfqGBuHoq0fkSl\nfqxvZndnd22329KexDnsvJn572/2vZ3sewH8W6O/hCvSLXnep/RPfndkDaWfTK7IVbTqH7G2fZ+P\nVmpdIlKJz+Jg+1Y0ewEr1kK4sCgybukAuUx2G0ceSh/h8xMD94HhfEeNRg3f+BRd9rqRUs/pHFuK\nnGo4wfrOErvy1pWjzkBewmGVBpKG1hpSOocldP5stKC4kZGMNEZiBHSdw97V2YjNdQ8ziyPjTQiW\naZDII9CEiy9VsvG+VWFdFnHVjZwCNrOVSMs51iWH+OsNzZItV+nCkY0aQmyrwAgSX6CRRU0r8N37\n0A038jHtwxxIcBkSks5GPIwNn8niyDUakmwQqdvIZCXIdx/Fjt7BfuNu4/oLmMmQaSKnwd2JKujK\nxX7QPEcWikiaLyNax5s77F0lIKXySL79+RKYZI9hNekrIUs0Stzlr0lDF7nz4cnjUlSNr6N5jpxU\n0GCe9xkFDzBZA6aBCwrAQhR/8pSUVouRslOhUWM+9ptCWUGY3ZulM5JuUcn6G3md5tA4QqGk0NLu\nPERsTjSBpJlHhvvkDvqEwHBcsRyXq2xFnkOoAoTJRQrRX02iZ+KO0/ygil3UXSgCo0LGkWuKCLHX\nA5ynbcsI/jZiT7sXanzeeaGNshmaeAV2iskb5o5EMTAbR0ariPMTEyriZqAMuY5tbJ12n4FkKkVH\nsblHnmI9PWU+xODkTnIWbI41jgw3mTHrAM7J5G3ZOJyBOTrU2w2hfT1pBq4IXDNPUkoNlkM5U8KR\nuEJnSvqO2Oim4yOxIqbU0OVease+4rhUsmGGFc1IzSiUMYH4Buxkut7xoqx1WzoDeeDMM+AGIrqu\nj2Ci5SiiZLGWv3XGUopeSp/PIaoh1vMij3Eu0+uxh2JrAynkq2H8R65GFA3GPxLL4NBz/5gEB47a\nIl/H+zFgq72sLewbYjVfZA/uWVrvfgydqlj1Qjryvf1xFXeZhhBt5InAnLWQ3vmevhFezVEUZK1P\nm/lxo1s88710VnwP5pHtogBX7UXrKRfJ956vx1EUBIvzkYvk+86MLXdZjqLgtmPBekqvfE+RnKw4\n9E7TLgqSSjInVkykd74fw5QdeXEbN+yiYP+23bbIRIrkPC/f7xl67QaJkaMoSOm6mDbSGSCQtLJg\nvrfvEBaridxFgbFkOe6X7wXINuYXBS4k/PK9TRKWRpa7KHAj/fK9AAljgaLAjfTL94IkDKO8dBUF\nbqRfvhckYSxQFLiRfvlekISxQFHgRgrlyg3rB7lykiD8R4pQrNhoW8Kf52Vtwv48L+Ev/nKY9Bf/\nD+iXYj8NWuVEAAAAAElFTkSuQmCC\n",
"prompt_number": 8,
"text": [
"\u23a1 4 3\u23a4\n",
"\u23a20.25\u22c5\\Delta t 0.5\u22c5\\Delta t \u23a5\n",
"\u23a2 \u23a5\n",
"\u23a2 3 2 \u23a5\n",
"\u23a30.5\u22c5\\Delta t \\Delta t \u23a6"
]
}
],
"prompt_number": 8
}
],
"metadata": {}
}
]
}

100
exp/satellite.ipynb Normal file
View File

@ -0,0 +1,100 @@
{
"metadata": {
"name": "",
"signature": "sha256:9fb5a469b0b0f27f1e721735fae76d74f0ad242621b94c3f528e9392b7deab82"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [
"import math\n",
"\n",
"def range_to(x1, x2):\n",
" dx = x1[0] - x2[0]\n",
" dy = x1[1] - x2[1]\n",
" print('dx',dx,dy)\n",
" return math.sqrt(dx*dx + dy*dy)"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 40
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"s1 = (0., 2.) # satellite 1\n",
"s2 = (1., 2.) # satellite 2\n",
"p = (-0.5, 1.) # platform postion\n",
"\n",
"\n",
"s1 = (0., 200.) # satellite 1\n",
"s2 = (100., 220.) # satellite 2\n",
"p = (10, 1.) # platform postion\n",
"\n",
"r1 = range_to(s1, p)\n",
"r2 = range_to(s2, p)\n",
"\n",
"x1, y1 = s1[0], s1[1]\n",
"x2, y2 = s2[0], s2[1]\n",
"\n",
"A = -(y2 - y1) / (x2 - x1)\n",
"B = (r1**2 - r2**2 - x1**2 - y1**2 + x2**2 + y2**2) / (2*(x2 - x1))\n",
"\n",
"a = 1 + A**2\n",
"b = -2*A*x1 + 2*A*B - 2*y1\n",
"c = x1**2 - 2*x1*B + y1**2 - r1**2\n",
"\n",
"y = (-b - math.sqrt(b**2 - 4*a*c)) / (2*a)\n",
"x = A*y + B\n",
"\n",
"print('p', x,y)\n",
"print('err', x-p[0], y-p[1])"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"dx -10.0 199.0\n",
"dx 90.0 219.0\n",
"p 10.051726583768032 0.7413670811596572\n",
"err 0.05172658376803163 -0.2586329188403428\n"
]
}
],
"prompt_number": 55
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"A"
],
"language": "python",
"metadata": {},
"outputs": [
{
"metadata": {},
"output_type": "pyout",
"prompt_number": 56,
"text": [
"-0.2"
]
}
],
"prompt_number": 56
}
],
"metadata": {}
}
]
}