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

Change to predict/update terminology.

Browse Source 
I originally used sense/update as my function names. But all the
literature uses update/predict, so I am normalizing on that.

I also wrote a test to ensure the update/predict functions in the
1d kalman filter chapter gives the same output as the full blown
kalman filter. Of course, they do. This is in the 1d_kf_compare
notebook. I will add this as an exercise in the multidimensional KF
chapter.
This commit is contained in:
Roger Labbe
2014-08-29 22:38:59 -07:00
parent a12322e2ef
commit 87d4d973ac
3 changed files with 397 additions and 444 deletions
Download Patch File Download Diff File Expand all files Collapse all files

201
exp/1d_kf_compare.ipynb Normal file
View File

@@ -0,0 +1,201 @@
{
"metadata": {
"name": "",
"signature": "sha256:65ed87658245046683e9bc29217fbb80b0e626bf1af1d8084589c08a10b413cc"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Compare 1d kalman filter with full kalman filter with only 1 observed state variable."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from __future__ import division\n",
"import numpy as np\n",
"\n",
"def predict(pos, variance, movement, movement_variance):\n",
" return (pos + movement, variance + movement_variance)\n",
" \n",
"def update(mean, variance, measurement, measurement_variance):\n",
" return multiply(mean, variance, measurement, measurement_variance)\n",
" \n",
"def multiply(mu1, var1, mu2, var2):\n",
" mean = (var1*mu2 + var2*mu1) / (var1+var2)\n",
" variance = 1 / (1/var1 + 1/var2)\n",
" return (mean, variance)"
],
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 1
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from numpy.random import randn\n",
"from filterpy.kalman import KalmanFilter\n",
"\n",
"f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)\n",
"f.P = 500\n",
"f.H = np.array([[1.]])\n",
"f.F = np.array([[1.]])\n",
"f.B = np.array([[1.]])\n",
"f.Q = 3\n",
"f.R = 5\n",
"\n",
"pos = (0., 500.)\n",
"\n",
"for i in range(100):\n",
" z = i + randn()\n",
" pos = update(pos[0], pos[1], z, 5)\n",
" f.update(z)\n",
" \n",
" assert abs(pos[0]- f.x[0,0]) < 1.e-12\n",
" \n",
" pos = predict(pos[0], pos[1], 1., 3.)\n",
" f.predict(u=1)\n",
" print pos[0], f.x[0,0]"
],
"language": "python",
"metadata": {},
"outputs": [
{
"output_type": "stream",
"stream": "stdout",
"text": [
"0.829799081522 0.829799081522\n",
"1.11657328836 1.11657328836\n",
"2.60484331656 2.60484331656\n",
"4.49991898992 4.49991898992\n",
"4.96959734671 4.96959734671\n",
"5.62090599924 5.62090599924\n",
"6.62676551801 6.62676551801\n",
"7.17411383707 7.17411383707\n",
"8.22589645236 8.22589645236\n",
"9.68407847845 9.68407847845\n",
"10.7846369056 10.7846369056\n",
"10.9474107429 10.9474107429\n",
"13.1938820521 13.1938820521\n",
"14.5982855965 14.5982855965\n",
"16.0869457423 16.0869457423\n",
"16.9075718464 16.9075718464\n",
"17.4098040813 17.4098040813\n",
"19.0869215468 19.0869215468\n",
"19.8234493932 19.8234493932\n",
"20.1047073436 20.1047073436\n",
"21.0052752387 21.0052752387\n",
"21.1874177725 21.1874177725\n",
"22.8594121392 22.8594121392\n",
"24.1776382364 24.1776382364\n",
"25.8376519754 25.8376519754\n",
"27.3430811764 27.3430811764\n",
"27.5187562442 27.5187562442\n",
"26.9915649343 26.9915649343\n",
"28.4780664826 28.4780664826\n",
"29.810753716 29.810753716\n",
"31.6508396458 31.6508396458\n",
"32.5037739338 32.5037739338\n",
"33.8584143825 33.8584143825\n",
"33.8776271222 33.8776271222\n",
"35.5242613545 35.5242613545\n",
"36.519783756 36.519783756\n",
"36.3488727922 36.3488727922\n",
"37.8636116598 37.8636116598\n",
"39.0723631747 39.0723631747\n",
"39.9711641266 39.9711641266\n",
"41.4487411592 41.4487411592\n",
"42.3356350788 42.3356350788\n",
"43.3965556332 43.3965556332\n",
"44.9391890292 44.9391890292\n",
"46.9342496588 46.9342496588\n",
"45.968617491 45.968617491\n",
"47.4965506188 47.4965506188\n",
"48.4848670589 48.4848670589\n",
"49.4249280321 49.4249280321\n",
"50.0093718198 50.0093718198\n",
"50.3027074022 50.3027074022\n",
"51.281454098 51.281454098\n",
"52.7264828805 52.7264828805\n",
"53.4501779367 53.4501779367\n",
"54.1096052489 54.1096052489\n",
"55.3709733324 55.3709733324\n",
"56.9577696685 56.9577696685\n",
"57.8901308737 57.8901308737\n",
"58.7172970247 58.7172970247\n",
"60.5336542662 60.5336542662\n",
"61.1350857058 61.1350857058\n",
"63.0005784455 63.0005784455\n",
"63.5000655463 63.5000655463\n",
"63.8518134142 63.8518134142\n",
"65.8465552577 65.8465552577\n",
"66.7400646473 66.7400646473\n",
"66.8160042604 66.8160042604\n",
"67.5001380601 67.5001380601\n",
"69.2213351148 69.2213351148\n",
"70.7970352555 70.7970352555\n",
"71.6170587238 71.6170587238\n",
"71.8435329248 71.8435329248\n",
"72.7477102909 72.7477102909\n",
"73.2184233535 73.2184233535\n",
"74.5671596854 74.5671596854\n",
"75.927838911 75.927838911\n",
"77.3248818672 77.3248818672\n",
"78.3493332795 78.3493332795\n",
"79.955729761 79.955729761\n",
"80.1451532378 80.1451532378\n",
"81.7478421332 81.7478421332\n",
"83.4286167873 83.4286167873\n",
"84.3119158883 84.3119158883\n",
"84.46121675 84.46121675\n",
"85.2398001115 85.2398001115\n",
"86.4216278548 86.4216278548\n",
"86.9207107003 86.9207107003\n",
"88.6062087038 88.6062087038\n",
"88.3828461934 88.3828461934\n",
"89.9747496843 89.9747496843\n",
"91.1423576281 91.1423576281\n",
"91.1750403785 91.1750403785\n",
"93.8774517576 93.8774517576\n",
"94.3911839298 94.3911839298\n",
"95.694756916 95.694756916\n",
"97.0314685474 97.0314685474\n",
"97.0502972107 97.0502972107\n",
"97.6877008238 97.6877008238\n",
"99.2864064463 99.2864064463\n",
"100.840809936 100.840809936\n"
]
}
],
"prompt_number": 19
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
}
],
"metadata": {}
}
]
}