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

Added Least Squares filter to comparison.

Browse Source 
Looking at how least squares filter performs compared to univariate
Kalman filter.
This commit is contained in:
Roger Labbe committed 2014-08-30 16:05:32 -07:00
1 parent 52dc8a6aa9
commit 110a4f7c6d
1 file changed
+31 -106
Download Patch File Download Diff File Expand all files Collapse all files
exp/1d_kf_compare.ipynb
+31 -106
View File
@@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:ce40207f3bc97f093a6d0e943ce3efb3506ade3873886bdf36d74e785611b3e0"
"signature": "sha256:6d2f6595215a9ad10adc06ace85fe18d7fb20af5b5b075d86c635ebc3af0abf5"
},
"nbformat": 3,
"nbformat_minor": 0,
@@ -44,14 +44,23 @@
"language": "python",
"metadata": {},
"outputs": [],
"prompt_number": 20
"prompt_number": 1
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"%matplotlib inline\n",
"from numpy.random import randn\n",
"import numpy as np\n",
"from filterpy.kalman import KalmanFilter\n",
"from filterpy.leastsq import LeastSquaresFilter\n",
"import matplotlib.pyplot as plt\n",
"import matplotlib.pylab as pylab\n",
"\n",
"pylab.rcParams['figure.figsize'] = (16.0, 16.0)\n",
"\n",
"lsq = LeastSquaresFilter(dt=1, order=2, noise_variance = 5)\n",
"\n",
"f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)\n",
"f.P = 500\n",
@@ -62,17 +71,24 @@
"f.R = 5\n",
"\n",
"pos = (0., 500.)\n",
"\n",
"for i in range(100):\n",
"print (pos)\n",
"plt.figure()\n",
"for i in range(50):\n",
" z = i + randn()\n",
" pos = update(pos[0], pos[1], z, 5)\n",
" x = lsq(z)\n",
" f.update(z)\n",
" \n",
" plt.scatter(i, pos[0], c='b')\n",
" plt.scatter(i, x, c='g')\n",
" plt.scatter(i, f.x[0,0], c='r')\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]"
" \n",
"plt.show()"
],
"language": "python",
"metadata": {},
@@ -81,110 +97,19 @@
"output_type": "stream",
"stream": "stdout",
"text": [
"0.664384271377 0.664384271377\n",
"2.22227758606 2.22227758606\n",
"3.4172403366 3.4172403366\n",
"4.24063420552 4.24063420552\n",
"5.22473319997 5.22473319997\n",
"5.46612331732 5.46612331732\n",
"6.7346061548 6.7346061548\n",
"7.89654509649 7.89654509649\n",
"8.47171516678 8.47171516678\n",
"10.7084468468 10.7084468468\n",
"12.6133769702 12.6133769702\n",
"12.3117642898 12.3117642898\n",
"13.8143561571 13.8143561571\n",
"15.1102237851 15.1102237851\n",
"14.6087213015 14.6087213015\n",
"15.6175764575 15.6175764575\n",
"17.4530455485 17.4530455485\n",
"19.218669382 19.218669382\n",
"19.842640257 19.842640257\n",
"21.0396707771 21.0396707771\n",
"20.8548540387 20.8548540387\n",
"21.7993178972 21.7993178972\n",
"23.1601232215 23.1601232215\n",
"23.8020951292 23.8020951292\n",
"24.5589139955 24.5589139955\n",
"25.2648382153 25.2648382153\n",
"26.3808199764 26.3808199764\n",
"28.0259953354 28.0259953354\n",
"28.9543385004 28.9543385004\n",
"30.0278535184 30.0278535184\n",
"31.5690196179 31.5690196179\n",
"32.4717047765 32.4717047765\n",
"33.8319099215 33.8319099215\n",
"34.3625317212 34.3625317212\n",
"34.2069889773 34.2069889773\n",
"34.9681205575 34.9681205575\n",
"37.1308718998 37.1308718998\n",
"38.3242660235 38.3242660235\n",
"39.5842365428 39.5842365428\n",
"39.7981314712 39.7981314712\n",
"41.3807379194 41.3807379194\n",
"42.1642193964 42.1642193964\n",
"42.8873765732 42.8873765732\n",
"45.2149230527 45.2149230527\n",
"46.6221441815 46.6221441815\n",
"46.9760943769 46.9760943769\n",
"46.8175543204 46.8175543204\n",
"48.5019757199 48.5019757199\n",
"49.9143390923 49.9143390923\n",
"50.4804330008 50.4804330008\n",
"52.0256389036 52.0256389036\n",
"51.8114561147 51.8114561147\n",
"53.974428537 53.974428537\n",
"53.9347539305 53.9347539305\n",
"54.8458985477 54.8458985477\n",
"55.8948299555 55.8948299555\n",
"56.8829522326 56.8829522326\n",
"58.1054439422 58.1054439422\n",
"58.5375097795 58.5375097795\n",
"59.2611573706 59.2611573706\n",
"59.7132715231 59.7132715231\n",
"61.5788978249 61.5788978249\n",
"62.6053334603 62.6053334603\n",
"64.1160504937 64.1160504937\n",
"64.9051800497 64.9051800497\n",
"65.8171413931 65.8171413931\n",
"67.5405856008 67.5405856008\n",
"69.3558420075 69.3558420075\n",
"69.2601512441 69.2601512441\n",
"70.5926254448 70.5926254448\n",
"72.2861748617 72.2861748617\n",
"72.2128001363 72.2128001363\n",
"73.3475331447 73.3475331447\n",
"73.9061038839 73.9061038839\n",
"75.4191354917 75.4191354917\n",
"76.0817944369 76.0817944369\n",
"76.9319955496 76.9319955496\n",
"79.2451599226 79.2451599226\n",
"78.8853267224 78.8853267224\n",
"79.631237244 79.631237244\n",
"80.6764469096 80.6764469096\n",
"82.5190126375 82.5190126375\n",
"83.2134146817 83.2134146817\n",
"84.2811052968 84.2811052968\n",
"84.8078912273 84.8078912273\n",
"86.5334238222 86.5334238222\n",
"86.5214834008 86.5214834008\n",
"87.8228643692 87.8228643692\n",
"89.0913414521 89.0913414521\n",
"89.9159627612 89.9159627612\n",
"91.2593135665 91.2593135665\n",
"92.4642469142 92.4642469142\n",
"92.7895992833 92.7895992833\n",
"93.9979031977 93.9979031977\n",
"94.6721942775 94.6721942775\n",
"96.7630484108 96.7630484108\n",
"96.7600593835 96.7600593835\n",
"98.0318096805 98.0318096805\n",
"99.4928692605 99.4928692605\n",
"100.452736197 100.452736197\n"
"(0.0, 500.0)\n"
]
},
{
"metadata": {},
"output_type": "display_data",
"png": "iVBORw0KGgoAAAANSUhEUgAAA6gAAAOeCAYAAAAOYj2YAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAIABJREFUeJzs3XmUXGWBNvCnqjudpLMSCFmAQAiENSyySgKEbUYYjCKD\ny+eOjNu44SjgjAsuMwMyM+InyCg6GhQQl1EENxCJKCgE2QwJBMKWCEkIJCGhs3VXfX90g3wokHS6\nc293/X7n9Ol6K3X7PufUOel6+n3vexMAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA+rmRSb6fZF6S\nuUkOSTIqybVJ5ie5pus1AAAA0KtmJjm163FzkhFJPp/kjK7nzkxyTgG5AAAAaCAjkjzwV56/J8mY\nrsdju8YAAADQa/ZLcnOSbyS5LcnFSYYkWf6c11SeNwYAAIC/UN3M45uTvCzJl7u+P53krOe9pt71\nBQAAAC+oeTOPX9T1Nbtr/P0kH0uyOJ1LexcnGZdk6fMPnDRpUn3BggWbeXoAAABKakGSXTblgEoP\nnPSGJKelc8fes5O0dj3/RJJz0zmjOjJ/ZWa1Xjex2qjOPvvsnH322UXHoCDe/8blvW9s3v/G5b1v\nbN7/xlWpVJJN7JybO4OaJO9PcmmSlnQ25LcnaUry3STvSPJQktf2wHkAAADox3qioN6Z5KC/8vyx\nPfCzAQAAaBCbu0kSdMv06dOLjkCBvP+Ny3vf2Lz/jct739i8/2yKnrgGtbtcgwoAANBPdecaVDOo\nAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSC\nggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABA\nKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAA\nAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIK\nAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCko\nqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACU\ngoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAA\nQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgA\nAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKC\nCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEAp\nKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAA\nlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoA\nAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAQCkoqAAAAJSCggoAAEApKKgAAACUgoIKAABAKSio\nAAAAlIKCCgAAQCkoqAAAAJRCcw/8jIeSPJWkI8mGJAcnGZXkiiQ7dv37a5Os6IFzAQAA0E/1xAxq\nPcn0JPuns5wmyVlJrk0yOcl1XWMAAAB4QT21xLfyvPGMJDO7Hs9M8uoeOg8AAAD91POLZXc8kGRl\nOpf4fiXJxUmWJ9nqOed48jnjZ9Tr9XoPnB4AAOhNTz/9dG688cZUq9VMmzYtgwYNKjoSfUClUkk2\nsXP2xDWoU5M8lmR0Opf13vO8f693fQEAAH3MY489lkOmHZIV1RVJLRk7eGxu/s3N2Wqr588/webr\niYL6WNf3x5P8MJ3XoS5JMjbJ4iTjkiz9aweeffbZzz6ePn16pk+f3gNxAACAnvLhMz+cx7Z7LO3H\ntCf1ZN3P1uUTZ38iF3zxgqKjUTKzZs3KrFmzNutnbO4S39YkTUlWJRmS5Jokn05ybJInkpybzg2S\nRuYvN0qyxBcAAEruwKkH5g8T/tA5tVVNsio5bsNxueaqa4qORskVscR3TDpnTZ/5WZems6TemuS7\nSd6RP99mBgAA6GMmT5qceZf/IZOqnfeUXFhL9vrAXkXHop/qiU2SussMKgAAlNxuO0zM3y16KP+V\nzo1l3p7kj3vtkz/MubPgZJRdd2ZQe+o2MwAAQD+06okn8oqux5Ukxyd54rHHXuQI6D4FFQAAeEE7\n7LprvpRqNiRZk+TLqWbXfaYUHYt+yhJfAADgBS1btiz7Td4jTy5flnqS8WO3yx8XzE9ra2vR0Si5\n7izxVVABAIAXVavVMm/evDQ3N2e33XYrOg59hIIKAABAKdgkCQAAgD5LQQUAAKAUFFQAAABKobno\nAAAAwJbT1taWK664IitWrMixxx6bKVPcMobysEkSAAA0iLa2thzw8gOycMPCbBi5Ic3zmvO9S7+X\nE044oeho9EPd2STJDCoAADSImTNn5uHaw1nz2jVJJVm/y/q86/3vysITFhYdDZK4BhUAABrGsmXL\nsm6rdX+e09o2WbF8RaGZ4LkUVAAAaBDHHHNMBs0ZlDyaZG0y8PqBOebYY4qOBc9SUAEAoEEcdthh\nueiLF2WrH26VAecPyNE7HJ1LvnZJ0bHgWQoqAAA0kOVPLs+AVeszbEOy4pHH09xsWxrKwy6+AADQ\nIH74wx/mTa95Tb6ZZOckH0g1a/bYM7fN/WPByeiPurOLr4IKAAAN4rjjjsuev7wuX0zn5/CHk+yZ\nSp6u14oNRr/UnYJqiS8AADSIIUOGZPFz+sLjSZoqRc5Zwf9PQQUAgAZx7rnn5ieV5F2p5AtJTkgl\nJ73pTUXHgmdZ4gsAAA1kzpw5ec8735VVT67Ia/7P6/LJT36y6Ej0U65BBQCABlKv1/OrX/0q9913\nX6ZMmZKpU6cWHQme1Z2Cak9pAADoo973ofdl5vdmpjahlsoDlZz5wTPzyX8xI0rfZQYVAAD6oHvu\nuScvO+xlWfPONUk9SXsy8CsD88gDj2TbbbctOh6YQQUAgEaxZMmS1FvqGfiFpNKRVFqS2qBk2bJl\nCip9ll18AQCgDxo4cGAqS9bmF+uTNR3J5WuSyvJ1GT58eNHRoNsUVAAA6INuvvnmTEo1R3aNX5Vk\nq1Ry2223FRkLNouCCgAAfdDee++dh1PL413jh5IsTz177LFHgalg8yioAADQBx1zzDE5aNoR2T2V\nvCLN2TfJK1/z99l1112LjgbdZhdfAADowy655JLceuutOfLII3PyyScXHQee1Z1dfBVUAAAogRUr\nVuTYaUfmwXvvycBBg/OFr30lr3vd64qOBd2moAIAQB+16/Y7Zuc/LcrnUsttSU5PJdf//nc55JBD\nio4G3aKgAgBAH9Te3p5BAwZkeZJhXc+dnKYMfcsbM3PmzCKjQbd1p6DaJAkAAApWrVZTTbLiOc89\nkWTw4MEFJYJimEEFAIASmHbIy7Polt/nzCQ3J/lBtZp5Dz+c7bffvuho0C3dmUFt7p0oAADApnhi\n7Yo8vHfy4WqyfkDSsqAljz76qIJKQ7HEFwAACtbR0ZF759ybnJSsfU1Se2VS2a2SW2+9tehosEUp\nqAAAULCmpqaM2nZU8nDXExuSpkebMmHChEJzwZbmGlQAACiBa6+9NiedclKadmxKx9KOnHDUCbni\n0iueuY4P+hy3mQEAgD7skUceya233pptt902U6dOVU7p0xRUAAAASsF9UAEAAOizFFQAAABKQUEF\nAACgFBRUAAAASkFBBQAAoBQUVAAAAEpBQQUAAKAUFFQAAABKQUEFAIBeUKvVsmDBgjz00EOp1+tF\nx4E+obnoAAAA0N+sWLEiu+wyMRueXJEkGTFh+9w/f0FaWloKTgblZgYVAAB62EEHHZghT67I1fXk\n+/Wk7ZFFOe5vjis6FpRepcBz1y11AACgP9qquSmXdtRyQtf460nOGDQwT6xZW2Qs2KIqlUqyiZ3T\nDCoAAPSwarUpTzxnvDRJS8ugouJAn6GgAgBADzv9k5/Me5L8e5JPJPlskv/48oXFhoI+wBJfAADo\nBV/96lfzX/9+bqrNTfnXz5+bk046qehIsEV1Z4mvggoAAECPcw0qAAAAfZaCCgAAQCkoqAAAAJSC\nggoAAEApKKgAAACUgoIKAABAKSioAAAAlIKCCgAAL2HBggU59Z2n5lWnvCqXX3550XGg32ouOgAA\nAJTZwoULc8ChB2TV3qtSG1HLLz/8yyx9fGk++IEPFh0N+h0zqAAA8CK+/e1v5+lJT6c2vZbsn7S9\nqi3/dt6/FR0L+iUzqAAA8CLa29tTq9aSpUnWJmlKOto7io4F/VKlwHPX6/V6gacHAICXNmfOnByy\n7z4ZVKln62qyqD155etemysuv6LoaFBqlUol2cTOaYkvAAC8iAsuuCCTasmijmT+huScejLryp8U\nHQv6JQUVAABexN1z5uSk1DO4a/yaJKvXtBUZCfotBRUAAF7Eyw44IN9JNU91jS9JJcOHDis0E/RX\nrkEFAIAXUavV8rI9p+T+e+dmRCpZ1dSUq391XY444oiio0GpdecaVAUVAAA2ws0335zFixfnmGOO\nydChQ4uOA6WnoAIAwEtYv3593ve+9+Wee+7JoYcemnPOOSfVqivfoKcpqAAA8CJqtVp2GTLine truncated
"text": [
"<matplotlib.figure.Figure at 0x7ff53fa86890>"
]
}
],
"prompt_number": 21
"prompt_number": 15
},
{
"cell_type": "code",