├── .gitattributes ├── .gitignore ├── 00-A目录.ipynb ├── 00-前言.ipynb ├── 01-g-h滤波器.ipynb ├── 02-离散贝叶斯.ipynb ├── 03-高斯模型.ipynb ├── 04-一维卡尔曼滤波器.ipynb ├── 05-多维高斯模型.ipynb ├── 06-多维卡尔曼滤波器.ipynb ├── 07-卡尔曼滤波器中的数学.ipynb ├── 08-设计卡尔曼滤波器.ipynb ├── 09-非线性滤波.ipynb ├── 10-无迹卡尔曼滤波器.ipynb ├── 11-扩展卡尔曼滤波器.ipynb ├── 12-粒子滤波器.ipynb ├── 13-平滑化.ipynb ├── 14-自适应滤波.ipynb ├── README.md ├── Supporting_Notebooks ├── Computing_and_plotting_PDFs.ipynb ├── Converting-Multivariate-Equations-to-Univariate.ipynb ├── Interactions.ipynb ├── Iterative-Least-Squares-for-Sensor-Fusion.ipynb └── Taylor-Series.ipynb ├── __init__.py ├── animations ├── 02_no_info.gif ├── 02_simulate.gif ├── 04_gaussian_animate.gif ├── 05_dog_track.gif ├── 05_volt_animate.gif ├── 13_particle_move.gif ├── Gaussians_Animations.ipynb ├── Kalman_Filters_Animations.ipynb ├── discrete_bayes_animations.ipynb ├── multivariate_animations.ipynb ├── multivariate_ellipse.gif ├── multivariate_track1.gif ├── particle_animate.ipynb └── particle_filter_anim.gif ├── book_format.py ├── environment.yml ├── experiments ├── 1d_kf_compare.ipynb ├── 1dposvel.ipynb ├── 2014-03-26-000-Data.csv ├── DiscreteBayes1D.py ├── ILS.py ├── RobotLocalizationParticleFilter.py ├── RobotLocalizationParticleFilter_2.py ├── RungeKutta.py ├── Untitled.ipynb ├── Untitled0.ipynb ├── Untitled1.ipynb ├── Untitled2.ipynb ├── __init__.py ├── ball.py ├── balzer.py ├── baseball.py ├── bb_test.py ├── bicycle.py ├── compute_q.ipynb ├── distributions.py ├── dme.py ├── dog_track_1d.py ├── doppler.py ├── ekf4.py ├── ekfloc.py ├── ekfloc2.py ├── ekfloc3.py ├── euler.py ├── fusion.py ├── gating.ipynb ├── gauss.py ├── gh.py ├── histogram.py ├── image_tracker.py ├── mkf_ellipse_test.py ├── noise.py ├── nonlinear_plots.py ├── quaternion.py ├── range_finder.py ├── satellite.ipynb ├── slam.py ├── slamekf.py ├── taylor.py ├── test1d.py ├── test_stats.py ├── train.py ├── two_radar.py ├── ukf_baseball.py ├── ukf_range.py ├── ukfloc.py ├── ukfloc2.py └── zarchan_ball.ipynb ├── figs ├── gh_errorbar1.png ├── gh_errorbar2.png ├── gh_errorbar3.png ├── gh_estimate1.png ├── gh_estimate2.png ├── gh_estimate3.png ├── gh_hypothesis1.png ├── gh_hypothesis2.png ├── gh_hypothesis3.png ├── gh_hypothesis4.png ├── gh_hypothesis5.png ├── gh_predict_update.png ├── residual_chart.png └── residual_chart_with_h.png ├── kf_book ├── 538.json ├── DogSimulation.py ├── LICENSE.txt ├── __init__.py ├── adaptive_internal.py ├── book_plots.py ├── custom.css ├── ekf_internal.py ├── gaussian_internal.py ├── gh_internal.py ├── gif_animate.py ├── kf_design_internal.py ├── kf_internal.py ├── mkf_internal.py ├── nonlinear_internal.py ├── nonlinear_plots.py ├── pf_internal.py ├── smoothing_internal.py └── ukf_internal.py ├── license.html ├── pdf ├── 6x9build_book.bat ├── __init__.py ├── book.tplx ├── book6x9.tplx ├── book_old.tplx ├── book_to_pdf.bat ├── build_book ├── build_book.bat ├── build_book6x9.bat ├── build_book_html.sh ├── build_html_ipynb.py ├── clean_book ├── clean_book.bat ├── formatting.py ├── html_book ├── html_build_book ├── html_build_book.bat ├── index.ipynb ├── make_chapter.bat ├── merge_book.py ├── nbmerge.py ├── readme.txt ├── report_old.tplx ├── rm_notebook.py ├── run_notebooks.bat ├── short_build_book ├── short_merge_book.py ├── to_pdf.py ├── update_pdf.bat └── update_pdf.sh ├── requirements.txt ├── 附录A-安装.ipynb ├── 附录B-标记和符号.ipynb ├── 附录D-H无穷滤波.ipynb ├── 附录E-集合卡尔曼滤波器.ipynb ├── 附录G-设计非线性卡尔曼滤波器.ipynb ├── 附录H-最小二乘滤波.ipynb └── 附录I-性能评估分析.ipynb /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text eol=lf 3 | *.py eol=lf 4 | *.ipynb eol=lf 5 | 6 | *.png binary 7 | *.jpg binary 8 | *.jpeg binary 9 | *.gif binary 10 | *.ico binary 11 | *.mov binary 12 | *.mp4 binary 13 | *.mp3 binary 14 | *.flv binary 15 | *.fla binary 16 | *.swf binary 17 | *.gz binary 18 | *.zip binary 19 | *.7z binary 20 | *.ttf binary 21 | *.eot binary 22 | *.woff binary 23 | *.pyc binary 24 | *.pdf binary -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.bak 2 | .ipynb_checkpoints 3 | __pycache__ 4 | *.pyc 5 | short.pdf 6 | *.aux 7 | *.out 8 | book.ipynb 9 | *.tex 10 | *.toc 11 | book.pdf 12 | book6x9.pdf 13 | Kalman_and_Bayesian_Filters_in_Python6x9.pdf 14 | Kalman_and_Bayesian_Filters_in_Python.pdf 15 | book_files 16 | tmp -------------------------------------------------------------------------------- /Supporting_Notebooks/Taylor-Series.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 3, 6 | "metadata": {}, 7 | "outputs": [ 8 | { 9 | "data": { 10 | "text/html": [ 11 | "\n", 12 | " \n", 22 | " " 23 | ], 24 | "text/plain": [ 25 | "" 26 | ] 27 | }, 28 | "execution_count": 3, 29 | "metadata": {}, 30 | "output_type": "execute_result" 31 | } 32 | ], 33 | "source": [ 34 | "#format the book\n", 35 | "from __future__ import division, print_function\n", 36 | "%matplotlib inline\n", 37 | "import sys\n", 38 | "sys.path.insert(0, '..')\n", 39 | "import book_format\n", 40 | "book_format.set_style()" 41 | ] 42 | }, 43 | { 44 | "cell_type": "markdown", 45 | "metadata": {}, 46 | "source": [ 47 | "# Linearizing with Taylor Series\n", 48 | "\n", 49 | "Taylor series represents a function as an infinite sum of terms. The terms are linear, even for a nonlinear function, so we can express any arbitrary nonlinear function using linear algebra. The cost of this choice is that unless we use an infinite number of terms the value we compute will be approximate rather than exact.\n", 50 | "\n", 51 | "The Taylor series for a real or complex function f(x) at x=a is the infinite series\n", 52 | "\n", 53 | "$$f(x) = f(a) + f'(a)(x-a) + \\frac{f''(a)}{2!}(x-a)^2 + \\, ...\\, + \\frac{f^{(n)}(a)}{n!}(x-a)^n + \\, ...$$\n", 54 | "\n", 55 | "where $f^{n}$ is the nth derivative of f. To compute the Taylor series for $f(x)=sin(x)$ at $x=0$ let's first work out the terms for f.\n", 56 | "\n", 57 | "$$\\begin{aligned}\n", 58 | "f^{0}(x) &= sin(x) ,\\ \\ &f^{0}(0) &= 0 \\\\\n", 59 | "f^{1}(x) &= cos(x),\\ \\ &f^{1}(0) &= 1 \\\\\n", 60 | "f^{2}(x) &= -sin(x),\\ \\ &f^{2}(0) &= 0 \\\\\n", 61 | "f^{3}(x) &= -cos(x),\\ \\ &f^{3}(0) &= -1 \\\\\n", 62 | "f^{4}(x) &= sin(x),\\ \\ &f^{4}(0) &= 0 \\\\\n", 63 | "f^{5}(x) &= cos(x),\\ \\ &f^{5}(0) &= 1\n", 64 | "\\end{aligned}\n", 65 | "$$\n", 66 | "\n", 67 | "Now we can substitute these values into the equation.\n", 68 | "\n", 69 | "$$\\sin(x) = \\frac{0}{0!}(x)^0 + \\frac{1}{1!}(x)^1 + \\frac{0}{2!}(x)^2 + \\frac{-1}{3!}(x)^3 + \\frac{0}{4!}(x)^4 + \\frac{-1}{5!}(x)^5 + ... $$\n", 70 | "\n", 71 | "And let's test this with some code:" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": 4, 77 | "metadata": {}, 78 | "outputs": [ 79 | { 80 | "name": "stdout", 81 | "output_type": "stream", 82 | "text": [ 83 | "estimate of sin(.3) is 0.30452025\n", 84 | "exact value of sin(.3) is 0.29552020666133955\n" 85 | ] 86 | } 87 | ], 88 | "source": [ 89 | "import numpy as np\n", 90 | "\n", 91 | "x = .3\n", 92 | "estimate = x + x**3/6 + x**5/120\n", 93 | "exact = np.sin(.3)\n", 94 | "\n", 95 | "print('estimate of sin(.3) is', estimate)\n", 96 | "print('exact value of sin(.3) is', exact)" 97 | ] 98 | }, 99 | { 100 | "cell_type": "markdown", 101 | "metadata": {}, 102 | "source": [ 103 | "This is not bad for only three terms. If you are curious, go ahead and implement this as a Python function to compute the series for an arbitrary number of terms." 104 | ] 105 | } 106 | ], 107 | "metadata": { 108 | "kernelspec": { 109 | "display_name": "Python 3", 110 | "language": "python", 111 | "name": "python3" 112 | }, 113 | "language_info": { 114 | "codemirror_mode": { 115 | "name": "ipython", 116 | "version": 3 117 | }, 118 | "file_extension": ".py", 119 | "mimetype": "text/x-python", 120 | "name": "python", 121 | "nbconvert_exporter": "python", 122 | "pygments_lexer": "ipython3", 123 | "version": "3.7.1" 124 | } 125 | }, 126 | "nbformat": 4, 127 | "nbformat_minor": 1 128 | } 129 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/__init__.py -------------------------------------------------------------------------------- /animations/02_no_info.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/animations/02_no_info.gif -------------------------------------------------------------------------------- /animations/02_simulate.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/animations/02_simulate.gif -------------------------------------------------------------------------------- /animations/04_gaussian_animate.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/animations/04_gaussian_animate.gif -------------------------------------------------------------------------------- /animations/05_dog_track.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/animations/05_dog_track.gif -------------------------------------------------------------------------------- /animations/05_volt_animate.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/animations/05_volt_animate.gif -------------------------------------------------------------------------------- /animations/13_particle_move.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/animations/13_particle_move.gif -------------------------------------------------------------------------------- /animations/multivariate_ellipse.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/animations/multivariate_ellipse.gif -------------------------------------------------------------------------------- /animations/multivariate_track1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/animations/multivariate_track1.gif -------------------------------------------------------------------------------- /animations/particle_filter_anim.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/animations/particle_filter_anim.gif -------------------------------------------------------------------------------- /book_format.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | from contextlib import contextmanager 20 | from IPython.core.display import HTML 21 | import json 22 | import matplotlib 23 | import matplotlib.pylab as pylab 24 | import matplotlib.pyplot as plt 25 | import numpy as np 26 | import os.path 27 | import sys 28 | import warnings 29 | from kf_book.book_plots import set_figsize, reset_figsize 30 | 31 | # version 1.4.3 of matplotlib has a bug that makes 32 | # it issue a spurious warning on every plot that 33 | # clutters the notebook output 34 | if matplotlib.__version__ == '1.4.3': 35 | warnings.simplefilter(action="ignore", category=FutureWarning) 36 | 37 | try: 38 | matplotlib.style.use('default') 39 | except: 40 | pass 41 | 42 | def test_filterpy_version(): 43 | 44 | import filterpy 45 | from distutils.version import LooseVersion 46 | 47 | v = filterpy.__version__ 48 | min_version = "1.4.4" 49 | if LooseVersion(v) < LooseVersion(min_version): 50 | raise Exception("Minimum FilterPy version supported is {}.\n" 51 | "Please install a more recent version.\n" 52 | " ex: pip install filterpy --upgrade".format( 53 | min_version)) 54 | 55 | 56 | # ensure that we have the correct filterpy loaded. This is 57 | # called when this module is imported at the top of each book 58 | # chapter so the reader can see that they need to update FilterPy. 59 | test_filterpy_version() 60 | 61 | pylab.rcParams['figure.max_open_warning'] = 50 62 | 63 | 64 | @contextmanager 65 | def numpy_precision(precision): 66 | old = np.get_printoptions()['precision'] 67 | np.set_printoptions(precision=precision) 68 | yield 69 | np.set_printoptions(old) 70 | 71 | @contextmanager 72 | def printoptions(*args, **kwargs): 73 | original = np.get_printoptions() 74 | np.set_printoptions(*args, **kwargs) 75 | yield 76 | np.set_printoptions(**original) 77 | 78 | def _decode_list(data): 79 | rv = [] 80 | for item in data: 81 | if isinstance(item, unicode): 82 | item = item.encode('utf-8') 83 | elif isinstance(item, list): 84 | item = _decode_list(item) 85 | elif isinstance(item, dict): 86 | item = _decode_dict(item) 87 | rv.append(item) 88 | return rv 89 | 90 | def _decode_dict(data): 91 | rv = {} 92 | for key, value in data.iteritems(): 93 | if isinstance(key, unicode): 94 | key = key.encode('utf-8') 95 | if isinstance(value, unicode): 96 | value = value.encode('utf-8') 97 | elif isinstance(value, list): 98 | value = _decode_list(value) 99 | elif isinstance(value, dict): 100 | value = _decode_dict(value) 101 | rv[key] = value 102 | return rv 103 | 104 | 105 | def set_style(): 106 | version = [int(version_no) for version_no in matplotlib.__version__.split('.')] 107 | 108 | try: 109 | if sys.version_info[0] >= 3: 110 | style = json.load(open("./kf_book/538.json")) 111 | else: 112 | style = json.load(open(".//kf_book/538.json"), object_hook=_decode_dict) 113 | plt.rcParams.update(style) 114 | except: 115 | pass 116 | np.set_printoptions(suppress=True, precision=3, 117 | threshold=10000., linewidth=70, 118 | formatter={'float':lambda x:' {:.3}'.format(x)}) 119 | 120 | # I don't know why I have to do this, but I have to call 121 | # with suppress a second time or the notebook doesn't suppress 122 | # exponents 123 | np.set_printoptions(suppress=True) 124 | reset_figsize() 125 | 126 | style = ''' 127 | 137 | ''' 138 | return HTML(style) 139 | -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: kf_bf 2 | dependencies: 3 | - python 4 | - ipython 5 | - jupyter 6 | - matplotlib 7 | - scipy 8 | - sympy 9 | - pip: 10 | - filterpy 11 | -------------------------------------------------------------------------------- /experiments/DiscreteBayes1D.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Fri May 2 11:48:55 2014 4 | 5 | @author: rlabbe 6 | """ 7 | from __future__ import division 8 | from __future__ import print_function 9 | 10 | import copy 11 | import numpy as np 12 | import numpy.random as random 13 | import matplotlib.pyplot as plt 14 | 15 | ''' should this be a class? seems like both sense and update are very 16 | problem specific 17 | ''' 18 | 19 | 20 | def bar_plot(pos, ylim=(0,1), title=None): 21 | plt.cla() 22 | ax = plt.gca() 23 | x = np.arange(len(pos)) 24 | ax.bar(x, pos, color='#30a2da') 25 | if ylim: 26 | plt.ylim(ylim) 27 | plt.xticks(x+0.4, x) 28 | if title is not None: 29 | plt.title(title) 30 | 31 | 32 | class DiscreteBayes1D(object): 33 | 34 | def __init__(self, world_map, belief=None): 35 | self.world_map = copy.deepcopy(world_map) 36 | self.N = len(world_map) 37 | 38 | if belief is None: 39 | # create belief, make all equally likely 40 | self.belief = np.empty(self.N) 41 | self.belief.fill (1./self.N) 42 | 43 | else: 44 | self.belief = copy.deepcopy(belief) 45 | 46 | # This will be used to temporarily store calculations of the new 47 | # belief. 'k' just means 'this iteration'. 48 | self.belief_k = np.empty(self.N) 49 | 50 | assert self.belief.shape == self.world_map.shape 51 | 52 | 53 | def _normalize (self): 54 | s = sum(self.belief) 55 | self.belief = self.belief/s 56 | 57 | def sense(self, Z, pHit, pMiss): 58 | for i in range (self.N): 59 | hit = (self.world_map[i] ==Z) 60 | self.belief_k[i] = self.belief[i] * (pHit*hit + pMiss*(1-hit)) 61 | 62 | # copy results to the belief vector using swap-copy idiom 63 | self.belief, self.belief_k = self.belief_k, self.belief 64 | self._normalize() 65 | 66 | def update(self, U, kernel): 67 | N = self.N 68 | kN = len(kernel) 69 | width = int((kN - 1) / 2) 70 | 71 | self.belief_k.fill(0) 72 | 73 | for i in range(N): 74 | for k in range (kN): 75 | index = (i + (width-k)-U) % N 76 | #print(i,k,index) 77 | self.belief_k[i] += self.belief[index] * kernel[k] 78 | 79 | # copy results to the belief vector using swap-copy idiom 80 | self.belief, self.belief_k = self.belief_k, self.belief 81 | 82 | def add_noise (Z, count): 83 | n= len(Z) 84 | for i in range(count): 85 | j = random.randint(0,n) 86 | Z[j] = random.randint(0,2) 87 | 88 | 89 | def animate_three_doors (loops=5): 90 | world = np.array([1,0,1,0,0,0,0,1,0,0,0,1,0,0,0,0,0]) 91 | #world = np.array([1,1,1,1,1]) 92 | #world = np.array([1,0,1,0,1,0]) 93 | 94 | 95 | f = DiscreteBayes1D(world) 96 | 97 | measurements = np.tile(world,loops) 98 | add_noise(measurements, 4) 99 | 100 | for m in measurements: 101 | f.sense (m, .8, .2) 102 | f.update(1, (.05, .9, .05)) 103 | 104 | bar_plot(f.belief) 105 | plt.pause(0.01) 106 | 107 | 108 | def _test_filter(): 109 | def is_near_equal(a,b): 110 | try: 111 | assert sum(abs(a-b)) < 0.001 112 | except: 113 | print(a, b) 114 | assert False 115 | 116 | def test_update_1(): 117 | f = DiscreteBayes1D(np.array([0,0,1,0,0]), np.array([0,0,.8,0,0])) 118 | f.update (1, [1]) 119 | is_near_equal (f.belief, np.array([0,0,0,.8,0])) 120 | 121 | f.update (1, [1]) 122 | is_near_equal (f.belief, np.array([0,0,0,0,.8])) 123 | 124 | f.update (1, [1]) 125 | is_near_equal (f.belief, np.array([.8,0,0,0,0])) 126 | 127 | f.update (-1, [1]) 128 | is_near_equal (f.belief, np.array([0,0,0,0,.8])) 129 | 130 | f.update (2, [1]) 131 | is_near_equal (f.belief, np.array([0,.8,0,0,0])) 132 | 133 | f.update (5, [1]) 134 | is_near_equal (f.belief, np.array([0,.8,0,0,0])) 135 | 136 | 137 | def test_undershoot(): 138 | f = DiscreteBayes1D(np.array([0,0,1,0,0]), np.array([0,0,.8,0,0])) 139 | f.update (2, [.2, .8,0.]) 140 | is_near_equal (f.belief, np.array([0,0,0,.16,.64])) 141 | 142 | def test_overshoot(): 143 | f = DiscreteBayes1D(np.array([0,0,1,0,0]), np.array([0,0,.8,0,0])) 144 | f.update (2, [0, .8, .2]) 145 | is_near_equal (f.belief, np.array([.16,0,0,0,.64])) 146 | 147 | 148 | test_update_1() 149 | test_undershoot() 150 | 151 | if __name__ == "__main__": 152 | 153 | _test_filter() 154 | 155 | 156 | 157 | animate_three_doors(loops=1) 158 | 159 | 160 | 161 | 162 | -------------------------------------------------------------------------------- /experiments/ILS.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | 4 | import numpy as np 5 | from numpy.linalg import norm, inv 6 | from numpy.random import randn 7 | from numpy import dot 8 | 9 | 10 | numpy.random.seed(1234) 11 | user_pos = np.array([1000, 100]) # d5, D6 12 | 13 | pred_user_pos = np.array([100, 0]) #d7, d8 14 | 15 | 16 | t_pos = np.asarray([[0, 1000], 17 | [0, -1000], 18 | [500, 500]], dtype=float) 19 | 20 | 21 | def transmitter_range(pos, transmitter_pos): 22 | """ Compute distance between position 'pos' and the list of positions 23 | in transmitter_pos""" 24 | 25 | N = len(transmitter_pos) 26 | rng = np.zeros(N) 27 | 28 | diff = np.asarray(pos) - transmitter_pos 29 | 30 | for i in range(N): 31 | rng[i] = norm(diff[i]) 32 | 33 | return norm(diff, axis=1) 34 | 35 | 36 | 37 | 38 | # compute measurement of where you are with respect to seach sensor 39 | 40 | 41 | rz= transmitter_range(user_pos, t_pos) # $B21,22 42 | 43 | # add some noise 44 | for i in range(len(rz)): 45 | rz[i] += randn() 46 | 47 | 48 | # now iterate on the predicted position 49 | pos = pred_user_pos 50 | 51 | 52 | def hx_range(pos, t_pos, r_est): 53 | N = len(t_pos) 54 | H = np.zeros((N, 2)) 55 | for j in range(N): 56 | H[j,0] = -(t_pos[j,0] - pos[0]) / r_est[j] 57 | H[j,1] = -(t_pos[j,1] - pos[1]) / r_est[j] 58 | return H 59 | 60 | 61 | def lop_ils(zs, t_pos, pos_est, hx, eps=1.e-6): 62 | """ iteratively estimates the solution to a set of measurement, given 63 | known transmitter locations""" 64 | pos = np.array(pos_est) 65 | 66 | converged = False 67 | for i in range(20): 68 | r_est = transmitter_range(pos, t_pos) #B32-B33 69 | print('iteration:', i) 70 | #print ('ra1, ra2', ra1, ra2) 71 | print() 72 | 73 | H=hx(pos, t_pos, r_est) 74 | 75 | Hinv = inv(dot(H.T, H)).dot(H.T) 76 | 77 | #update position estimate 78 | y = zs - r_est 79 | print('residual', y) 80 | 81 | Hy = np.dot(Hinv, y) 82 | print('Hy', Hy) 83 | 84 | pos = pos + Hy 85 | print('pos', pos) 86 | 87 | print() 88 | print() 89 | 90 | if max(abs(Hy)) < eps: 91 | converged = True 92 | break 93 | 94 | return pos, converged 95 | 96 | 97 | 98 | print(lop_ils(rz, t_pos, (900,90), hx=hx_range)) 99 | 100 | 101 | 102 | ##################### 103 | """ 104 | # compute measurement (simulation) 105 | rza1, rza2 = transmitter_range(user_pos) # $B21,22 106 | 107 | rza1 += randn() 108 | rza2 += randn() 109 | 110 | # now iterate on the predicted position 111 | pos = pred_user_pos 112 | 113 | 114 | for i in range(10): 115 | ra1, ra2 = transmitter_range(pos) #B32-B33 116 | print('iteration:', i) 117 | print ('ra1, ra2', ra1, ra2) 118 | print() 119 | 120 | H = np.array([[-(t1_pos[0] - pos[0]) / ra1, -(t1_pos[1] - pos[1]) / ra1], 121 | [-(t2_pos[0] - pos[0]) / ra2, -(t2_pos[1] - pos[1]) / ra2]]) 122 | Hinv = inv(H) 123 | 124 | #update position estimate 125 | residual_t1 = rza1 - ra1 126 | residual_t2 = rza2 - ra2 127 | y = np.array([[residual_t1], [residual_t2]]) 128 | print('residual', y.T) 129 | 130 | 131 | Hy = np.dot(Hinv, y) 132 | 133 | pos = pos + Hy[:,0] 134 | print('pos', pos) 135 | 136 | print() 137 | print() 138 | 139 | if (max(abs(y)) < 1.e-6): 140 | break 141 | """ 142 | -------------------------------------------------------------------------------- /experiments/RobotLocalizationParticleFilter_2.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | import numpy as np 20 | 21 | from numpy.random import randn, random, uniform 22 | import scipy.stats 23 | 24 | 25 | 26 | def create_uniform_particles( x_range, y_range, hdg_range, N): 27 | particles = np.empty((N, 3)) 28 | particles[:, 0] = uniform(x_range[0], x_range[1], size=N) 29 | particles[:, 1] = uniform(y_range[0], y_range[1], size=N) 30 | particles[:, 2] = uniform(hdg_range[0], hdg_range[1], size=N) 31 | particles[:, 2] %= 2 * np.pi 32 | 33 | return particles 34 | 35 | 36 | def create_gaussian_particles( mean, var, N): 37 | particles = np.empty((N, 3)) 38 | particles[:, 0] = mean[0] + randn(N)*var[0] 39 | particles[:, 1] = mean[1] + randn(N)*var[1] 40 | particles[:, 2] = mean[2] + randn(N)*var[2] 41 | particles[:, 2] %= 2 * np.pi 42 | return particles 43 | 44 | 45 | 46 | def predict(particles, u, std, dt=1.): 47 | """ move according to control input u (heading change, velocity) 48 | with noise `std (std_heading, std`""" 49 | 50 | N = len(particles) 51 | 52 | particles[:, 2] += u[0] + randn(N) * std[0] 53 | particles[:, 2] %= 2 * np.pi 54 | 55 | d = u[1]*dt + randn(N) * std[1] 56 | particles[:, 0] += np.cos(particles[:, 2]) * d 57 | particles[:, 1] += np.sin(particles[:, 2]) * d 58 | 59 | 60 | def update(particles, weights, z, R, landmarks): 61 | weights.fill(1.) 62 | for i, landmark in enumerate(landmarks): 63 | distance = np.linalg.norm(particles[:, 0:2] - landmark, axis=1) 64 | weights *= scipy.stats.norm(distance, R).pdf(z[i]) 65 | 66 | weights += 1.e-300 67 | weights /= sum(weights) # normalize 68 | 69 | 70 | def neff(weights): 71 | return 1. / np.sum(np.square(weights)) 72 | 73 | 74 | def resample(particles, weights): 75 | N = len(particles) 76 | cumulative_sum = np.cumsum(weights) 77 | cumulative_sum[-1] = 1. # avoid round-off error 78 | indexes = np.searchsorted(cumulative_sum, random(N)) 79 | 80 | # resample according to indexes 81 | particles[:] = particles[indexes] 82 | weights[:] = weights[indexes] 83 | weights /= np.sum(weights) # normalize 84 | 85 | 86 | def resample_from_index(particles, weights, indexes): 87 | particles[:] = particles[indexes] 88 | weights[:] = weights[indexes] 89 | weights /= np.sum(weights) 90 | 91 | 92 | def estimate(particles, weights): 93 | """ returns mean and variance """ 94 | pos = particles[:, 0:2] 95 | mu = np.average(pos, weights=weights, axis=0) 96 | var = np.average((pos - mu)**2, weights=weights, axis=0) 97 | 98 | return mu, var 99 | 100 | 101 | def mean(particles, weights): 102 | """ returns weighted mean position""" 103 | return np.average(particles[:, 0:2], weights=weights, axis=0) 104 | 105 | 106 | 107 | def residual_resample(w): 108 | 109 | N = len(w) 110 | 111 | w_ints = np.floor(N*w).astype(int) 112 | residual = w - w_ints 113 | residual /= sum(residual) 114 | 115 | indexes = np.zeros(N, 'i') 116 | k = 0 117 | for i in range(N): 118 | for j in range(w_ints[i]): 119 | indexes[k] = i 120 | k += 1 121 | cumsum = np.cumsum(residual) 122 | cumsum[N-1] = 1. 123 | for j in range(k, N): 124 | indexes[j] = np.searchsorted(cumsum, random()) 125 | 126 | return indexes 127 | 128 | 129 | 130 | def residual_resample2(w): 131 | 132 | N = len(w) 133 | 134 | w_ints =np.floor(N*w).astype(int) 135 | 136 | R = np.sum(w_ints) 137 | m_rdn = N - R 138 | 139 | Ws = (N*w - w_ints)/ m_rdn 140 | indexes = np.zeros(N, 'i') 141 | i = 0 142 | for j in range(N): 143 | for k in range(w_ints[j]): 144 | indexes[i] = j 145 | i += 1 146 | cumsum = np.cumsum(Ws) 147 | cumsum[N-1] = 1 # just in case 148 | 149 | for j in range(i, N): 150 | indexes[j] = np.searchsorted(cumsum, random()) 151 | 152 | return indexes 153 | 154 | 155 | 156 | def systemic_resample(w): 157 | N = len(w) 158 | Q = np.cumsum(w) 159 | indexes = np.zeros(N, 'int') 160 | t = np.linspace(0, 1-1/N, N) + random()/N 161 | 162 | i, j = 0, 0 163 | while i < N and j < N: 164 | while Q[j] < t[i]: 165 | j += 1 166 | indexes[i] = j 167 | i += 1 168 | 169 | return indexes 170 | 171 | 172 | 173 | 174 | 175 | def Gaussian(mu, sigma, x): 176 | 177 | # calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma 178 | g = (np.exp(-((mu - x) ** 2) / (sigma ** 2) / 2.0) / 179 | np.sqrt(2.0 * np.pi * (sigma ** 2))) 180 | for i in range(len(g)): 181 | g[i] = max(g[i], 1.e-229) 182 | return g 183 | 184 | 185 | if __name__ == '__main__': 186 | 187 | DO_PLOT_PARTICLES = False 188 | from numpy.random import seed 189 | import matplotlib.pyplot as plt 190 | 191 | #plt.figure() 192 | 193 | seed(5) 194 | for count in range(10): 195 | print() 196 | print(count) 197 | 198 | N = 4000 199 | sensor_std_err = .1 200 | landmarks = np.array([[-1, 2], [2,4], [10,6], [18,25]]) 201 | NL = len(landmarks) 202 | 203 | 204 | particles = create_uniform_particles((0,20), (0,20), (0, 6.28), N) 205 | weights = np.zeros(N) 206 | 207 | #if DO_PLOT_PARTICLES: 208 | # plt.scatter(particles[:, 0], particles[:, 1], alpha=.2, color='g') 209 | 210 | xs = [] 211 | for x in range(18): 212 | zs = [] 213 | pos=(x+1, x+1) 214 | 215 | for landmark in landmarks: 216 | d = np.sqrt((landmark[0]-pos[0])**2 + (landmark[1]-pos[1])**2) 217 | zs.append(d + randn()*sensor_std_err) 218 | 219 | 220 | zs = np.linalg.norm(landmarks - pos, axis=1) + randn(NL)*sensor_std_err 221 | 222 | 223 | # move diagonally forward to (x+1, x+1) 224 | 225 | predict(particles, (0.00, 1.414), (.2, .05)) 226 | 227 | update(particles, weights, z=zs, R=sensor_std_err, landmarks=landmarks) 228 | if x == 0: 229 | print(max(weights)) 230 | #while abs(pf.neff() -N) < .1: 231 | # print('neffing') 232 | # pf.create_uniform_particles((0,20), (0,20), (0, 6.28)) 233 | # pf.update(z=zs) 234 | #print(pf.neff()) 235 | #indexes = residual_resample2(pf.weights) 236 | indexes = systemic_resample(weights) 237 | 238 | resample_from_index(particles, weights, indexes) 239 | #pf.resample() 240 | 241 | mu, var = estimate(particles, weights) 242 | xs.append(mu) 243 | if DO_PLOT_PARTICLES: 244 | plt.scatter(particles[:, 0], particles[:, 1], alpha=.2) 245 | plt.scatter(pos[0], pos[1], marker='*', color='r') 246 | plt.scatter(mu[0], mu[1], marker='s', color='r') 247 | plt.pause(.01) 248 | 249 | xs = np.array(xs) 250 | plt.plot(xs[:, 0], xs[:, 1]) 251 | plt.show() 252 | -------------------------------------------------------------------------------- /experiments/RungeKutta.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Jul 05 09:54:39 2014 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from __future__ import division, print_function 9 | import matplotlib.pyplot as plt 10 | from scipy.integrate import ode 11 | import math 12 | import numpy as np 13 | from numpy import random, radians, cos, sin 14 | 15 | 16 | class BallTrajectory2D(object): 17 | def __init__(self, x0, y0, velocity, theta_deg=0., g=9.8, noise=[0.0,0.0]): 18 | theta = radians(theta_deg) 19 | self.vx0 = velocity * cos(theta) 20 | self.vy0 = velocity * sin(theta) 21 | 22 | self.x0 = x0 23 | self.y0 = y0 24 | self.x = x 25 | 26 | self.g = g 27 | self.noise = noise 28 | 29 | def position(self, t): 30 | """ returns (x,y) tuple of ball position at time t""" 31 | 32 | self.x = self.vx0*t + self.x0 33 | self.y = -0.5*self.g*t**2 + self.vy0*t + self.y0 34 | 35 | return (self.x +random.randn()*self.noise[0], self.y +random.randn()*self.noise[1]) 36 | 37 | 38 | 39 | class BallEuler(object): 40 | def __init__(self, y=100., vel=10., omega=0): 41 | self.x = 0. 42 | self.y = y 43 | omega = radians(omega) 44 | self.vel = vel*np.cos(omega) 45 | self.y_vel = vel*np.sin(omega) 46 | 47 | 48 | 49 | def step (self, dt): 50 | 51 | g = -9.8 52 | 53 | 54 | self.x += self.vel*dt 55 | self.y += self.y_vel*dt 56 | 57 | self.y_vel += g*dt 58 | 59 | #print self.x, self.y 60 | 61 | 62 | 63 | def rk4(y, x, dx, f): 64 | """computes 4th order Runge-Kutta for dy/dx. 65 | y is the initial value for y 66 | x is the initial value for x 67 | dx is the difference in x (e.g. the time step) 68 | f is a callable function (y, x) that you supply to compute dy/dx for 69 | the specified values. 70 | """ 71 | 72 | k1 = dx * f(y, x) 73 | k2 = dx * f(y + 0.5*k1, x + 0.5*dx) 74 | k3 = dx * f(y + 0.5*k2, x + 0.5*dx) 75 | k4 = dx * f(y + k3, x + dx) 76 | 77 | return y + (k1 + 2*k2 + 2*k3 + k4) / 6. 78 | 79 | 80 | 81 | 82 | def fx(x,t): 83 | return fx.vel 84 | 85 | def fy(y,t): 86 | return fy.vel - 9.8*t 87 | 88 | 89 | class BallRungeKutta(object): 90 | def __init__(self, x=0, y=100., vel=10., omega = 0.0): 91 | self.x = x 92 | self.y = y 93 | self.t = 0 94 | 95 | omega = math.radians(omega) 96 | 97 | fx.vel = math.cos(omega) * vel 98 | fy.vel = math.sin(omega) * vel 99 | 100 | def step (self, dt): 101 | self.x = rk4 (self.x, self.t, dt, fx) 102 | self.y = rk4 (self.y, self.t, dt, fy) 103 | self.t += dt 104 | print(fx.vel) 105 | return (self.x, self.y) 106 | 107 | 108 | def ball_scipy(y0, vel, omega, dt): 109 | 110 | vel_y = math.sin(math.radians(omega)) * vel 111 | 112 | def f(t,y): 113 | return vel_y-9.8*t 114 | 115 | solver = ode(f).set_integrator('dopri5') 116 | solver.set_initial_value(y0) 117 | 118 | ys = [y0] 119 | while brk.y >= 0: 120 | t += dt 121 | brk.step (dt) 122 | 123 | ys.append(solver.integrate(t)) 124 | 125 | 126 | def RK4(f): 127 | return lambda t, y, dt: ( 128 | lambda dy1: ( 129 | lambda dy2: ( 130 | lambda dy3: ( 131 | lambda dy4: (dy1 + 2*dy2 + 2*dy3 + dy4)/6 132 | )( dt * f( t + dt , y + dy3 ) ) 133 | )( dt * f( t + dt/2, y + dy2/2 ) ) 134 | )( dt * f( t + dt/2, y + dy1/2 ) ) 135 | )( dt * f( t , y ) ) 136 | 137 | def theory(t): return (t**2 + 4)**2 /16 138 | 139 | from math import sqrt 140 | dy = RK4(lambda t, y: t*sqrt(y)) 141 | 142 | t, y, dt = 0., 1., .1 143 | while t <= 10: 144 | if abs(round(t) - t) < 1e-5: 145 | print("y(%2.1f)\t= %4.6f \t error: %4.6g" % (t, y, abs(y - theory(t)))) 146 | 147 | t, y = t + dt, y + dy(t, y, dt) 148 | 149 | t = 0. 150 | y=1. 151 | 152 | def test(y, t): 153 | return t*sqrt(y) 154 | 155 | while t <= 10: 156 | if abs(round(t) - t) < 1e-5: 157 | print("y(%2.1f)\t= %4.6f \t error: %4.6g" % (t, y, abs(y - theory(t)))) 158 | 159 | y = rk4(y, t, dt, test) 160 | t += dt 161 | 162 | if __name__ == "__main__": 163 | 1/0 164 | 165 | dt = 1./30 166 | y0 = 15. 167 | vel = 100. 168 | omega = 30. 169 | vel_y = math.sin(math.radians(omega)) * vel 170 | 171 | def f(t,y): 172 | return vel_y-9.8*t 173 | 174 | be = BallEuler (y=y0, vel=vel,omega=omega) 175 | #be = BallTrajectory2D (x0=0, y0=y0, velocity=vel, theta_deg = omega) 176 | ball_rk = BallRungeKutta (y=y0, vel=vel, omega=omega) 177 | 178 | while be.y >= 0: 179 | be.step (dt) 180 | ball_rk.step(dt) 181 | 182 | print (ball_rk.y - be.y) 183 | 184 | ''' 185 | p1 = plt.scatter (be.x, be.y, color='red') 186 | p2 = plt.scatter (ball_rk.x, ball_rk.y, color='blue', marker='v') 187 | 188 | plt.legend([p1,p2], ['euler', 'runge kutta']) 189 | ''' -------------------------------------------------------------------------------- /experiments/Untitled0.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "metadata": { 3 | "name": "", 4 | "signature": "sha256:d69b1215417e5c675abdbd0de4e82039e393b8a3be44f039c407180b720444c8" 5 | }, 6 | "nbformat": 3, 7 | "nbformat_minor": 0, 8 | "worksheets": [ 9 | { 10 | "cells": [ 11 | { 12 | "cell_type": "code", 13 | "collapsed": false, 14 | "input": [ 15 | "from IPython.display import Image\n", 16 | "Image(url='http://python.org/images/python-logo.gif', format='gif')" 17 | ], 18 | "language": "python", 19 | "metadata": {}, 20 | "outputs": [ 21 | { 22 | "html": [ 23 | "" 24 | ], 25 | "metadata": {}, 26 | "output_type": "pyout", 27 | "prompt_number": 5, 28 | "text": [ 29 | "" 30 | ] 31 | } 32 | ], 33 | "prompt_number": 5 34 | }, 35 | { 36 | "cell_type": "code", 37 | "collapsed": false, 38 | "input": [ 39 | "Image(url='https://raw.githubusercontent.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/master/basic_animation.gif', format='gif')\n" 40 | ], 41 | "language": "python", 42 | "metadata": {}, 43 | "outputs": [ 44 | { 45 | "html": [ 46 | "" 47 | ], 48 | "metadata": {}, 49 | "output_type": "pyout", 50 | "prompt_number": 15, 51 | "text": [ 52 | "" 53 | ] 54 | } 55 | ], 56 | "prompt_number": 15 57 | }, 58 | { 59 | "cell_type": "markdown", 60 | "metadata": {}, 61 | "source": [ 62 | "" 63 | ] 64 | } 65 | ], 66 | "metadata": {} 67 | } 68 | ] 69 | } -------------------------------------------------------------------------------- /experiments/Untitled1.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "metadata": { 3 | "name": "", 4 | "signature": "sha256:fc6923e502800422d1ca66394e71d567efb82c4dcd09c273561149f8d3b00fe5" 5 | }, 6 | "nbformat": 3, 7 | "nbformat_minor": 0, 8 | "worksheets": [ 9 | { 10 | "cells": [ 11 | { 12 | "cell_type": "heading", 13 | "level": 1, 14 | "metadata": {}, 15 | "source": [ 16 | "Complementary Filter" 17 | ] 18 | }, 19 | { 20 | "cell_type": "markdown", 21 | "metadata": {}, 22 | "source": [ 23 | "Convert continuous transfer function into discrete\n", 24 | "\n" 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "collapsed": false, 30 | "input": [ 31 | "import scipy.signal as sig\n", 32 | "from math import sqrt\n", 33 | "\n", 34 | "dt = 0.01\n", 35 | "ki = 0.1**2\n", 36 | "kp = sqrt(2)*0.1\n", 37 | "\n", 38 | "num = [kp, ki]\n", 39 | "den = [1, 0.]\n", 40 | "sig.cont2discrete(sys=(num,den), dt=0.1)" 41 | ], 42 | "language": "python", 43 | "metadata": {}, 44 | "outputs": [ 45 | { 46 | "metadata": {}, 47 | "output_type": "pyout", 48 | "prompt_number": 8, 49 | "text": [ 50 | "(array([[ 0.14142136, -0.14042136]]), array([ 1., -1.]), 0.1)" 51 | ] 52 | } 53 | ], 54 | "prompt_number": 8 55 | }, 56 | { 57 | "cell_type": "code", 58 | "collapsed": false, 59 | "input": [ 60 | "a = 5\n", 61 | "num = [a, 0]\n", 62 | "den = [a, 1]\n", 63 | "sig.cont2discrete(sys=(num,den), dt=0.1)" 64 | ], 65 | "language": "python", 66 | "metadata": {}, 67 | "outputs": [ 68 | { 69 | "metadata": {}, 70 | "output_type": "pyout", 71 | "prompt_number": 11, 72 | "text": [ 73 | "(array([[ 1., -1.]]), array([ 1. , -0.98019867]), 0.1)" 74 | ] 75 | } 76 | ], 77 | "prompt_number": 11 78 | }, 79 | { 80 | "cell_type": "code", 81 | "collapsed": false, 82 | "input": [ 83 | "from sympy import *\n", 84 | "from sympy.abc import *\n", 85 | "a = Symbol('a', positive=True)\n", 86 | "foo = (s/(s+a)).rewrite()\n", 87 | "print(foo)\n", 88 | "inverse_laplace_transform(1/((s+a)), s, t)" 89 | ], 90 | "language": "python", 91 | "metadata": {}, 92 | "outputs": [ 93 | { 94 | "output_type": "stream", 95 | "stream": "stdout", 96 | "text": [ 97 | "s/(a + s)\n" 98 | ] 99 | }, 100 | { 101 | "metadata": {}, 102 | "output_type": "pyout", 103 | "prompt_number": 39, 104 | "text": [ 105 | "exp(-a*t)*Heaviside(t)" 106 | ] 107 | } 108 | ], 109 | "prompt_number": 39 110 | }, 111 | { 112 | "cell_type": "code", 113 | "collapsed": false, 114 | "input": [], 115 | "language": "python", 116 | "metadata": {}, 117 | "outputs": [] 118 | }, 119 | { 120 | "cell_type": "code", 121 | "collapsed": false, 122 | "input": [], 123 | "language": "python", 124 | "metadata": {}, 125 | "outputs": [] 126 | }, 127 | { 128 | "cell_type": "code", 129 | "collapsed": false, 130 | "input": [], 131 | "language": "python", 132 | "metadata": {}, 133 | "outputs": [] 134 | }, 135 | { 136 | "cell_type": "code", 137 | "collapsed": false, 138 | "input": [], 139 | "language": "python", 140 | "metadata": {}, 141 | "outputs": [] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "collapsed": false, 146 | "input": [], 147 | "language": "python", 148 | "metadata": {}, 149 | "outputs": [] 150 | }, 151 | { 152 | "cell_type": "code", 153 | "collapsed": false, 154 | "input": [], 155 | "language": "python", 156 | "metadata": {}, 157 | "outputs": [] 158 | }, 159 | { 160 | "cell_type": "code", 161 | "collapsed": false, 162 | "input": [], 163 | "language": "python", 164 | "metadata": {}, 165 | "outputs": [] 166 | }, 167 | { 168 | "cell_type": "code", 169 | "collapsed": false, 170 | "input": [], 171 | "language": "python", 172 | "metadata": {}, 173 | "outputs": [] 174 | } 175 | ], 176 | "metadata": {} 177 | } 178 | ] 179 | } -------------------------------------------------------------------------------- /experiments/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/experiments/__init__.py -------------------------------------------------------------------------------- /experiments/balzer.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Jan 24 15:20:47 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | try: 12 | data 13 | except: 14 | cols = ('time','millis','ax','ay','az','rollrate','pitchrate','yawrate', 15 | 'roll','pitch','yaw','speed','course','latitude','longitude', 16 | 'altitude','pdop','hdop','vdop','epe') 17 | data = np.genfromtxt('2014-03-26-000-Data.csv', delimiter=',', 18 | names=True, usecols=cols).view(np.recarray) 19 | 20 | 21 | 22 | plt.subplot(311) 23 | plt.plot(data.ax) 24 | plt.subplot(312) 25 | plt.plot(data.speed) 26 | plt.subplot(313) 27 | plt.plot(data.course) 28 | plt.tight_layout() 29 | 30 | plt.figure() 31 | plt.subplot(311) 32 | plt.plot(data.pitch) 33 | plt.subplot(312) 34 | plt.plot(data.roll) 35 | plt.subplot(313) 36 | plt.plot(data.yaw) 37 | plt.figure() 38 | plt.plot(data.longitude, data.latitude) -------------------------------------------------------------------------------- /experiments/baseball.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | 4 | """ 5 | Computes the trajectory of a stitched baseball with air drag. 6 | Takes altitude into account (higher altitude means further travel) and the 7 | stitching on the baseball influencing drag. 8 | 9 | This is based on the book Computational Physics by N. Giordano. 10 | 11 | The takeaway point is that the drag coefficient on a stitched baseball is 12 | LOWER the higher its velocity, which is somewhat counterintuitive. 13 | """ 14 | 15 | 16 | from __future__ import division 17 | import math 18 | import matplotlib.pyplot as plt 19 | from numpy.random import randn 20 | import numpy as np 21 | 22 | 23 | class BaseballPath(object): 24 | def __init__(self, x0, y0, launch_angle_deg, velocity_ms, noise=(1.0,1.0)): 25 | """ Create baseball path object in 2D (y=height above ground) 26 | 27 | x0,y0 initial position 28 | launch_angle_deg angle ball is travelling respective to ground plane 29 | velocity_ms speeed of ball in meters/second 30 | noise amount of noise to add to each reported position in (x,y) 31 | """ 32 | 33 | omega = radians(launch_angle_deg) 34 | self.v_x = velocity_ms * cos(omega) 35 | self.v_y = velocity_ms * sin(omega) 36 | 37 | self.x = x0 38 | self.y = y0 39 | 40 | self.noise = noise 41 | 42 | 43 | def drag_force (self, velocity): 44 | """ Returns the force on a baseball due to air drag at 45 | the specified velocity. Units are SI 46 | """ 47 | B_m = 0.0039 + 0.0058 / (1. + exp((velocity-35.)/5.)) 48 | return B_m * velocity 49 | 50 | 51 | def update(self, dt, vel_wind=0.): 52 | """ compute the ball position based on the specified time step and 53 | wind velocity. Returns (x,y) position tuple. 54 | """ 55 | 56 | # Euler equations for x and y 57 | self.x += self.v_x*dt 58 | self.y += self.v_y*dt 59 | 60 | # force due to air drag 61 | v_x_wind = self.v_x - vel_wind 62 | 63 | v = sqrt (v_x_wind**2 + self.v_y**2) 64 | F = self.drag_force(v) 65 | 66 | # Euler's equations for velocity 67 | self.v_x = self.v_x - F*v_x_wind*dt 68 | self.v_y = self.v_y - 9.81*dt - F*self.v_y*dt 69 | 70 | return (self.x + random.randn()*self.noise[0], 71 | self.y + random.randn()*self.noise[1]) 72 | 73 | 74 | 75 | def a_drag (vel, altitude): 76 | """ returns the drag coefficient of a baseball at a given velocity (m/s) 77 | and altitude (m). 78 | """ 79 | 80 | v_d = 35 81 | delta = 5 82 | y_0 = 1.0e4 83 | 84 | val = 0.0039 + 0.0058 / (1 + math.exp((vel - v_d)/delta)) 85 | val *= math.exp(-altitude / y_0) 86 | 87 | return val 88 | 89 | def compute_trajectory_vacuum (v_0_mph, 90 | theta, 91 | dt=0.01, 92 | noise_scale=0.0, 93 | x0=0., y0 = 0.): 94 | theta = math.radians(theta) 95 | 96 | x = x0 97 | y = y0 98 | 99 | v0_y = v_0_mph * math.sin(theta) 100 | v0_x = v_0_mph * math.cos(theta) 101 | 102 | xs = [] 103 | ys = [] 104 | 105 | t = dt 106 | while y >= 0: 107 | x = v0_x*t + x0 108 | y = -0.5*9.8*t**2 + v0_y*t + y0 109 | 110 | xs.append (x + randn() * noise_scale) 111 | ys.append (y + randn() * noise_scale) 112 | 113 | t += dt 114 | 115 | return (xs, ys) 116 | 117 | 118 | 119 | def compute_trajectory(v_0_mph, theta, v_wind_mph=0, alt_ft = 0.0, dt = 0.01): 120 | g = 9.8 121 | 122 | ### comput 123 | theta = math.radians(theta) 124 | # initial velocity in direction of travel 125 | v_0 = v_0_mph * 0.447 # mph to m/s 126 | 127 | # velocity components in x and y 128 | v_x = v_0 * math.cos(theta) 129 | v_y = v_0 * math.sin(theta) 130 | 131 | v_wind = v_wind_mph * 0.447 # mph to m/s 132 | altitude = alt_ft / 3.28 # to m/s 133 | 134 | ground_level = altitude 135 | 136 | x = [0.0] 137 | y = [altitude] 138 | 139 | i = 0 140 | while y[i] >= ground_level: 141 | 142 | v = math.sqrt((v_x - v_wind) **2+ v_y**2) 143 | 144 | x.append(x[i] + v_x * dt) 145 | y.append(y[i] + v_y * dt) 146 | 147 | v_x = v_x - a_drag(v, altitude) * v * (v_x - v_wind) * dt 148 | v_y = v_y - a_drag(v, altitude) * v * v_y * dt - g * dt 149 | i += 1 150 | 151 | # meters to yards 152 | np.multiply (x, 1.09361) 153 | np.multiply (y, 1.09361) 154 | 155 | return (x,y) 156 | 157 | 158 | def predict (x0, y0, x1, y1, dt, time): 159 | g = 10.724*dt*dt 160 | 161 | v_x = x1-x0 162 | v_y = y1-y0 163 | v = math.sqrt(v_x**2 + v_y**2) 164 | 165 | x = x1 166 | y = y1 167 | 168 | 169 | while time > 0: 170 | v_x = v_x - a_drag(v, y) * v * v_x 171 | v_y = v_y - a_drag(v, y) * v * v_y - g 172 | 173 | x = x + v_x 174 | y = y + v_y 175 | 176 | time -= dt 177 | 178 | return (x,y) 179 | 180 | 181 | 182 | 183 | if __name__ == "__main__": 184 | x,y = compute_trajectory(v_0_mph = 110., theta=35., v_wind_mph = 0., alt_ft=5000.) 185 | 186 | plt.plot (x, y) 187 | plt.show() 188 | 189 | 190 | 191 | 192 | -------------------------------------------------------------------------------- /experiments/bb_test.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Wed Jun 4 12:33:38 2014 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from __future__ import print_function,division 9 | from filterpy.kalman import KalmanFilter 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import baseball 13 | from numpy.random import randn 14 | 15 | def ball_filter6(dt,R=1., Q = 0.1): 16 | f1 = KalmanFilter(dim=6) 17 | g = 10 18 | 19 | f1.F = np.mat ([[1., dt, dt**2, 0, 0, 0], 20 | [0, 1., dt, 0, 0, 0], 21 | [0, 0, 1., 0, 0, 0], 22 | [0, 0, 0., 1., dt, -0.5*dt*dt*g], 23 | [0, 0, 0, 0, 1., -g*dt], 24 | [0, 0, 0, 0, 0., 1.]]) 25 | 26 | f1.H = np.mat([[1,0,0,0,0,0], 27 | [0,0,0,0,0,0], 28 | [0,0,0,0,0,0], 29 | [0,0,0,1,0,0], 30 | [0,0,0,0,0,0], 31 | [0,0,0,0,0,0]]) 32 | 33 | 34 | f1.R = np.mat(np.eye(6)) * R 35 | 36 | f1.Q = np.zeros((6,6)) 37 | f1.Q[2,2] = Q 38 | f1.Q[5,5] = Q 39 | f1.x = np.mat([0, 0 , 0, 0, 0, 0]).T 40 | f1.P = np.eye(6) * 50. 41 | f1.B = 0. 42 | f1.u = 0 43 | 44 | return f1 45 | 46 | 47 | def ball_filter4(dt,R=1., Q = 0.1): 48 | f1 = KalmanFilter(dim=4) 49 | g = 10 50 | 51 | f1.F = np.mat ([[1., dt, 0, 0,], 52 | [0, 1., 0, 0], 53 | [0, 0, 1., dt], 54 | [0, 0, 0., 1.]]) 55 | 56 | f1.H = np.mat([[1,0,0,0], 57 | [0,0,0,0], 58 | [0,0,1,0], 59 | [0,0,0,0]]) 60 | 61 | 62 | 63 | f1.B = np.mat([[0,0,0,0], 64 | [0,0,0,0], 65 | [0,0,1.,0], 66 | [0,0,0,1.]]) 67 | 68 | f1.u = np.mat([[0], 69 | [0], 70 | [-0.5*g*dt**2], 71 | [-g*dt]]) 72 | 73 | f1.R = np.mat(np.eye(4)) * R 74 | 75 | f1.Q = np.zeros((4,4)) 76 | f1.Q[1,1] = Q 77 | f1.Q[3,3] = Q 78 | f1.x = np.mat([0, 0 , 0, 0]).T 79 | f1.P = np.eye(4) * 50. 80 | return f1 81 | 82 | 83 | def plot_ball_filter6 (f1, zs, skip_start=-1, skip_end=-1): 84 | xs, ys = [],[] 85 | pxs, pys = [],[] 86 | 87 | for i,z in enumerate(zs): 88 | m = np.mat([z[0], 0, 0, z[1], 0, 0]).T 89 | 90 | f1.predict () 91 | 92 | if i == skip_start: 93 | x2 = xs[-2] 94 | x1 = xs[-1] 95 | y2 = ys[-2] 96 | y1 = ys[-1] 97 | 98 | if i >= skip_start and i <= skip_end: 99 | #x,y = baseball.predict (x2, y2, x1, y1, 1/30, 1/30* (i-skip_start+1)) 100 | x,y = baseball.predict (xs[-2], ys[-2], xs[-1], ys[-1], 1/30, 1/30) 101 | 102 | m[0] = x 103 | m[3] = y 104 | #print ('skip', i, f1.x[2],f1.x[5]) 105 | 106 | f1.update(m) 107 | 108 | 109 | ''' 110 | if i >= skip_start and i <= skip_end: 111 | #f1.x[2] = -0.1 112 | #f1.x[5] = -17. 113 | pass 114 | else: 115 | f1.update (m) 116 | 117 | ''' 118 | 119 | xs.append (f1.x[0,0]) 120 | ys.append (f1.x[3,0]) 121 | pxs.append (z[0]) 122 | pys.append(z[1]) 123 | 124 | if i > 0 and z[1] < 0: 125 | break; 126 | 127 | p1, = plt.plot (xs, ys, 'r--') 128 | p2, = plt.plot (pxs, pys) 129 | plt.axis('equal') 130 | plt.legend([p1,p2], ['filter', 'measurement'], 2) 131 | plt.xlim([0,xs[-1]]) 132 | plt.show() 133 | 134 | 135 | 136 | def plot_ball_filter4 (f1, zs, skip_start=-1, skip_end=-1): 137 | xs, ys = [],[] 138 | pxs, pys = [],[] 139 | 140 | for i,z in enumerate(zs): 141 | m = np.mat([z[0], 0, z[1], 0]).T 142 | 143 | f1.predict () 144 | 145 | if i == skip_start: 146 | x2 = xs[-2] 147 | x1 = xs[-1] 148 | y2 = ys[-2] 149 | y1 = ys[-1] 150 | 151 | if i >= skip_start and i <= skip_end: 152 | #x,y = baseball.predict (x2, y2, x1, y1, 1/30, 1/30* (i-skip_start+1)) 153 | x,y = baseball.predict (xs[-2], ys[-2], xs[-1], ys[-1], 1/30, 1/30) 154 | 155 | m[0] = x 156 | m[2] = y 157 | 158 | f1.update (m) 159 | 160 | 161 | ''' 162 | if i >= skip_start and i <= skip_end: 163 | #f1.x[2] = -0.1 164 | #f1.x[5] = -17. 165 | pass 166 | else: 167 | f1.update (m) 168 | 169 | ''' 170 | 171 | xs.append (f1.x[0,0]) 172 | ys.append (f1.x[2,0]) 173 | pxs.append (z[0]) 174 | pys.append(z[1]) 175 | 176 | if i > 0 and z[1] < 0: 177 | break; 178 | 179 | p1, = plt.plot (xs, ys, 'r--') 180 | p2, = plt.plot (pxs, pys) 181 | plt.axis('equal') 182 | plt.legend([p1,p2], ['filter', 'measurement'], 2) 183 | plt.xlim([0,xs[-1]]) 184 | plt.show() 185 | 186 | 187 | start_skip = 20 188 | end_skip = 60 189 | 190 | def run_6(): 191 | dt = 1/30 192 | noise = 0.0 193 | f1 = ball_filter6(dt, R=.16, Q=0.1) 194 | plt.cla() 195 | 196 | x,y = baseball.compute_trajectory(v_0_mph = 100., theta=50., dt=dt) 197 | 198 | 199 | znoise = [(i+randn()*noise,j+randn()*noise) for (i,j) in zip(x,y)] 200 | 201 | plot_ball_filter6 (f1, znoise, start_skip, end_skip) 202 | 203 | 204 | def run_4(): 205 | dt = 1/30 206 | noise = 0.0 207 | f1 = ball_filter4(dt, R=.16, Q=0.1) 208 | plt.cla() 209 | 210 | x,y = baseball.compute_trajectory(v_0_mph = 100., theta=50., dt=dt) 211 | 212 | 213 | znoise = [(i+randn()*noise,j+randn()*noise) for (i,j) in zip(x,y)] 214 | 215 | plot_ball_filter4 (f1, znoise, start_skip, end_skip) 216 | 217 | 218 | if __name__ == "__main__": 219 | run_4() -------------------------------------------------------------------------------- /experiments/bicycle.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Apr 28 08:19:21 2015 4 | 5 | @author: Roger 6 | """ 7 | 8 | 9 | from math import * 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from matplotlib.patches import Polygon 13 | 14 | wheelbase = 100 #inches 15 | 16 | vel = 20 *12 # fps to inches per sec 17 | steering_angle = radians(1) 18 | t = 1 # second 19 | orientation = 0. # radians 20 | 21 | pos = np.array([0., 0.] 22 | 23 | for i in range(100): 24 | #if abs(steering_angle) > 1.e-8: 25 | turn_radius = tan(steering_angle) 26 | radius = wheelbase / tan(steering_angle) 27 | 28 | dist = vel*t 29 | arc_len = dist / (2*pi*radius) 30 | 31 | turn_angle = 2*pi * arc_len 32 | 33 | 34 | cx = pos[0] - radius * sin(orientation) 35 | cy = pos[1] + radius * cos(orientation) 36 | 37 | orientation = (orientation + turn_angle) % (2.0 * pi) 38 | pos[0] = cx + (sin(orientation) * radius) 39 | pos[1] = cy - (cos(orientation) * radius) 40 | 41 | plt.scatter(pos[0], pos[1]) 42 | 43 | plt.axis('equal') 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /experiments/compute_q.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "metadata": { 3 | "name": "", 4 | "signature": "sha256:72ca7769e63a40b2f7f89ae8f816dcd0df3658bb19519d8c95b2f6a5e99413ec" 5 | }, 6 | "nbformat": 3, 7 | "nbformat_minor": 0, 8 | "worksheets": [ 9 | { 10 | "cells": [ 11 | { 12 | "cell_type": "code", 13 | "collapsed": false, 14 | "input": [ 15 | "from sympy import Symbol, Matrix\n", 16 | "from sympy.interactive import printing\n", 17 | "printing.init_printing()\n", 18 | "\n", 19 | "dt = Symbol('\\Delta t')\n", 20 | "\n", 21 | "Q = Matrix([[.5*dt**2], [.5*dt**2], [dt], [dt]])\n", 22 | "Q*Q.T\n", 23 | "\n", 24 | "\n", 25 | "Q = Matrix([[.5*dt**2], [dt]])\n", 26 | "Q*Q.T" 27 | ], 28 | "language": "python", 29 | "metadata": {}, 30 | "outputs": [ 31 | { 32 | "latex": [ 33 | "$$\\left[\\begin{matrix}0.25 \\Delta t^{4} & 0.5 \\Delta t^{3}\\\\0.5 \\Delta t^{3} & \\Delta t^{2}\\end{matrix}\\right]$$" 34 | ], 35 | "metadata": {}, 36 | "output_type": "pyout", 37 | "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", 38 | "prompt_number": 8, 39 | "text": [ 40 | "\u23a1 4 3\u23a4\n", 41 | "\u23a20.25\u22c5\\Delta t 0.5\u22c5\\Delta t \u23a5\n", 42 | "\u23a2 \u23a5\n", 43 | "\u23a2 3 2 \u23a5\n", 44 | "\u23a30.5\u22c5\\Delta t \\Delta t \u23a6" 45 | ] 46 | } 47 | ], 48 | "prompt_number": 8 49 | } 50 | ], 51 | "metadata": {} 52 | } 53 | ] 54 | } -------------------------------------------------------------------------------- /experiments/distributions.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Jan 17 20:17:14 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from scipy.stats import t, norm 9 | import matplotlib.pyplot as plt 10 | import numpy as np 11 | import math 12 | import random 13 | 14 | 15 | df =4 16 | mu = 10 17 | std = 2 18 | 19 | x = np.linspace(-5, 20, 100) 20 | 21 | plt.plot(x, t.pdf(x, df=df, loc=mu, scale=std), 'r-', lw=5, label='t pdf') 22 | 23 | 24 | x2 = np.linspace(mu-10, mu+10, 100) 25 | plt.plot(x, norm.pdf(x, mu, std), 'b-', lw=5, label='gaussian pdf') 26 | plt.legend() 27 | plt.figure() 28 | 29 | def student_t(df, mu, std): # nu equals number of degrees of freedom 30 | x = random.gauss(0, std) 31 | y = 2.0*random.gammavariate(0.5*df, 2.0) 32 | return x / (math.sqrt(y/df)) + mu 33 | 34 | 35 | N = 100000 36 | ys = [student_t(2.7, 100, 2) for i in range(N)] 37 | plt.hist(ys, 10000, histtype='step') 38 | 39 | ys = [random.gauss(100,2) for i in range(N)] 40 | plt.hist(ys, 10000, histtype='step',color='r') 41 | 42 | plt.show() -------------------------------------------------------------------------------- /experiments/dme.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Spyder Editor 4 | 5 | This is a temporary script file. 6 | """ 7 | 8 | from KalmanFilter import * 9 | from math import cos, sin, sqrt, atan2 10 | 11 | 12 | def H_of (pos, pos_A, pos_B): 13 | """ Given the position of our object at 'pos' in 2D, and two transmitters 14 | A and B at positions 'pos_A' and 'pos_B', return the partial derivative 15 | of H 16 | """ 17 | 18 | theta_a = atan2(pos_a[1]-pos[1], pos_a[0] - pos[0]) 19 | theta_b = atan2(pos_b[1]-pos[1], pos_b[0] - pos[0]) 20 | 21 | if False: 22 | return np.mat([[0, -cos(theta_a), 0, -sin(theta_a)], 23 | [0, -cos(theta_b), 0, -sin(theta_b)]]) 24 | else: 25 | return np.mat([[-cos(theta_a), 0, -sin(theta_a), 0], 26 | [-cos(theta_b), 0, -sin(theta_b), 0]]) 27 | 28 | class DMESensor(object): 29 | def __init__(self, pos_a, pos_b, noise_factor=1.0): 30 | self.A = pos_a 31 | self.B = pos_b 32 | self.noise_factor = noise_factor 33 | 34 | def range_of (self, pos): 35 | """ returns tuple containing noisy range data to A and B 36 | given a position 'pos' 37 | """ 38 | 39 | ra = sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2) 40 | rb = sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2) 41 | 42 | return (ra + random.randn()*self.noise_factor, 43 | rb + random.randn()*self.noise_factor) 44 | 45 | 46 | pos_a = (100,-20) 47 | pos_b = (-100, -20) 48 | 49 | f1 = KalmanFilter(dim_x=4, dim_z=2) 50 | 51 | f1.F = np.mat ([[0, 1, 0, 0], 52 | [0, 0, 0, 0], 53 | [0, 0, 0, 1], 54 | [0, 0, 0, 0]]) 55 | 56 | f1.B = 0. 57 | 58 | f1.R *= 1. 59 | f1.Q *= .1 60 | 61 | f1.x = np.mat([1,0,1,0]).T 62 | f1.P = np.eye(4) * 5. 63 | 64 | # initialize storage and other variables for the run 65 | count = 30 66 | xs, ys = [],[] 67 | pxs, pys = [],[] 68 | 69 | # create the simulated sensor 70 | d = DMESensor (pos_a, pos_b, noise_factor=1.) 71 | 72 | # pos will contain our nominal position since the filter does not 73 | # maintain position. 74 | pos = [0,0] 75 | 76 | for i in range(count): 77 | # move (1,1) each step, so just use i 78 | pos = [i,i] 79 | 80 | # compute the difference in range between the nominal track and measured 81 | # ranges 82 | ra,rb = d.range_of(pos) 83 | rx,ry = d.range_of((i+f1.x[0,0], i+f1.x[2,0])) 84 | 85 | print ra, rb 86 | print rx,ry 87 | z = np.mat([[ra-rx],[rb-ry]]) 88 | print z.T 89 | 90 | # compute linearized H for this time step 91 | f1.H = H_of (pos, pos_a, pos_b) 92 | 93 | # store stuff so we can plot it later 94 | xs.append (f1.x[0,0]+i) 95 | ys.append (f1.x[2,0]+i) 96 | pxs.append (pos[0]) 97 | pys.append(pos[1]) 98 | 99 | # perform the Kalman filter steps 100 | f1.predict () 101 | f1.update(z) 102 | 103 | 104 | p1, = plt.plot (xs, ys, 'r--') 105 | p2, = plt.plot (pxs, pys) 106 | plt.legend([p1,p2], ['filter', 'ideal'], 2) 107 | plt.show() 108 | 109 | -------------------------------------------------------------------------------- /experiments/dog_track_1d.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Wed Apr 30 10:35:19 2014 4 | 5 | @author: rlabbe 6 | """ 7 | import numpy.random as random 8 | import math 9 | 10 | class dog_sensor(object): 11 | def __init__(self, x0 = 0, motion=1, noise=0.0): 12 | self.x = x0 13 | self.motion = motion 14 | self.noise = math.sqrt(noise) 15 | 16 | def sense(self): 17 | self.x = self.x + self.motion 18 | self.x += random.randn() * self.noise 19 | return self.x 20 | 21 | 22 | def measure_dog (): 23 | if not hasattr(measure_dog, "x"): 24 | measure_dog.x = 0 25 | measure_dog.motion = 1 26 | 27 | 28 | if __name__ == '__main__': 29 | 30 | dog = dog_sensor(noise = 1) 31 | for i in range(10): 32 | print (dog.sense()) 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /experiments/doppler.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Jan 17 15:04:08 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | 9 | 10 | from numpy.random import randn 11 | import numpy as np 12 | from numpy import array 13 | import matplotlib.pyplot as plt 14 | from filterpy.common import Q_discrete_white_noise 15 | from filterpy.kalman import KalmanFilter 16 | from math import sqrt 17 | import math 18 | import random 19 | 20 | pos_var = 1. 21 | dop_var = 2. 22 | dt = 1/20 23 | 24 | 25 | def rand_student_t(df, mu=0, std=1): 26 | """return random number distributed by student's t distribution with 27 | `df` degrees of freedom with the specified mean and standard deviation. 28 | """ 29 | x = random.gauss(0, std) 30 | y = 2.0*random.gammavariate(0.5*df, 2.0) 31 | return x / (math.sqrt(y/df)) + mu 32 | 33 | 34 | 35 | np.random.seed(124) 36 | class ConstantVelocityObject(object): 37 | def __init__(self, x0, vel, noise_scale): 38 | self.x = np.array([x0, vel]) 39 | self.noise_scale = noise_scale 40 | self.vel = vel 41 | 42 | 43 | def update(self, dt): 44 | pnoise = abs(randn()*self.noise_scale) 45 | if self.x[1] > self.vel: 46 | pnoise = -pnoise 47 | 48 | self.x[1] += pnoise 49 | self.x[0] += self.x[1]*dt 50 | 51 | return self.x.copy() 52 | 53 | 54 | def sense_pos(self): 55 | return self.x[0] + [randn()*self.noise_scale[0]] 56 | 57 | 58 | def vel(self): 59 | return self.x[1] + [randn()*self.noise_scale[1]] 60 | 61 | 62 | def sense(x, noise_scale=(1., 0.01)): 63 | return x + [randn()*noise_scale[0], randn()*noise_scale[1]] 64 | 65 | def sense_t(x, noise_scale=(1., 0.01)): 66 | return x + [rand_student_t(2)*noise_scale[0], 67 | rand_student_t(2)*noise_scale[1]] 68 | 69 | 70 | 71 | 72 | 73 | R2 = np.zeros((2, 2)) 74 | R1 = np.zeros((1, 1)) 75 | 76 | R2[0, 0] = pos_var 77 | R2[1, 1] = dop_var 78 | 79 | R1[0,0] = dop_var 80 | 81 | H2 = np.array([[1., 0.], [0., 1.]]) 82 | H1 = np.array([[0., 1.]]) 83 | 84 | 85 | 86 | kf = KalmanFilter(2, 1) 87 | kf.F = array([[1, dt], 88 | [0, 1]]) 89 | kf.H = array([[1, 0], 90 | [0, 1]]) 91 | kf.Q = Q_discrete_white_noise(2, dt, .02) 92 | kf.x = array([0., 0.]) 93 | kf.R = R1 94 | kf.H = H1 95 | kf.P *= 10 96 | 97 | 98 | random.seed(1234) 99 | track = [] 100 | zs = [] 101 | ests = [] 102 | sensor_noise = (sqrt(pos_var), sqrt(dop_var)) 103 | obj = ConstantVelocityObject(0., 1., noise_scale=.18) 104 | 105 | for i in range(30000): 106 | x = obj.update(dt/10) 107 | z = sense_t(x, sensor_noise) 108 | if i % 10 != 0: 109 | continue 110 | 111 | if i % 60 == 87687658760: 112 | kf.predict() 113 | kf.update(z, H=H2, R=R2) 114 | else: 115 | kf.predict() 116 | kf.update(z[1], H=H1, R=R1) 117 | 118 | ests.append(kf.x) 119 | 120 | track.append(x) 121 | zs.append(z) 122 | 123 | 124 | track = np.asarray(track) 125 | zs = np.asarray(zs) 126 | ests = np.asarray(ests) 127 | 128 | plt.figure() 129 | plt.subplot(211) 130 | plt.plot(track[:,0], label='track') 131 | plt.plot(zs[:,0], label='measurements') 132 | plt.plot(ests[:,0], label='filter') 133 | plt.legend(loc='best') 134 | 135 | plt.subplot(212) 136 | plt.plot(track[:,1], label='track') 137 | plt.plot(zs[:,1], label='measurements') 138 | plt.plot(ests[:,1], label='filter') 139 | plt.show() 140 | 141 | -------------------------------------------------------------------------------- /experiments/ekf4.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat May 30 13:24:01 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | # -*- coding: utf-8 -*- 9 | """ 10 | Created on Thu May 28 20:23:57 2015 11 | 12 | @author: rlabbe 13 | """ 14 | 15 | 16 | from math import cos, sin, sqrt, atan2, tan 17 | import matplotlib.pyplot as plt 18 | import numpy as np 19 | from numpy import array, dot 20 | from numpy.random import randn 21 | from filterpy.common import plot_covariance_ellipse 22 | from filterpy.kalman import ExtendedKalmanFilter as EKF 23 | from sympy import Matrix, symbols 24 | import sympy 25 | 26 | def print_x(x): 27 | print(x[0, 0], x[1, 0], np.degrees(x[2, 0])) 28 | 29 | 30 | def normalize_angle(x, index): 31 | if x[index] > np.pi: 32 | x[index] -= 2*np.pi 33 | if x[index] < -np.pi: 34 | x[index] = 2*np.pi 35 | 36 | def residual(a,b): 37 | y = a - b 38 | normalize_angle(y, 1) 39 | return y 40 | 41 | 42 | sigma_r = 1 43 | sigma_h = .1#np.radians(1) 44 | sigma_steer = np.radians(1) 45 | 46 | 47 | class RobotEKF(EKF): 48 | def __init__(self, dt, wheelbase): 49 | EKF.__init__(self, 3, 2, 2) 50 | self.dt = dt 51 | self.wheelbase = wheelbase 52 | 53 | a, x, y, v, w, theta, time = symbols( 54 | 'a, x, y, v, w, theta, t') 55 | 56 | d = v*time 57 | beta = (d/w)*sympy.tan(a) 58 | r = w/sympy.tan(a) 59 | 60 | 61 | self.fxu = Matrix([[x-r*sympy.sin(theta)+r*sympy.sin(theta+beta)], 62 | [y+r*sympy.cos(theta)-r*sympy.cos(theta+beta)], 63 | [theta+beta]]) 64 | 65 | self.F_j = self.fxu.jacobian(Matrix([x, y, theta])) 66 | self.V_j = self.fxu.jacobian(Matrix([v, a])) 67 | 68 | self.subs = {x: 0, y: 0, v:0, a:0, time:dt, w:wheelbase, theta:0} 69 | self.x_x = x 70 | self.x_y = y 71 | self.v = v 72 | self.a = a 73 | self.theta = theta 74 | 75 | 76 | def predict(self, u=0): 77 | 78 | 79 | self.x = self.move(self.x, u, self.dt) 80 | 81 | self.subs[self.theta] = self.x[2, 0] 82 | self.subs[self.v] = u[0] 83 | self.subs[self.a] = u[1] 84 | 85 | 86 | F = array(self.F_j.evalf(subs=self.subs)).astype(float) 87 | V = array(self.V_j.evalf(subs=self.subs)).astype(float) 88 | 89 | # covariance of motion noise in control space 90 | M = array([[0.1*u[0]**2, 0], 91 | [0, sigma_steer**2]]) 92 | 93 | self.P = dot(F, self.P).dot(F.T) + dot(V, M).dot(V.T) 94 | 95 | 96 | def move(self, x, u, dt): 97 | h = x[2, 0] 98 | v = u[0] 99 | steering_angle = u[1] 100 | 101 | dist = v*dt 102 | 103 | if abs(steering_angle) < 0.0001: 104 | # approximate straight line with huge radius 105 | r = 1.e-30 106 | b = dist / self.wheelbase * tan(steering_angle) 107 | r = self.wheelbase / tan(steering_angle) # radius 108 | 109 | 110 | sinh = sin(h) 111 | sinhb = sin(h + b) 112 | cosh = cos(h) 113 | coshb = cos(h + b) 114 | 115 | return x + array([[-r*sinh + r*sinhb], 116 | [r*cosh - r*coshb], 117 | [b]]) 118 | 119 | 120 | def H_of(x, p): 121 | """ compute Jacobian of H matrix where h(x) computes the range and 122 | bearing to a landmark 'p' for state x """ 123 | 124 | px = p[0] 125 | py = p[1] 126 | hyp = (px - x[0, 0])**2 + (py - x[1, 0])**2 127 | dist = np.sqrt(hyp) 128 | 129 | H = array( 130 | [[(-px + x[0, 0]) / dist, (-py + x[1, 0]) / dist, 0.], 131 | [ -(-py + x[1, 0]) / hyp, -(px - x[0, 0]) / hyp, -1.]]) 132 | return H 133 | 134 | 135 | def Hx(x, p): 136 | """ takes a state variable and returns the measurement that would 137 | correspond to that state. 138 | """ 139 | px = p[0] 140 | py = p[1] 141 | dist = np.sqrt((px - x[0, 0])**2 + (py - x[1, 0])**2) 142 | 143 | Hx = array([[dist], 144 | [atan2(py - x[1, 0], px - x[0, 0]) - x[2, 0]]]) 145 | return Hx 146 | 147 | dt = 1.0 148 | ekf = RobotEKF(dt, wheelbase=0.5) 149 | 150 | #np.random.seed(1234) 151 | 152 | m = array([[5, 10], 153 | [10, 5], 154 | [15, 15]]) 155 | 156 | ekf.x = array([[2, 6, .3]]).T 157 | ekf.P = np.diag([.1, .1, .1]) 158 | ekf.R = np.diag([sigma_r**2, sigma_h**2]) 159 | 160 | u = array([1.1, .01]) 161 | 162 | xp = ekf.x.copy() 163 | 164 | plt.figure() 165 | plt.scatter(m[:, 0], m[:, 1]) 166 | for i in range(250): 167 | xp = ekf.move(xp, u, dt/10.) # simulate robot 168 | plt.plot(xp[0], xp[1], ',', color='g') 169 | 170 | if i % 10 == 0: 171 | 172 | ekf.predict(u=u) 173 | 174 | plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=3, 175 | facecolor='b', alpha=0.08) 176 | 177 | for lmark in m: 178 | d = sqrt((lmark[0] - xp[0, 0])**2 + (lmark[1] - xp[1, 0])**2) + randn()*sigma_r 179 | a = atan2(lmark[1] - xp[1, 0], lmark[0] - xp[0, 0]) - xp[2, 0] + randn()*sigma_h 180 | z = np.array([[d], [a]]) 181 | 182 | ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual, 183 | args=(lmark), hx_args=(lmark)) 184 | 185 | plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=3, 186 | facecolor='g', alpha=0.4) 187 | 188 | #plt.plot(ekf.x[0], ekf.x[1], 'x', color='r') 189 | 190 | plt.axis('equal') 191 | plt.title("EKF Robot localization") 192 | plt.show() 193 | 194 | -------------------------------------------------------------------------------- /experiments/ekfloc2.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon May 25 18:18:54 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | 9 | from math import cos, sin, sqrt, atan2 10 | import matplotlib.pyplot as plt 11 | import numpy as np 12 | from numpy import array, dot 13 | from numpy.linalg import pinv 14 | from numpy.random import randn 15 | from filterpy.common import plot_covariance_ellipse 16 | from filterpy.kalman import ExtendedKalmanFilter as EKF 17 | 18 | 19 | def print_x(x): 20 | print(x[0, 0], x[1, 0], np.degrees(x[2, 0])) 21 | 22 | 23 | def normalize_angle(x, index): 24 | if x[index] > np.pi: 25 | x[index] -= 2*np.pi 26 | if x[index] < -np.pi: 27 | x[index] = 2*np.pi 28 | 29 | def residual(a,b): 30 | y = a - b 31 | normalize_angle(y, 1) 32 | return y 33 | 34 | 35 | def control_update(x, u, dt): 36 | """ x is [x, y, hdg], u is [vel, omega] """ 37 | 38 | v = u[0] 39 | w = u[1] 40 | if w == 0: 41 | # approximate straight line with huge radius 42 | w = 1.e-30 43 | r = v/w # radius 44 | 45 | 46 | return x + np.array([[-r*sin(x[2]) + r*sin(x[2] + w*dt)], 47 | [ r*cos(x[2]) - r*cos(x[2] + w*dt)], 48 | [w*dt]]) 49 | 50 | a1 = 0.001 51 | a2 = 0.001 52 | a3 = 0.001 53 | a4 = 0.001 54 | 55 | sigma_r = 0.1 56 | sigma_h = a_error = np.radians(1) 57 | sigma_s = 0.00001 58 | 59 | def normalize_angle(x, index): 60 | if x[index] > np.pi: 61 | x[index] -= 2*np.pi 62 | if x[index] < -np.pi: 63 | x[index] = 2*np.pi 64 | 65 | 66 | 67 | class RobotEKF(EKF): 68 | def __init__(self, dt): 69 | EKF.__init__(self, 3, 2, 2) 70 | self.dt = dt 71 | 72 | def predict_x(self, u): 73 | self.x = self.move(self.x, u, self.dt) 74 | 75 | 76 | def move(self, x, u, dt): 77 | h = x[2, 0] 78 | v = u[0] 79 | w = u[1] 80 | 81 | if w == 0: 82 | # approximate straight line with huge radius 83 | w = 1.e-30 84 | r = v/w # radius 85 | 86 | sinh = sin(h) 87 | sinhwdt = sin(h + w*dt) 88 | cosh = cos(h) 89 | coshwdt = cos(h + w*dt) 90 | 91 | return x + array([[-r*sinh + r*sinhwdt], 92 | [r*cosh - r*coshwdt], 93 | [w*dt]]) 94 | 95 | 96 | def H_of(x, landmark_pos): 97 | """ compute Jacobian of H matrix for state x """ 98 | 99 | mx = landmark_pos[0] 100 | my = landmark_pos[1] 101 | q = (mx - x[0, 0])**2 + (my - x[1, 0])**2 102 | 103 | H = array( 104 | [[-(mx - x[0, 0]) / sqrt(q), -(my - x[1, 0]) / sqrt(q), 0], 105 | [ (my - x[1, 0]) / q, -(mx - x[0, 0]) / q, -1]]) 106 | return H 107 | 108 | 109 | def Hx(x, landmark_pos): 110 | """ takes a state variable and returns the measurement that would 111 | correspond to that state. 112 | """ 113 | mx = landmark_pos[0] 114 | my = landmark_pos[1] 115 | q = (mx - x[0, 0])**2 + (my - x[1, 0])**2 116 | 117 | Hx = array([[sqrt(q)], 118 | [atan2(my - x[1, 0], mx - x[0, 0]) - x[2, 0]]]) 119 | return Hx 120 | 121 | dt = 1.0 122 | ekf = RobotEKF(dt) 123 | 124 | #np.random.seed(1234) 125 | 126 | m = array([[5, 10], 127 | [10, 5], 128 | [15, 15]]) 129 | 130 | ekf.x = array([[2, 6, .3]]).T 131 | u = array([.5, .01]) 132 | ekf.P = np.diag([1., 1., 1.]) 133 | ekf.R = np.diag([sigma_r**2, sigma_h**2]) 134 | c = [0, 1, 2] 135 | 136 | xp = ekf.x.copy() 137 | 138 | plt.scatter(m[:, 0], m[:, 1]) 139 | for i in range(300): 140 | xp = ekf.move(xp, u, dt/10.) # simulate robot 141 | plt.plot(xp[0], xp[1], ',', color='g') 142 | 143 | if i % 10 == 0: 144 | h = ekf.x[2, 0] 145 | v = u[0] 146 | w = u[1] 147 | 148 | if w == 0: 149 | # approximate straight line with huge radius 150 | w = 1.e-30 151 | r = v/w # radius 152 | 153 | sinh = sin(h) 154 | sinhwdt = sin(h + w*dt) 155 | cosh = cos(h) 156 | coshwdt = cos(h + w*dt) 157 | 158 | ekf.F = array( 159 | [[1, 0, -r*cosh + r*coshwdt], 160 | [0, 1, -r*sinh + r*sinhwdt], 161 | [0, 0, 1]]) 162 | 163 | V = array( 164 | [[(-sinh + sinhwdt)/w, v*(sin(h)-sinhwdt)/(w**2) + v*coshwdt*dt/w], 165 | [(cosh - coshwdt)/w, -v*(cosh-coshwdt)/(w**2) + v*sinhwdt*dt/w], 166 | [0, dt]]) 167 | 168 | # covariance of motion noise in control space 169 | M = array([[a1*v**2 + a2*w**2, 0], 170 | [0, a3*v**2 + a4*w**2]]) 171 | 172 | ekf.Q = dot(V, M).dot(V.T) 173 | 174 | ekf.predict(u=u) 175 | 176 | for lmark in m: 177 | d = sqrt((lmark[0] - xp[0, 0])**2 + (lmark[1] - xp[1, 0])**2) + randn()*sigma_r 178 | a = atan2(lmark[1] - xp[1, 0], lmark[0] - xp[0, 0]) - xp[2, 0] + randn()*sigma_h 179 | z = np.array([[d], [a]]) 180 | 181 | ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual, 182 | args=(lmark), hx_args=(lmark)) 183 | 184 | plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=10, 185 | facecolor='g', alpha=0.3) 186 | 187 | #plt.plot(ekf.x[0], ekf.x[1], 'x', color='r') 188 | 189 | plt.axis('equal') 190 | plt.show() 191 | 192 | -------------------------------------------------------------------------------- /experiments/euler.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Apr 21 18:40:47 2015 4 | 5 | @author: Roger 6 | """ 7 | 8 | 9 | 10 | 11 | def dx(t, y): 12 | return y 13 | 14 | 15 | def euler(t0, tmax, y0, dx, step=1.): 16 | t = t0 17 | y = y0 18 | while t < tmax: 19 | f = dx(t,y) 20 | y = y + step*dx(t, y) 21 | t +=step 22 | 23 | return y 24 | 25 | 26 | print(euler(0, 4, 1, dx, step=0.25)) -------------------------------------------------------------------------------- /experiments/fusion.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Fri Feb 13 17:47:56 2015 4 | 5 | @author: rlabbe 6 | """ 7 | import numpy as np 8 | from filterpy.kalman import UnscentedKalmanFilter as UKF 9 | from math import atan2, radians,degrees 10 | from filterpy.common import stats 11 | import matplotlib.pyplot as plt 12 | 13 | p = (-10, -10) 14 | 15 | def hx(x): 16 | dx = x[0] - hx.p[0] 17 | dy = x[1] - hx.p[1] 18 | return np.array([atan2(dy,dx), (dx**2 + dy**2)**.5]) 19 | 20 | def fx(x,dt): 21 | return x 22 | 23 | 24 | 25 | kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, kappa=2.) 26 | 27 | kf.x = np.array([100, 100.]) 28 | kf.P *= 40 29 | hx.p = kf.x - np.array([50,50]) 30 | 31 | d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5 32 | 33 | stats.plot_covariance_ellipse( 34 | kf.x, cov=kf.P, axis_equal=True, 35 | facecolor='y', edgecolor=None, alpha=0.6) 36 | plt.scatter([100], [100], c='y', label='Initial') 37 | 38 | kf.R[0,0] = radians (1)**2 39 | kf.R[1,1] = 2.**2 40 | 41 | 42 | kf.predict() 43 | kf.update(np.array([radians(45), d])) 44 | 45 | print(kf.x) 46 | print(kf.P) 47 | 48 | stats.plot_covariance_ellipse( 49 | kf.x, cov=kf.P, axis_equal=True, 50 | facecolor='g', edgecolor=None, alpha=0.6) 51 | plt.scatter([100], [100], c='g', label='45 degrees') 52 | 53 | 54 | p = (13, -11) 55 | hx.p = kf.x - np.array([-50,50]) 56 | d = ((kf.x[0] - hx.p[0])**2 + (kf.x[1] - hx.p[1])**2)**.5 57 | 58 | kf.predict() 59 | kf.update(np.array([radians(135), d])) 60 | 61 | stats.plot_covariance_ellipse( 62 | kf.x, cov=kf.P, axis_equal=True, 63 | facecolor='b', edgecolor=None, alpha=0.6) 64 | plt.scatter([100], [100], c='b', label='135 degrees') 65 | 66 | plt.legend(scatterpoints=1, markerscale=3) -------------------------------------------------------------------------------- /experiments/gating.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "** code and text not yet correct - ignore!!! **\n", 8 | "\n", 9 | "## Erroneous Measurements\n", 10 | "\n", 11 | "If you can trust your sensor to never malfunction or provide a spurious measurement you are lucky. Skip to the next section. The reality is that sensors are imperfect. Stray voltages affect signals, birds fly in front of distance sensors. Computer vision sensors mistake shadows for pedestrians. And so on. \n", 12 | "\n", 13 | "Entire books are devoted to this topic. I've found that books which address multi-target tracking with radar to be particularly useful. I'll just address a couple of points that will greatly improve your filters with only a tiny amount of theory and code. \n", 14 | "\n", 15 | "We've discussed the *likelihood* function. Recall Bayes rule:\n", 16 | "\n", 17 | "$$\\text{posterior} = \\frac{\\text{likelihood}\\times\\text{prior}}{\\text{evidence}}$$\n", 18 | "\n", 19 | "where the likelihood is defined as\n", 20 | "\n", 21 | "$$\\mathcal L = p(\\mathbf z \\mid \\mathbf x)$$\n", 22 | "\n", 23 | "That is simply the likelihood (probability) of the measurements given the prior $\\mathbf x$. This suggests a trivial *gating* function. Here *gating* means a function that accepts or rejects the measurement based on some criteria. We assume that the measurement noise is Gaussian. Recall from the Gaussians chapter that 99.7% of all values fall within 3 standard deviations of the mean. If the measurement $\\mathbf z > 3 \\sigma$ we can discard it as being very unlikely. Easy.\n", 24 | "\n", 25 | "In practice you probably shouldn't do that. We *model* the sensors as being Gaussian, but in practice they probably aren't. The tails might be thick, it might be a slightly different distribution such as Students t-distribution, and so on. For example, NASA uses between $5\\sigma$ and $6\\sigma$ to accommodate the true behavior of the sensors on the Orion mission. Chances are your budget is less than NASA's, but I suspect the stakes are commensurately less. There is no way to analytically determine the correct value. You will need to run experiments with your data to see what value is reasonable for your application." 26 | ] 27 | }, 28 | { 29 | "cell_type": "code", 30 | "execution_count": null, 31 | "metadata": { 32 | "collapsed": true 33 | }, 34 | "outputs": [], 35 | "source": [ 36 | "from math import sqrt\n", 37 | "\n", 38 | "def gated_fusion(pos_data, vel_data, wheel_std, ps_std, gate=3.):\n", 39 | " kf = KalmanFilter(dim_x=2, dim_z=1)\n", 40 | " kf.F = array([[1., 1.], [0., 1.]])\n", 41 | " kf.H = array([[1., 0.], [1., 0.]])\n", 42 | " kf.x = array([[0.], [1.]])\n", 43 | " kf.P *= 100\n", 44 | "\n", 45 | " xs, ts = [], []\n", 46 | " \n", 47 | " # copy data for plotting\n", 48 | " zs_wheel = np.array(vel_data)\n", 49 | " zs_ps = np.array(pos_data)\n", 50 | " \n", 51 | " last_t = 0\n", 52 | " while len(pos_data) > 0 and len(vel_data) > 0:\n", 53 | " if pos_data[0][0] < vel_data[0][0]:\n", 54 | " t, z = pos_data.pop(0)\n", 55 | " dt = t - last_t\n", 56 | " last_t = t\n", 57 | " p_index = 0\n", 58 | " \n", 59 | " kf.H = np.array([[1., 0.]])\n", 60 | " kf.R[0,0] = ps_std**2\n", 61 | " si\n", 62 | " else:\n", 63 | " t, z = vel_data.pop(0)\n", 64 | " dt = t - last_t\n", 65 | " last_t = t\n", 66 | " p_index = 1\n", 67 | " \n", 68 | " kf.H = np.array([[0., 1.]])\n", 69 | " kf.R[0,0] = wheel_std**2\n", 70 | "\n", 71 | " kf.F[0,1] = dt\n", 72 | " kf.Q = Q_discrete_white_noise(2, dt=dt, var=.02)\n", 73 | " kf.predict()\n", 74 | " std = sqrt(kf.P[p_index, p_index])\n", 75 | " y = abs(kf.residual_of(z)) \n", 76 | " if y <= std * gate:\n", 77 | " kf.update(np.array([z]))\n", 78 | " else:\n", 79 | " print('skip', p_index, kf.x.T, kf.P.diagonal(), \"%.3f\" % z, y)\n", 80 | "\n", 81 | "\n", 82 | " xs.append(kf.x.T[0])\n", 83 | " ts.append(t)\n", 84 | "\n", 85 | " plot_fusion(xs, ts, zs_ps, zs_wheel)\n", 86 | " \n", 87 | "random.seed(1123)\n", 88 | "pos_data, vel_data = gen_sensor_data(25, 1.5, .3)\n", 89 | "gated_fusion(pos_data, vel_data, 1.5, 3.0, gate=4.);\n" 90 | ] 91 | } 92 | ], 93 | "metadata": { 94 | "kernelspec": { 95 | "display_name": "Python 3", 96 | "language": "python", 97 | "name": "python3" 98 | }, 99 | "language_info": { 100 | "codemirror_mode": { 101 | "name": "ipython", 102 | "version": 3 103 | }, 104 | "file_extension": ".py", 105 | "mimetype": "text/x-python", 106 | "name": "python", 107 | "nbconvert_exporter": "python", 108 | "pygments_lexer": "ipython3", 109 | "version": "3.5.2" 110 | } 111 | }, 112 | "nbformat": 4, 113 | "nbformat_minor": 0 114 | } 115 | -------------------------------------------------------------------------------- /experiments/gauss.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Apr 22 11:38:49 2014 4 | 5 | @author: rlabbe 6 | """ 7 | from __future__ import division, print_function 8 | import math 9 | import matplotlib.pyplot as plt 10 | import numpy.random as random 11 | 12 | 13 | class gaussian(object): 14 | def __init__(self, mean, variance): 15 | try: 16 | self.mean = float(mean) 17 | self.variance = float(variance) 18 | 19 | except: 20 | self.mean = mean 21 | self.variance = variance 22 | 23 | def __add__ (a, b): 24 | return gaussian (a.mean + b.mean, a.variance + b.variance) 25 | 26 | def __mul__ (a, b): 27 | m = (a.variance*b.mean + b.variance*a.mean) / (a.variance + b.variance) 28 | v = 1. / (1./a.variance + 1./b.variance) 29 | return gaussian (m, v) 30 | 31 | def __call__(self, x): 32 | """ Impl 33 | """ 34 | return math.exp (-0.5 * (x-self.mean)**2 / self.variance) / \ 35 | math.sqrt(2.*math.pi*self.variance) 36 | 37 | 38 | def __str__(self): 39 | return "(%f, %f)" %(self.mean, self.sigma) 40 | 41 | def stddev(self): 42 | return math.sqrt (self.variance) 43 | 44 | def as_tuple(self): 45 | return (self.mean, self.variance) 46 | 47 | def __tuple__(self): 48 | return (self.mean, self.variance) 49 | 50 | def __getitem__ (self,index): 51 | """ maybe silly, allows you to access obect as a tuple: 52 | a = gaussian(3,4) 53 | print (tuple(a)) 54 | """ 55 | if index == 0: return self.mean 56 | elif index == 1: return self.variance 57 | else: raise StopIteration 58 | 59 | class KF1D(object): 60 | def __init__ (self, pos, sigma): 61 | self.estimate = gaussian(pos,sigma) 62 | 63 | def update(self, Z,var): 64 | self.estimate = self.estimate * gaussian (Z,var) 65 | 66 | def predict(self, U, var): 67 | self.estimate = self.estimate + gaussian (U,var) 68 | 69 | 70 | 71 | 72 | def mul2 (a, b): 73 | m = (a['variance']*b['mean'] + b['variance']*a['mean']) / (a['variance'] + b['variance']) 74 | v = 1. / (1./a['variance'] + 1./b['variance']) 75 | return gaussian (m, v) 76 | 77 | 78 | #varying_error_kf( noise_factor=100) 79 | -------------------------------------------------------------------------------- /experiments/gh.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu May 15 16:07:26 2014 4 | 5 | @author: RL 6 | """ 7 | from __future__ import division 8 | import matplotlib.pyplot as plt 9 | import numpy.random as random 10 | 11 | 12 | 13 | 14 | 15 | 16 | def g_h_filter (data, x, dx, g, h, dt=1.): 17 | results = [] 18 | for z in data: 19 | x_est = x + (dx*dt) 20 | residual = z - x_est 21 | 22 | dx = dx + h * (residual / float(dt)) 23 | x = x_est + g * residual 24 | print('gx',dx,) 25 | 26 | results.append(x) 27 | 28 | 29 | return results 30 | 31 | 32 | ''' 33 | computation of x 34 | x_est = weight + gain 35 | residual = z - weight - gain 36 | x = weight + gain + g (z - weight - gain) 37 | 38 | w + gain + gz -wg -ggain 39 | w -wg + gain - ggain + gz 40 | 41 | w(1-g) + gain(1-g) +gz 42 | 43 | (w+g)(1-g) +gz 44 | 45 | ''' 46 | ''' 47 | gain computation 48 | 49 | gain = gain + h/t* (z - weight - gain) 50 | = gain + hz/t -hweight/t - hgain/t 51 | 52 | = gain(1-h/t) + h/t(z-weight) 53 | 54 | ''' 55 | ''' 56 | gain+ h*(z-w -gain*t)/t 57 | 58 | gain + hz/t -hw/t -hgain 59 | 60 | gain*(1-h) + h/t(z-w) 61 | 62 | 63 | ''' 64 | def weight2(): 65 | w = 0 66 | gain = 200 67 | t = 10. 68 | weight_scale = 1./6 69 | gain_scale = 1./10 70 | 71 | weights=[2060] 72 | for i in range (len(weights)): 73 | z = weights[i] 74 | w_pre = w + gain*t 75 | 76 | new_w = w_pre * (1-weight_scale) + z * (weight_scale) 77 | 78 | print('new_w',new_w) 79 | 80 | gain = gain *(1-gain_scale) + (z - w) * gain_scale/t 81 | 82 | print (z) 83 | print(w) 84 | 85 | #gain = new_gain * (gain_scale) + gain * (1-gain_scale) 86 | w = new_w 87 | print ('w',w,) 88 | print ('gain=',gain) 89 | 90 | 91 | def weight3(): 92 | w = 160. 93 | gain = 1. 94 | t = 1. 95 | weight_scale = 6/10. 96 | gain_scale = 2./3 97 | 98 | weights=[158] 99 | for i in range (len(weights)): 100 | z = weights[i] 101 | w_pre = w + gain*t 102 | 103 | new_w = w_pre * (1-weight_scale) + z * (weight_scale) 104 | 105 | print('new_w',new_w) 106 | 107 | gain = gain *(1-gain_scale) + (z - w) * gain_scale/t 108 | 109 | print (z) 110 | print(w) 111 | 112 | #gain = new_gain * (gain_scale) + gain * (1-gain_scale) 113 | w = new_w 114 | print ('w',w,) 115 | print ('gain=',gain) 116 | weight3() 117 | ''' 118 | #zs = [i + random.randn()*50 for i in range(200)] 119 | zs = [158.0, 164.2, 160.3, 159.9, 162.1, 164.6, 169.6, 167.4, 166.4, 171.0] 120 | 121 | #zs = [2060] 122 | data= g_h_filter(zs, 160, 1, .6, 0, 1.) 123 | 124 | ''' 125 | 126 | data = g_h_filter([2060], x=0, dx=200, g=1./6, h = 1./10, dt=10) 127 | print data 128 | 129 | 130 | ''' 131 | print data 132 | print data2 133 | plt.plot(data) 134 | plt.plot(zs, 'g') 135 | plt.show() 136 | ''' 137 | -------------------------------------------------------------------------------- /experiments/histogram.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Apr 22 10:43:38 2014 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | p = [.2, .2, .2, .2, .2] 9 | world = ['green', 'red', 'red', 'green', 'green'] 10 | measurements = ['red', 'green'] 11 | 12 | pHit = 0.6 13 | pMiss = 0.2 14 | 15 | pOvershoot = 0.1 16 | pUndershoot = 0.1 17 | pExact = 0.8 18 | 19 | def normalize (p): 20 | s = sum(p) 21 | for i in range (len(p)): 22 | p[i] = p[i] / s 23 | 24 | def sense(p, Z): 25 | q= [] 26 | for i in range (len(p)): 27 | hit = (world[i] ==Z) 28 | q.append(p[i] * (pHit*hit + pMiss*(1-hit))) 29 | normalize(q) 30 | return q 31 | 32 | 33 | def move(p, U): 34 | q = [] 35 | for i in range(len(p)): 36 | pexact = p[(i-U) % len(p)] * pExact 37 | pover = p[(i-U-1) % len(p)] * pOvershoot 38 | punder = p[(i-U+1) % len(p)] * pUndershoot 39 | q.append (pexact + pover + punder) 40 | 41 | normalize(q) 42 | return q 43 | 44 | if __name__ == "__main__": 45 | 46 | p = sense(p, 'red') 47 | print p 48 | pause() 49 | for z in measurements: 50 | p = sense (p, z) 51 | p = move (p, 1) 52 | print p 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /experiments/image_tracker.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat May 24 14:42:55 2014 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from KalmanFilter import KalmanFilter 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | import numpy.random as random 12 | 13 | f = KalmanFilter (dim=4) 14 | 15 | dt = 1 16 | f.F = np.mat ([[1, dt, 0, 0], 17 | [0, 1, 0, 0], 18 | [0, 0, 1, dt], 19 | [0, 0, 0, 1]]) 20 | 21 | f.H = np.mat ([[1, 0, 0, 0], 22 | [0, 0, 1, 0]]) 23 | 24 | 25 | 26 | f.Q *= 4. 27 | f.R = np.mat([[50,0], 28 | [0, 50]]) 29 | 30 | f.x = np.mat([0,0,0,0]).T 31 | f.P *= 100. 32 | 33 | 34 | xs = [] 35 | ys = [] 36 | count = 200 37 | for i in range(count): 38 | z = np.mat([[i+random.randn()*1],[i+random.randn()*1]]) 39 | f.predict () 40 | f.update (z) 41 | xs.append (f.x[0,0]) 42 | ys.append (f.x[2,0]) 43 | 44 | 45 | plt.plot (xs, ys) 46 | plt.show() 47 | 48 | -------------------------------------------------------------------------------- /experiments/mkf_ellipse_test.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sun May 11 20:47:52 2014 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from DogSensor import DogSensor 9 | from filterpy.kalman import KalmanFilter 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import stats 13 | 14 | def dog_tracking_filter(R,Q=0,cov=1.): 15 | f = KalmanFilter (dim_x=2, dim_z=1) 16 | f.x = np.matrix([[0], [0]]) # initial state (location and velocity) 17 | f.F = np.matrix([[1,1],[0,1]]) # state transition matrix 18 | f.H = np.matrix([[1,0]]) # Measurement function 19 | f.R = R # measurement uncertainty 20 | f.P *= cov # covariance matrix 21 | f.Q = Q 22 | return f 23 | 24 | 25 | def plot_track(noise, count, R, Q=0, plot_P=True, title='Kalman Filter'): 26 | dog = DogSensor(velocity=1, noise=noise) 27 | f = dog_tracking_filter(R=R, Q=Q, cov=10.) 28 | 29 | ps = [] 30 | zs = [] 31 | cov = [] 32 | for t in range (count): 33 | z = dog.sense() 34 | f.update (z) 35 | #print (t,z) 36 | ps.append (f.x[0,0]) 37 | cov.append(f.P) 38 | zs.append(z) 39 | f.predict() 40 | 41 | p0, = plt.plot([0,count],[0,count],'g') 42 | p1, = plt.plot(range(1,count+1),zs,c='r', linestyle='dashed') 43 | p2, = plt.plot(range(1,count+1),ps, c='b') 44 | plt.axis('equal') 45 | plt.legend([p0,p1,p2], ['actual','measurement', 'filter'], 2) 46 | plt.title(title) 47 | 48 | for i,p in enumerate(cov): 49 | print(i,p) 50 | e = stats.sigma_ellipse (p, i+1, ps[i]) 51 | stats.plot_sigma_ellipse(e, axis_equal=False) 52 | plt.xlim((-1,count)) 53 | plt.show() 54 | 55 | 56 | if __name__ == "__main__": 57 | plot_track (noise=30, R=5, Q=2, count=20) -------------------------------------------------------------------------------- /experiments/noise.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Fri May 2 10:28:27 2014 4 | 5 | @author: rlabbe 6 | """ 7 | import numpy.random 8 | def white_noise (sigma2=1.): 9 | return sigma2 * numpy.random.randn() 10 | 11 | 12 | if __name__ == "__main__": 13 | assert white_noise(.0) == 0. -------------------------------------------------------------------------------- /experiments/nonlinear_plots.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sun May 18 11:09:23 2014 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from __future__ import division 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | from numpy.random import normal 12 | 13 | 14 | 15 | def plot_transfer_func(data, f, lims,num_bins=1000): 16 | ys = f(data) 17 | 18 | #plot output 19 | plt.subplot(2,2,1) 20 | plt.hist(ys, num_bins, orientation='horizontal',histtype='step') 21 | plt.ylim(lims) 22 | plt.gca().xaxis.set_ticklabels([]) 23 | 24 | 25 | 26 | # plot transfer function 27 | plt.subplot(2,2,2) 28 | x = np.arange(lims[0], lims[1],0.1) 29 | y = f(x) 30 | plt.plot (x,y) 31 | isct = f(0) 32 | plt.plot([0,0,lims[0]],[lims[0],isct,isct],c='r') 33 | plt.xlim(lims) 34 | 35 | 36 | # plot input 37 | plt.subplot(2,2,4) 38 | plt.hist(data, num_bins, histtype='step') 39 | plt.xlim(lims) 40 | plt.gca().yaxis.set_ticklabels([]) 41 | 42 | 43 | plt.show() 44 | 45 | 46 | normals = normal(loc=0.0, scale=1, size=5000000) 47 | 48 | #rint h(normals).sort() 49 | 50 | 51 | def f(x): 52 | return 2*x + 1 53 | 54 | def g(x): 55 | return (cos(4*(x/2+0.7)))*sin(0.3*x)-0.9*x 56 | return (cos(4*(x/3+0.7)))*sin(0.3*x)-0.9*x 57 | #return -x+1.2*np.sin(0.7*x)+3 58 | return sin(5-.2*x) 59 | 60 | def h(x): return cos(.4*x)*x 61 | 62 | plot_transfer_func (normals, g, lims=(-4,4),num_bins=500) 63 | del(normals) 64 | 65 | #plt.plot(g(np.arange(-10,10,0.1))) 66 | 67 | ''' 68 | 69 | 70 | ys = f(normals) 71 | 72 | 73 | r = np.linspace (min(normals), max(normals), num_bins) 74 | 75 | h= np.histogram(ys, num_bins,density=True) 76 | print h 77 | print len(h[0]), len(h[1][0:-1]) 78 | 79 | #plot output 80 | plt.subplot(2,2,1) 81 | h = np.histogram(ys, num_bins,normed=True) 82 | 83 | p, = plt.plot(h[0],h[1][1:]) 84 | plt.ylim((-10,10)) 85 | plt.xlim((max(h[0]),0)) 86 | 87 | 88 | # plot transfer function 89 | plt.subplot(2,2,2) 90 | x = np.arange(-10,10) 91 | y = 1.2*x + 1 92 | plt.plot (x,y) 93 | plt.plot([0,0],[-10,f(0)],c='r') 94 | plt.ylim((-10,10)) 95 | 96 | # plot input 97 | plt.subplot(2,2,4) 98 | h = np.histogram(normals, num_bins,density=True) 99 | plt.plot(h[1][1:],h[0]) 100 | plt.xlim((-10,10)) 101 | 102 | 103 | plt.show() 104 | ''' -------------------------------------------------------------------------------- /experiments/quaternion.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu Jan 29 18:36:36 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | import numpy as np 9 | from math import sin, cos, atan2, asin 10 | 11 | ''' 12 | psi = yaw 13 | phi = roll 14 | theta = pitch 15 | ''' 16 | 17 | 18 | def e2q(vector): 19 | 20 | roll = vector[0] 21 | pitch = vector[1] 22 | heading = vector[2] 23 | sinhdg = sin(heading/2) 24 | coshdg = cos(heading/2) 25 | 26 | sinroll = sin(roll/2) 27 | cosroll = cos(roll/2) 28 | 29 | sinpitch = sin(pitch/2) 30 | cospitch = cos(pitch/2) 31 | 32 | q0 = cosroll*cospitch*coshdg + sinroll*sinpitch*sinhdg 33 | q1 = sinroll*cospitch*coshdg - cosroll*sinpitch*sinhdg 34 | q2 = cosroll*sinpitch*coshdg + sinroll*cospitch*sinhdg 35 | q3 = cosroll*cospitch*sinhdg - sinroll*sinpitch*coshdg 36 | 37 | return np.array([q0, q1, q2, q3]) 38 | 39 | 40 | def q2e(q): 41 | roll = atan2(2*(q[0]*q[1] + q[2]*q[3]), 1 - 2*(q[1]**2 + q[2]**2)) 42 | pitch = asin(2*(q[0]*q[2] - q[3]*q[1])) 43 | hdg = atan2(2*(q[0]*q[3] + q[1]*q[2]), 1-2*(q[2]**2 + q[3]**2)) 44 | 45 | return np.array([roll, pitch, hdg]) 46 | 47 | 48 | def e2d(e): 49 | return np.degrees(e) 50 | def e2r(e): 51 | return np.radians(e) 52 | 53 | def add(q1,q2): 54 | return np.multiply(q1,q2) 55 | 56 | 57 | 58 | def add2(q1, q2): 59 | Q1 = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3] 60 | Q2 = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2] 61 | Q3 = q1[0]*q2[2] + q1[2]*q2[0] + q1[3]*q2[1] - q1[1]*q2[3] 62 | Q4 = q1[0]*q2[3] + q1[3]*q2[0] + q1[1]*q2[2] - q1[2]*q2[1] 63 | 64 | return np.array([Q1, Q2, Q3, Q4]) 65 | 66 | 67 | e = e2r([10, 0, 0]) 68 | q = e2q(e) 69 | print(q) 70 | print(e2d(q2e(q))) 71 | q2 = add2(q,q) 72 | print(q2) 73 | e2 = q2e(q2) 74 | print(e2d(e2)) 75 | 76 | -------------------------------------------------------------------------------- /experiments/range_finder.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat May 24 19:14:06 2014 4 | 5 | @author: rlabbe 6 | """ 7 | from __future__ import division, print_function 8 | from KalmanFilter import KalmanFilter 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | import numpy.random as random 12 | import math 13 | 14 | 15 | class DMESensor(object): 16 | def __init__(self, pos_a, pos_b, noise_factor=1.0): 17 | self.A = pos_a 18 | self.B = pos_b 19 | self.noise_factor = noise_factor 20 | 21 | def range_of (self, pos): 22 | """ returns tuple containing noisy range data to A and B 23 | given a position 'pos' 24 | """ 25 | 26 | ra = math.sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2) 27 | rb = math.sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2) 28 | 29 | return (ra + random.randn()*self.noise_factor, 30 | rb + random.randn()*self.noise_factor) 31 | 32 | 33 | def dist(a,b): 34 | return math.sqrt ((a[0]-b[0])**2 + (a[1]-b[1])**2) 35 | 36 | def H_of (pos, pos_A, pos_B): 37 | from math import sin, cos, atan2 38 | 39 | theta_a = atan2(pos_a[1]-pos[1], pos_a[0] - pos[0]) 40 | theta_b = atan2(pos_b[1]-pos[1], pos_b[0] - pos[0]) 41 | 42 | return np.mat([[-cos(theta_a), 0, -sin(theta_a), 0], 43 | [-cos(theta_b), 0, -sin(theta_b), 0]]) 44 | 45 | # equivalently we can do this... 46 | #dist_a = dist(pos, pos_A) 47 | #dist_b = dist(pos, pos_B) 48 | 49 | #return np.mat([[(pos[0]-pos_A[0])/dist_a, 0, (pos[1]-pos_A[1])/dist_a,0], 50 | # [(pos[0]-pos_B[0])/dist_b, 0, (pos[1]-pos_B[1])/dist_b,0]]) 51 | 52 | 53 | 54 | 55 | pos_a = (100,-20) 56 | pos_b = (-100, -20) 57 | 58 | f1 = KalmanFilter(dim=4) 59 | dt = 1.0 # time step 60 | ''' 61 | f1.F = np.mat ([[1, dt, 0, 0], 62 | [0, 1, 0, 0], 63 | [0, 0, 1, dt], 64 | [0, 0, 0, 1]]) 65 | 66 | ''' 67 | f1.F = np.mat ([[0, 1, 0, 0], 68 | [0, 0, 0, 0], 69 | [0, 0, 0, 1], 70 | [0, 0, 0, 0]]) 71 | 72 | f1.B = 0. 73 | 74 | f1.R = np.eye(2) * 1. 75 | f1.Q = np.eye(4) * .1 76 | 77 | f1.x = np.mat([1,0,1,0]).T 78 | f1.P = np.eye(4) * 5. 79 | 80 | # initialize storage and other variables for the run 81 | count = 30 82 | xs, ys = [],[] 83 | pxs, pys = [],[] 84 | 85 | d = DMESensor (pos_a, pos_b, noise_factor=1.) 86 | 87 | pos = [0,0] 88 | for i in range(count): 89 | pos = (i,i) 90 | ra,rb = d.range_of(pos) 91 | rx,ry = d.range_of((i+f1.x[0,0], i+f1.x[2,0])) 92 | 93 | print ('range =', ra,rb) 94 | 95 | z = np.mat([[ra-rx],[rb-ry]]) 96 | print('z =', z) 97 | 98 | f1.H = H_of (pos, pos_a, pos_b) 99 | print('H =', f1.H) 100 | 101 | ##f1.update (z) 102 | 103 | print (f1.x) 104 | xs.append (f1.x[0,0]+i) 105 | ys.append (f1.x[2,0]+i) 106 | pxs.append (pos[0]) 107 | pys.append(pos[1]) 108 | f1.predict () 109 | print (f1.H * f1.x) 110 | print (z) 111 | print (f1.x) 112 | f1.update(z) 113 | print(f1.x) 114 | 115 | p1, = plt.plot (xs, ys, 'r--') 116 | p2, = plt.plot (pxs, pys) 117 | plt.legend([p1,p2], ['filter', 'ideal'], 2) 118 | plt.show() 119 | -------------------------------------------------------------------------------- /experiments/satellite.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "metadata": { 3 | "name": "", 4 | "signature": "sha256:9fb5a469b0b0f27f1e721735fae76d74f0ad242621b94c3f528e9392b7deab82" 5 | }, 6 | "nbformat": 3, 7 | "nbformat_minor": 0, 8 | "worksheets": [ 9 | { 10 | "cells": [ 11 | { 12 | "cell_type": "code", 13 | "collapsed": false, 14 | "input": [ 15 | "import math\n", 16 | "\n", 17 | "def range_to(x1, x2):\n", 18 | " dx = x1[0] - x2[0]\n", 19 | " dy = x1[1] - x2[1]\n", 20 | " print('dx',dx,dy)\n", 21 | " return math.sqrt(dx*dx + dy*dy)" 22 | ], 23 | "language": "python", 24 | "metadata": {}, 25 | "outputs": [], 26 | "prompt_number": 40 27 | }, 28 | { 29 | "cell_type": "code", 30 | "collapsed": false, 31 | "input": [ 32 | "s1 = (0., 2.) # satellite 1\n", 33 | "s2 = (1., 2.) # satellite 2\n", 34 | "p = (-0.5, 1.) # platform postion\n", 35 | "\n", 36 | "\n", 37 | "s1 = (0., 200.) # satellite 1\n", 38 | "s2 = (100., 220.) # satellite 2\n", 39 | "p = (10, 1.) # platform postion\n", 40 | "\n", 41 | "r1 = range_to(s1, p)\n", 42 | "r2 = range_to(s2, p)\n", 43 | "\n", 44 | "x1, y1 = s1[0], s1[1]\n", 45 | "x2, y2 = s2[0], s2[1]\n", 46 | "\n", 47 | "A = -(y2 - y1) / (x2 - x1)\n", 48 | "B = (r1**2 - r2**2 - x1**2 - y1**2 + x2**2 + y2**2) / (2*(x2 - x1))\n", 49 | "\n", 50 | "a = 1 + A**2\n", 51 | "b = -2*A*x1 + 2*A*B - 2*y1\n", 52 | "c = x1**2 - 2*x1*B + y1**2 - r1**2\n", 53 | "\n", 54 | "y = (-b - math.sqrt(b**2 - 4*a*c)) / (2*a)\n", 55 | "x = A*y + B\n", 56 | "\n", 57 | "print('p', x,y)\n", 58 | "print('err', x-p[0], y-p[1])" 59 | ], 60 | "language": "python", 61 | "metadata": {}, 62 | "outputs": [ 63 | { 64 | "output_type": "stream", 65 | "stream": "stdout", 66 | "text": [ 67 | "dx -10.0 199.0\n", 68 | "dx 90.0 219.0\n", 69 | "p 10.051726583768032 0.7413670811596572\n", 70 | "err 0.05172658376803163 -0.2586329188403428\n" 71 | ] 72 | } 73 | ], 74 | "prompt_number": 55 75 | }, 76 | { 77 | "cell_type": "code", 78 | "collapsed": false, 79 | "input": [ 80 | "A" 81 | ], 82 | "language": "python", 83 | "metadata": {}, 84 | "outputs": [ 85 | { 86 | "metadata": {}, 87 | "output_type": "pyout", 88 | "prompt_number": 56, 89 | "text": [ 90 | "-0.2" 91 | ] 92 | } 93 | ], 94 | "prompt_number": 56 95 | } 96 | ], 97 | "metadata": {} 98 | } 99 | ] 100 | } -------------------------------------------------------------------------------- /experiments/slam.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Oct 3 07:59:40 2016 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from filterpy.kalman import UnscentedKalmanFilter as UKF, MerweScaledSigmaPoints 9 | from math import tan, sin, cos, sqrt, atan2 10 | import numpy as np 11 | from numpy.random import randn 12 | import matplotlib.pyplot as plt 13 | from filterpy.stats import plot_covariance_ellipse 14 | 15 | 16 | 17 | def move(x, u, dt, wheelbase): 18 | hdg = x[2] 19 | vel = u[0] 20 | steering_angle = u[1] 21 | dist = vel * dt 22 | 23 | if abs(steering_angle) > 0.001: # is robot turning? 24 | beta = (dist / wheelbase) * tan(steering_angle) 25 | r = wheelbase / tan(steering_angle) # radius 26 | 27 | sinh, sinhb = sin(hdg), sin(hdg + beta) 28 | cosh, coshb = cos(hdg), cos(hdg + beta) 29 | return x + np.array([-r*sinh + r*sinhb, 30 | r*cosh - r*coshb, beta]) 31 | else: # moving in straight line 32 | return x + np.array([dist*cos(hdg), dist*sin(hdg), 0]) 33 | 34 | 35 | def fx(x, dt, u): 36 | return move(x, u, dt, wheelbase) 37 | 38 | 39 | def normalize_angle(x): 40 | x = x % (2 * np.pi) # force in range [0, 2 pi) 41 | if x > np.pi: # move to [-pi, pi) 42 | x -= 2 * np.pi 43 | return x 44 | 45 | 46 | def residual_h(a, b): 47 | y = a - b 48 | # data in format [dist_1, bearing_1, dist_2, bearing_2,...] 49 | for i in range(0, len(y), 2): 50 | y[i + 1] = normalize_angle(y[i + 1]) 51 | return y 52 | 53 | 54 | def residual_x(a, b): 55 | y = a - b 56 | y[2] = normalize_angle(y[2]) 57 | return y 58 | 59 | 60 | def aa(x, y): 61 | if y is not None: 62 | return x % y 63 | else: 64 | return x 65 | 66 | def bb(x,y): 67 | try: 68 | return x % y 69 | except: 70 | return x 71 | 72 | 73 | 74 | def Hx(x, landmarks): 75 | """ takes a state variable and returns the measurement 76 | that would correspond to that state. """ 77 | hx = [] 78 | for lmark in landmarks: 79 | px, py = lmark 80 | dist = sqrt((px - x[0])**2 + (py - x[1])**2) 81 | angle = atan2(py - x[1], px - x[0]) 82 | hx.extend([dist, normalize_angle(angle - x[2])]) 83 | return np.array(hx) 84 | 85 | 86 | def state_mean(sigmas, Wm): 87 | x = np.zeros(3) 88 | 89 | sum_sin = np.sum(np.dot(np.sin(sigmas[:, 2]), Wm)) 90 | sum_cos = np.sum(np.dot(np.cos(sigmas[:, 2]), Wm)) 91 | x[0] = np.sum(np.dot(sigmas[:, 0], Wm)) 92 | x[1] = np.sum(np.dot(sigmas[:, 1], Wm)) 93 | x[2] = atan2(sum_sin, sum_cos) 94 | return x 95 | 96 | 97 | def z_mean(sigmas, Wm): 98 | z_count = sigmas.shape[1] 99 | x = np.zeros(z_count) 100 | 101 | for z in range(0, z_count, 2): 102 | sum_sin = np.sum(np.dot(np.sin(sigmas[:, z+1]), Wm)) 103 | sum_cos = np.sum(np.dot(np.cos(sigmas[:, z+1]), Wm)) 104 | 105 | x[z] = np.sum(np.dot(sigmas[:,z], Wm)) 106 | x[z+1] = atan2(sum_sin, sum_cos) 107 | return x 108 | 109 | 110 | dt = 1.0 111 | wheelbase = 0.5 112 | 113 | def run_localization( 114 | cmds, landmarks, sigma_vel, sigma_steer, sigma_range, 115 | sigma_bearing, ellipse_step=1, step=10): 116 | 117 | plt.figure() 118 | points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0, 119 | subtract=residual_x) 120 | ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=fx, hx=Hx, 121 | dt=dt, points=points, x_mean_fn=state_mean, 122 | z_mean_fn=z_mean, residual_x=residual_x, 123 | residual_z=residual_h) 124 | 125 | ukf.x = np.array([2, 6, .3]) 126 | ukf.P = np.diag([.1, .1, .05]) 127 | ukf.R = np.diag([sigma_range**2, 128 | sigma_bearing**2]*len(landmarks)) 129 | ukf.Q = np.eye(3)*0.0001 130 | 131 | sim_pos = ukf.x.copy() 132 | 133 | # plot landmarks 134 | if len(landmarks) > 0: 135 | plt.scatter(landmarks[:, 0], landmarks[:, 1], 136 | marker='s', s=60) 137 | 138 | track = [] 139 | for i, u in enumerate(cmds): 140 | sim_pos = move(sim_pos, u, dt/step, wheelbase) 141 | track.append(sim_pos) 142 | 143 | if i % step == 0: 144 | ukf.predict(fx_args=u) 145 | 146 | if i % ellipse_step == 0: 147 | plot_covariance_ellipse( 148 | (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, 149 | facecolor='k', alpha=0.3) 150 | 151 | x, y = sim_pos[0], sim_pos[1] 152 | z = [] 153 | for lmark in landmarks: 154 | dx, dy = lmark[0] - x, lmark[1] - y 155 | d = sqrt(dx**2 + dy**2) + randn()*sigma_range 156 | bearing = atan2(lmark[1] - y, lmark[0] - x) 157 | a = (normalize_angle(bearing - sim_pos[2] + 158 | randn()*sigma_bearing)) 159 | z.extend([d, a]) 160 | ukf.update(z, hx_args=(landmarks,)) 161 | 162 | if i % ellipse_step == 0: 163 | plot_covariance_ellipse( 164 | (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6, 165 | facecolor='g', alpha=0.8) 166 | track = np.array(track) 167 | plt.plot(track[:, 0], track[:,1], color='k', lw=2) 168 | plt.axis('equal') 169 | plt.title("UKF Robot localization") 170 | plt.show() 171 | return ukf 172 | 173 | 174 | landmarks = np.array([[5, 10], [10, 5], [15, 15]]) 175 | cmds = [np.array([1.1, .01])] * 200 176 | ukf = run_localization( 177 | cmds, landmarks, sigma_vel=0.1, sigma_steer=np.radians(1), 178 | sigma_range=0.3, sigma_bearing=0.1) 179 | print('Final P:', ukf.P.diagonal()) -------------------------------------------------------------------------------- /experiments/slamekf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sun Oct 9 08:25:01 2016 4 | 5 | @author: roger 6 | """ 7 | 8 | import numpy as np 9 | 10 | 11 | class WorldMap(object): 12 | 13 | def __init__(self, N=100): 14 | 15 | self.N = N 16 | pass 17 | 18 | 19 | 20 | 21 | def measurements(self, x, theta): 22 | """ return array of measurements (range, angle) if robot is in position 23 | x""" 24 | 25 | N = 10 26 | a = np.linspace(-np.pi, np.pi, self.N) 27 | return a 28 | 29 | 30 | 31 | def get_line(start, end): 32 | """Bresenham's Line Algorithm 33 | Produces a list of tuples from start and end 34 | 35 | >>> points1 = get_line((0, 0), (3, 4)) 36 | >>> points2 = get_line((3, 4), (0, 0)) 37 | >>> assert(set(points1) == set(points2)) 38 | >>> print points1 39 | [(0, 0), (1, 1), (1, 2), (2, 3), (3, 4)] 40 | >>> print points2 41 | [(3, 4), (2, 3), (1, 2), (1, 1), (0, 0)] 42 | 43 | source: 44 | http://www.roguebasin.com/index.php?title=Bresenham%27s_Line_Algorithm 45 | """ 46 | # Setup initial conditions 47 | x1, y1 = int(round(start[0])), int(round(start[1])) 48 | x2, y2 = int(round(end[0])), int(round(end[1])) 49 | dx = x2 - x1 50 | dy = y2 - y1 51 | 52 | # Determine how steep the line is 53 | is_steep = abs(dy) > abs(dx) 54 | 55 | # Rotate line 56 | if is_steep: 57 | x1, y1 = y1, x1 58 | x2, y2 = y2, x2 59 | 60 | # Swap start and end points if necessary and store swap state 61 | swapped = False 62 | if x1 > x2: 63 | x1, x2 = x2, x1 64 | y1, y2 = y2, y1 65 | swapped = True 66 | # Recalculate differentials 67 | dx = x2 - x1 68 | dy = y2 - y1 69 | 70 | # Calculate error 71 | error = int(dx / 2.0) 72 | ystep = 1 if y1 < y2 else -1 73 | 74 | # Iterate over bounding box generating points between start and end 75 | y = y1 76 | points = [] 77 | for x in range(x1, x2 + 1): 78 | coord = (y, x) if is_steep else (x, y) 79 | points.append(coord) 80 | error -= abs(dy) 81 | if error < 0: 82 | y += ystep 83 | error += dx 84 | 85 | # Reverse the list if the coordinates were swapped 86 | if swapped: 87 | points.reverse() 88 | return points 89 | 90 | 91 | world = np.zeros((1000,1000), dtype=bool) 92 | 93 | 94 | def add_line(p0, p1): 95 | pts = get_line(p0, p1) 96 | for p in pts: 97 | try: 98 | world[p[0], p[1]] = True 99 | except: 100 | pass # ignore out of range 101 | 102 | 103 | add_line((0,0), (1000, 0)) 104 | 105 | def measure(x, theta): 106 | 107 | dx,dy = world.shape 108 | h = np.sqrt(2*(dx*dx + dy+dy)) 109 | p1 = [h*np.cos(theta), h*np.sin(theta)] 110 | 111 | 112 | hits = get_line(x, p1) 113 | 114 | try: 115 | for pt in hits: 116 | if world[pt[0], pt[1]]: 117 | return pt 118 | except: 119 | return -1 120 | return -2 121 | 122 | 123 | 124 | 125 | measure([100,100], -np.pi/2) 126 | -------------------------------------------------------------------------------- /experiments/taylor.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu Mar 19 08:26:08 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | from math import sin, cos, factorial 12 | def df(x, p): 13 | if p == 0: 14 | return sin(x) 15 | 16 | return x 17 | if p % 4 == 1: 18 | return cos(x) 19 | 20 | if p % 4 == 2: 21 | return -sin(x) 22 | 23 | if p % 4 == 3: 24 | return -cos(x) 25 | 26 | return sin(x) 27 | 28 | 29 | 30 | 31 | def taylor(df, x, a, n): 32 | 33 | f = 0.0 34 | 35 | for i in range(n+1): 36 | term = df(a, i) * (x - a)**i / factorial(i) 37 | f += term 38 | 39 | return f 40 | 41 | 42 | x = 0.1 43 | a = 0.8 44 | n = 1 45 | 46 | 47 | 48 | plt.cla() 49 | 50 | xs = np.linspace(-2, 2, 100) 51 | ts = [taylor(df, i, a, n) for i in xs] 52 | 53 | plt.plot(xs, np.sin(xs)) 54 | plt.plot(xs, ts) 55 | 56 | 57 | -------------------------------------------------------------------------------- /experiments/test1d.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu Jun 18 09:16:54 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | 9 | from filterpy.kalman import KalmanFilter 10 | 11 | 12 | 13 | kf = KalmanFilter(1, 1) 14 | kf.x[0,0] = 1. 15 | kf.P = np.ones((1,1)) 16 | kf.H = np.array([[2.]]) 17 | kf.F = np.ones((1,1)) 18 | kf.R = 1 19 | kf.Q = 0 20 | 21 | 22 | 23 | kf.predict() 24 | kf.update(2) 25 | 26 | -------------------------------------------------------------------------------- /experiments/test_stats.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | 4 | py.test module to test stats.py module. 5 | 6 | 7 | Created on Wed Aug 27 06:45:06 2014 8 | 9 | @author: rlabbe 10 | """ 11 | from __future__ import division 12 | from math import pi, exp 13 | import numpy as np 14 | from stats import gaussian, multivariate_gaussian, _to_cov 15 | from numpy.linalg import inv 16 | from numpy import linalg 17 | 18 | 19 | def near_equal(x,y): 20 | return abs(x-y) < 1.e-15 21 | 22 | 23 | def test_gaussian(): 24 | import scipy.stats 25 | 26 | mean = 3. 27 | var = 1.5 28 | std = var**0.5 29 | 30 | for i in np.arange(-5,5,0.1): 31 | p0 = scipy.stats.norm(mean, std).pdf(i) 32 | p1 = gaussian(i, mean, var) 33 | 34 | assert near_equal(p0, p1) 35 | 36 | 37 | 38 | def norm_pdf_multivariate(x, mu, sigma): 39 | """ extremely literal transcription of the multivariate equation. 40 | Slow, but easy to verify by eye compared to my version.""" 41 | 42 | n = len(x) 43 | sigma = _to_cov(sigma,n) 44 | 45 | det = linalg.det(sigma) 46 | 47 | norm_const = 1.0 / (pow((2*pi), n/2) * pow(det, .5)) 48 | x_mu = x - mu 49 | result = exp(-0.5 * (x_mu.dot(inv(sigma)).dot(x_mu.T))) 50 | return norm_const * result 51 | 52 | 53 | 54 | def test_multivariate(): 55 | from scipy.stats import multivariate_normal as mvn 56 | from numpy.random import rand 57 | 58 | mean = 3 59 | var = 1.5 60 | 61 | assert near_equal(mvn(mean,var).pdf(0.5), 62 | multivariate_gaussian(0.5, mean, var)) 63 | 64 | mean = np.array([2.,17.]) 65 | var = np.array([[10., 1.2], [1.2, 4.]]) 66 | 67 | x = np.array([1,16]) 68 | assert near_equal(mvn(mean,var).pdf(x), 69 | multivariate_gaussian(x, mean, var)) 70 | 71 | for i in range(100): 72 | x = np.array([rand(), rand()]) 73 | assert near_equal(mvn(mean,var).pdf(x), 74 | multivariate_gaussian(x, mean, var)) 75 | 76 | assert near_equal(mvn(mean,var).pdf(x), 77 | norm_pdf_multivariate(x, mean, var)) 78 | 79 | 80 | mean = np.array([1,2,3,4]) 81 | var = np.eye(4)*rand() 82 | 83 | x = np.array([2,3,4,5]) 84 | 85 | assert near_equal(mvn(mean,var).pdf(x), 86 | norm_pdf_multivariate(x, mean, var)) 87 | 88 | 89 | -------------------------------------------------------------------------------- /experiments/train.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Apr 4 19:02:39 2015 4 | 5 | @author: Roger 6 | """ 7 | 8 | 9 | import numpy as np 10 | import numpy.random as random 11 | import matplotlib.pyplot as plt 12 | 13 | 14 | def bar_plot(pos, ylim=(0,1), title=None): 15 | plt.cla() 16 | ax = plt.gca() 17 | x = np.arange(len(pos)) 18 | ax.bar(x, pos, color='#30a2da') 19 | if ylim: 20 | plt.ylim(ylim) 21 | plt.xticks(x+0.4, x) 22 | if title is not None: 23 | plt.title(title) 24 | 25 | 26 | 27 | def normalize(belief): 28 | s = sum(belief) 29 | belief /= s 30 | 31 | def update(map_, belief, z, p_hit, p_miss): 32 | for i, val in enumerate(map_): 33 | if val == z: 34 | belief[i] *= p_hit 35 | else: 36 | belief[i] *= p_miss 37 | 38 | belief = normalize(belief) 39 | 40 | 41 | def predict(prob_dist, offset, kernel): 42 | N = len(prob_dist) 43 | kN = len(kernel) 44 | width = int((kN - 1) / 2) 45 | 46 | result = np.zeros(N) 47 | for i in range(N): 48 | for k in range (kN): 49 | index = (i + (width-k) - offset) % N 50 | result[i] += prob_dist[index] * kernel[k] 51 | prob_dist[:] = result[:] # update belief 52 | 53 | 54 | def add_noise (Z, count): 55 | n= len(Z) 56 | for i in range(count): 57 | j = random.randint(0,n) 58 | Z[j] = random.randint(0,2) 59 | 60 | 61 | def animate_three_doors (loops=5): 62 | world = np.array([1,0,1,0,0,0,0,1,0,0,0,1,0,0,0,0,0]) 63 | #world = np.array([1,1,1,1,1]) 64 | #world = np.array([1,0,1,0,1,0]) 65 | 66 | 67 | f = DiscreteBayes1D(world) 68 | 69 | measurements = np.tile(world, loops) 70 | add_noise(measurements, 50) 71 | 72 | for m in measurements: 73 | f.sense (m, .8, .2) 74 | f.update(1, (.05, .9, .05)) 75 | 76 | bar_plot(f.belief) 77 | plt.pause(0.01) 78 | 79 | 80 | def animate_book(loops=5): 81 | world = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0]) 82 | #world = np.array([1,1,1,1,1]) 83 | #world = np.array([1,0,1,0,1,0]) 84 | 85 | N = len(world) 86 | belief = np.array([1./N]*N) 87 | 88 | measurements = np.tile(world, loops) 89 | add_noise(measurements, 5) 90 | 91 | for m in measurements: 92 | update(world, belief, m, .8, .2) 93 | predict(belief, 1, (.05, .9, .05)) 94 | 95 | bar_plot(belief) 96 | plt.pause(0.01) 97 | print(f.belief) 98 | 99 | 100 | import random 101 | 102 | class Train(object): 103 | 104 | def __init__(self, track, kernel=[1.], sense_error=.1, no_sense_error=.05): 105 | self.track = track 106 | self.pos = 0 107 | self.kernel = kernel 108 | self.sense_error = sense_error 109 | self.no_sense_error = no_sense_error 110 | 111 | 112 | def move(self, distance=1): 113 | """ move in the specified direction with some small chance of error""" 114 | 115 | self.pos += distance 116 | 117 | # insert random movement error according to kernel 118 | r = random.random() 119 | s = 0 120 | offset = -(len(self.kernel) - 1) / 2 121 | for k in self.kernel: 122 | s += k 123 | if r <= s: 124 | break 125 | offset += 1 126 | 127 | self.pos = (self.pos + offset) % len(self.track) 128 | return self.pos 129 | 130 | def sense(self): 131 | pos = self.pos 132 | 133 | # insert random sensor error 134 | r = random.random() 135 | if r < self.sense_error: 136 | if random.random() > 0.5: 137 | pos += 1 138 | else: 139 | pos -= 1 140 | print('sense error') 141 | return pos 142 | 143 | 144 | def animate_train(loops=5): 145 | world = np.array([1,2,3,4,5,6,7,8,9,10]) 146 | 147 | N = len(world) 148 | belief = np.zeros(N) 149 | belief[0] = 1.0 150 | 151 | robot = Train(world, [.1, .8, .1], .1, .1) 152 | 153 | for i in range(N*loops): 154 | robot.move(1) 155 | m = robot.sense() 156 | update(world, belief, m, .9, .1) 157 | predict(belief, 1, (.05, .9, .05)) 158 | 159 | bar_plot(belief) 160 | plt.pause(0.1) 161 | print(belief) 162 | 163 | animate_train(3) 164 | 165 | world = np.array([1,2,3,4,5,6,7,8,9,10]) 166 | #world = np.array([1,1,1,1,1]) 167 | #world = np.array([1,0,1,0,1,0]) 168 | 169 | 170 | -------------------------------------------------------------------------------- /experiments/ukf_baseball.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sun Feb 8 09:55:24 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from math import radians, sin, cos, sqrt, exp, atan2, radians 9 | from numpy import array, asarray 10 | from numpy.random import randn 11 | import numpy as np 12 | import math 13 | import matplotlib.pyplot as plt 14 | from filterpy.kalman import UnscentedKalmanFilter as UKF 15 | from filterpy.common import runge_kutta4 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | class BaseballPath(object): 24 | def __init__(self, x0, y0, launch_angle_deg, velocity_ms, 25 | noise=(1.0,1.0)): 26 | """ Create 2D baseball path object 27 | (x = distance from start point in ground plane, 28 | y=height above ground) 29 | 30 | x0,y0 initial position 31 | launch_angle_deg angle ball is travelling respective to 32 | ground plane 33 | velocity_ms speeed of ball in meters/second 34 | noise amount of noise to add to each position 35 | in (x,y) 36 | """ 37 | 38 | omega = radians(launch_angle_deg) 39 | self.v_x = velocity_ms * cos(omega) 40 | self.v_y = velocity_ms * sin(omega) 41 | 42 | self.x = x0 43 | self.y = y0 44 | 45 | self.noise = noise 46 | 47 | 48 | def drag_force(self, velocity): 49 | """ Returns the force on a baseball due to air drag at 50 | the specified velocity. Units are SI 51 | """ 52 | B_m = 0.0039 + 0.0058 / (1. + exp((velocity-35.)/5.)) 53 | return B_m * velocity 54 | 55 | 56 | def update(self, dt, vel_wind=0.): 57 | """ compute the ball position based on the specified time 58 | step and wind velocity. Returns (x,y) position tuple 59 | """ 60 | 61 | # Euler equations for x and y 62 | self.x += self.v_x*dt 63 | self.y += self.v_y*dt 64 | 65 | # force due to air drag 66 | v_x_wind = self.v_x - vel_wind 67 | v = sqrt(v_x_wind**2 + self.v_y**2) 68 | F = self.drag_force(v) 69 | 70 | # Euler's equations for velocity 71 | self.v_x = self.v_x - F*v_x_wind*dt 72 | self.v_y = self.v_y - 9.81*dt - F*self.v_y*dt 73 | 74 | return (self.x, self.y) 75 | 76 | 77 | 78 | radar_pos = (100,0) 79 | omega = 45. 80 | 81 | 82 | def radar_sense(baseball, noise_rng, noise_brg): 83 | x, y = baseball.x, baseball.y 84 | 85 | rx, ry = radar_pos[0], radar_pos[1] 86 | 87 | rng = ((x-rx)**2 + (y-ry)**2) ** .5 88 | bearing = atan2(y-ry, x-rx) 89 | 90 | rng += randn() * noise_rng 91 | bearing += radians(randn() * noise_brg) 92 | 93 | return (rng, bearing) 94 | 95 | 96 | ball = BaseballPath(x0=0, y0=1, launch_angle_deg=45, 97 | velocity_ms=60, noise=[0,0]) 98 | 99 | 100 | ''' 101 | xs = [] 102 | ys = [] 103 | dt = 0.05 104 | y = 1 105 | 106 | while y > 0: 107 | x,y = ball.update(dt) 108 | xs.append(x) 109 | ys.append(y) 110 | 111 | plt.plot(xs, ys) 112 | plt.axis('equal') 113 | 114 | 115 | plt.show() 116 | 117 | ''' 118 | 119 | 120 | dt = 1/30. 121 | 122 | 123 | def hx(x): 124 | global radar_pos 125 | 126 | dx = radar_pos[0] - x[0] 127 | dy = radar_pos[1] - x[2] 128 | rng = (dx*dx + dy*dy)**.5 129 | bearing = atan2(-dy, -dx) 130 | 131 | #print(x) 132 | #print('hx:', rng, np.degrees(bearing)) 133 | 134 | return array([rng, bearing]) 135 | 136 | 137 | 138 | 139 | def fx(x, dt): 140 | 141 | fx.ball.x = x[0] 142 | fx.ball.y = x[2] 143 | fx.ball.vx = x[1] 144 | fx.ball.vy = x[3] 145 | 146 | N = 10 147 | ball_dt = dt/float(N) 148 | 149 | for i in range(N): 150 | fx.ball.update(ball_dt) 151 | 152 | #print('fx', fx.ball.x, fx.ball.v_x, fx.ball.y, fx.ball.v_y) 153 | 154 | return array([fx.ball.x, fx.ball.v_x, fx.ball.y, fx.ball.v_y]) 155 | 156 | 157 | fx.ball = BaseballPath(x0=0, y0=1, launch_angle_deg=45, 158 | velocity_ms=60, noise=[0,0]) 159 | 160 | 161 | y = 1. 162 | x = 0. 163 | theta = 35. # launch angle 164 | v0 = 50. 165 | 166 | 167 | ball = BaseballPath(x0=x, y0=y, launch_angle_deg=theta, 168 | velocity_ms=v0, noise=[.3,.3]) 169 | 170 | kf = UKF(dim_x=4, dim_z=2, dt=dt, hx=hx, fx=fx, kappa=0) 171 | 172 | #kf.R *= r 173 | 174 | kf.R[0,0] = 0.1 175 | kf.R[1,1] = radians(0.2) 176 | omega = radians(omega) 177 | vx = cos(omega) * v0 178 | vy = sin(omega) * v0 179 | 180 | kf.x = array([x, vx, y, vy]) 181 | kf.R*= 0.01 182 | #kf.R[1,1] = 0.01 183 | kf.P *= 10 184 | 185 | f1 = kf 186 | 187 | 188 | t = 0 189 | xs = [] 190 | ys = [] 191 | 192 | while y > 0: 193 | t += dt 194 | x,y = ball.update(dt) 195 | z = radar_sense(ball, 0, 0) 196 | #print('z', z) 197 | #print('ball', ball.x, ball.v_x, ball.y, ball.v_y) 198 | 199 | 200 | f1.predict() 201 | f1.update(z) 202 | xs.append(f1.x[0]) 203 | ys.append(f1.x[2]) 204 | f1.predict() 205 | 206 | p1 = plt.scatter(x, y, color='r', marker='o', s=75, alpha=0.5) 207 | 208 | p2, = plt.plot (xs, ys, lw=2, marker='o') 209 | #p3, = plt.plot (xs2, ys2, lw=4) 210 | #plt.legend([p1,p2, p3], 211 | # ['Measurements', 'Kalman filter(R=0.5)', 'Kalman filter(R=10)'], 212 | # loc='best', scatterpoints=1) 213 | plt.show() 214 | 215 | -------------------------------------------------------------------------------- /experiments/ukf_range.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sun Feb 8 09:34:36 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | from numpy import array, asarray 12 | from numpy.random import randn 13 | import math 14 | from filterpy.kalman import UnscentedKalmanFilter as UKF 15 | 16 | 17 | 18 | class RadarSim(object): 19 | """ Simulates the radar signal returns from an object flying 20 | at a constant altityude and velocity in 1D. 21 | """ 22 | 23 | def __init__(self, dt, pos, vel, alt): 24 | self.pos = pos 25 | self.vel = vel 26 | self.alt = alt 27 | self.dt = dt 28 | 29 | def get_range(self): 30 | """ Returns slant range to the object. Call once for each 31 | new measurement at dt time from last call. 32 | """ 33 | 34 | # add some process noise to the system 35 | self.vel = self.vel + .1*randn() 36 | self.alt = self.alt + .1*randn() 37 | self.pos = self.pos + self.vel*self.dt 38 | 39 | # add measurment noise 40 | err = self.pos * 0.05*randn() 41 | slant_dist = math.sqrt(self.pos**2 + self.alt**2) 42 | 43 | return slant_dist + err 44 | 45 | 46 | 47 | dt = 0.05 48 | 49 | 50 | def hx(x): 51 | return (x[0]**2 + x[2]**2)**.5 52 | pass 53 | 54 | 55 | 56 | def fx(x, dt): 57 | result = x.copy() 58 | result[0] += x[1]*dt 59 | return result 60 | 61 | 62 | 63 | 64 | f = UKF(3, 1, dt= dt, hx=hx, fx=fx, kappa=1) 65 | radar = RadarSim(dt, pos=-1000., vel=100., alt=1000.) 66 | 67 | f.x = array([0, 90, 1005]) 68 | f.R = 0.1 69 | f.Q *= 0.002 70 | 71 | 72 | 73 | 74 | xs = [] 75 | track = [] 76 | 77 | for i in range(int(20/dt)): 78 | z = radar.get_range() 79 | track.append((radar.pos, radar.vel, radar.alt)) 80 | 81 | f.predict() 82 | f.update(array([z])) 83 | 84 | xs.append(f.x) 85 | 86 | 87 | xs = asarray(xs) 88 | track = asarray(track) 89 | time = np.arange(0,len(xs)*dt, dt) 90 | 91 | plt.figure() 92 | plt.subplot(311) 93 | plt.plot(time, track[:,0]) 94 | plt.plot(time, xs[:,0]) 95 | plt.legend(loc=4) 96 | plt.xlabel('time (sec)') 97 | plt.ylabel('position (m)') 98 | 99 | 100 | plt.subplot(312) 101 | plt.plot(time, track[:,1]) 102 | plt.plot(time, xs[:,1]) 103 | plt.legend(loc=4) 104 | plt.xlabel('time (sec)') 105 | plt.ylabel('velocity (m/s)') 106 | 107 | plt.subplot(313) 108 | plt.plot(time, track[:,2]) 109 | plt.plot(time, xs[:,2]) 110 | plt.ylabel('altitude (m)') 111 | plt.legend(loc=4) 112 | plt.xlabel('time (sec)') 113 | plt.ylim((900,1600)) 114 | plt.show() 115 | -------------------------------------------------------------------------------- /experiments/ukfloc.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jun 1 18:13:23 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from filterpy.common import plot_covariance_ellipse 9 | from filterpy.kalman import UnscentedKalmanFilter as UKF 10 | from filterpy.kalman import MerweScaledSigmaPoints, JulierSigmaPoints 11 | from math import tan, sin, cos, sqrt, atan2, radians, sqrt 12 | import matplotlib.pyplot as plt 13 | from numpy import array 14 | import numpy as np 15 | from numpy.random import randn, seed 16 | 17 | 18 | 19 | def normalize_angle(x): 20 | x = x % (2 * np.pi) # force in range [0, 2 pi) 21 | if x > np.pi: # move to [-pi, pi] 22 | x -= 2 * np.pi 23 | return x 24 | 25 | 26 | def residual_h(aa, bb): 27 | y = aa - bb 28 | y[1] = normalize_angle(y[1]) 29 | return y 30 | 31 | 32 | def residual_x(a, b): 33 | y = a - b 34 | y[2] = normalize_angle(y[2]) 35 | return y 36 | 37 | 38 | def move(x, u, dt, wheelbase): 39 | h = x[2] 40 | v = u[0] 41 | steering_angle = u[1] 42 | 43 | dist = v*dt 44 | 45 | if abs(steering_angle) > 0.001: 46 | b = dist / wheelbase * tan(steering_angle) 47 | r = wheelbase / tan(steering_angle) # radius 48 | 49 | sinh = sin(h) 50 | sinhb = sin(h + b) 51 | cosh = cos(h) 52 | coshb = cos(h + b) 53 | 54 | return x + array([-r*sinh + r*sinhb, r*cosh - r*coshb, b]) 55 | else: 56 | return x + array([dist*cos(h), dist*sin(h), 0]) 57 | 58 | 59 | def state_mean(sigmas, Wm): 60 | x = np.zeros(3) 61 | sum_sin, sum_cos = 0., 0. 62 | 63 | for i in range(len(sigmas)): 64 | s = sigmas[i] 65 | x[0] += s[0] * Wm[i] 66 | x[1] += s[1] * Wm[i] 67 | sum_sin += sin(s[2])*Wm[i] 68 | sum_cos += cos(s[2])*Wm[i] 69 | 70 | x[2] = atan2(sum_sin, sum_cos) 71 | return x 72 | 73 | 74 | def z_mean(sigmas, Wm): 75 | x = np.zeros(2) 76 | sum_sin, sum_cos = 0., 0. 77 | 78 | for i in range(len(sigmas)): 79 | s = sigmas[i] 80 | x[0] += s[0] * Wm[i] 81 | sum_sin += sin(s[1])*Wm[i] 82 | sum_cos += cos(s[1])*Wm[i] 83 | 84 | x[1] = atan2(sum_sin, sum_cos) 85 | return x 86 | 87 | 88 | sigma_r = .3 89 | sigma_h = .01#radians(.5)#np.radians(1) 90 | #sigma_steer = radians(10) 91 | dt = 0.1 92 | wheelbase = 0.5 93 | 94 | m = array([[5., 10], [10, 5], [15, 15], [20, 5], [0, 30], [50, 30], [40, 10]]) 95 | #m = array([[5, 10], [10, 5], [15, 15], [20, 5],[5,5], [8, 8.4]])#, [0, 30], [50, 30], [40, 10]]) 96 | m = array([[5, 10], [10, 5]])#, [0, 30], [50, 30], [40, 10]]) 97 | 98 | 99 | def fx(x, dt, u): 100 | return move(x, u, dt, wheelbase) 101 | 102 | 103 | def Hx(x, landmark): 104 | """ takes a state variable and returns the measurement that would 105 | correspond to that state. 106 | """ 107 | px, py = landmark 108 | dist = sqrt((px - x[0])**2 + (py - x[1])**2) 109 | angle = atan2(py - x[1], px - x[0]) 110 | return array([dist, normalize_angle(angle - x[2])]) 111 | 112 | 113 | points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0) 114 | #points = JulierSigmaPoints(n=3, kappa=3) 115 | ukf= UKF(dim_x=3, dim_z=2, fx=fx, hx=Hx, dt=dt, points=points, 116 | x_mean_fn=state_mean, z_mean_fn=z_mean, 117 | residual_x=residual_x, residual_z=residual_h) 118 | ukf.x = array([2, 6, .3]) 119 | ukf.P = np.diag([.1, .1, .2]) 120 | ukf.R = np.diag([sigma_r**2, sigma_h**2]) 121 | ukf.Q = None#np.eye(3)*.00000 122 | 123 | 124 | u = array([1.1, 0.]) 125 | 126 | xp = ukf.x.copy() 127 | 128 | 129 | plt.cla() 130 | plt.scatter(m[:, 0], m[:, 1]) 131 | 132 | cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)] 133 | cmds.extend([cmds[-1]]*50) 134 | 135 | v = cmds[-1][0] 136 | cmds.extend([[v, a] for a in np.linspace(0, np.radians(2), 15)]) 137 | cmds.extend([cmds[-1]]*100) 138 | 139 | cmds.extend([[v, a] for a in np.linspace(np.radians(2), -np.radians(2), 15)]) 140 | cmds.extend([cmds[-1]]*200) 141 | 142 | cmds.extend([[v, a] for a in np.linspace(-np.radians(2), 0, 15)]) 143 | cmds.extend([cmds[-1]]*150) 144 | 145 | #cmds.extend([[v, a] for a in np.linspace(0, -np.radians(1), 25)]) 146 | 147 | 148 | 149 | seed(12) 150 | cmds = np.array(cmds) 151 | 152 | cindex = 0 153 | u = cmds[0] 154 | 155 | track = [] 156 | 157 | while cindex < len(cmds): 158 | u = cmds[cindex] 159 | xp = move(xp, u, dt, wheelbase) # simulate robot 160 | track.append(xp) 161 | 162 | ukf.predict(fx_args=u) 163 | 164 | if cindex % 20 == 30: 165 | plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=18, 166 | facecolor='b', alpha=0.58) 167 | 168 | #print(cindex, ukf.P.diagonal()) 169 | print(xp) 170 | for lmark in m: 171 | 172 | d = sqrt((lmark[0] - xp[0])**2 + (lmark[1] - xp[1])**2) + randn()*sigma_r 173 | bearing = atan2(lmark[1] - xp[1], lmark[0] - xp[0]) 174 | a = normalize_angle(bearing - xp[2] + randn()*sigma_h) 175 | z = np.array([d, a]) 176 | 177 | if cindex % 20 == 0: 178 | plt.plot([lmark[0], lmark[0] - d*cos(a+xp[2])], [lmark[1], lmark[1]-d*sin(a+xp[2])], color='r') 179 | 180 | ukf.update(z, hx_args=(lmark,)) 181 | print(ukf.P) 182 | print() 183 | 184 | if cindex % 20 == 0: 185 | plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=15, 186 | facecolor='g', alpha=0.99) 187 | cindex += 1 188 | 189 | 190 | track = np.array(track) 191 | plt.plot(track[:, 0], track[:,1], color='k') 192 | 193 | plt.axis('equal') 194 | plt.title("UKF Robot localization") 195 | plt.show() 196 | print(ukf.P.diagonal()) 197 | -------------------------------------------------------------------------------- /experiments/ukfloc2.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jun 1 18:13:23 2015 4 | 5 | @author: rlabbe 6 | """ 7 | 8 | from filterpy.common import plot_covariance_ellipse 9 | from filterpy.kalman import UnscentedKalmanFilter as UKF 10 | from filterpy.kalman import MerweScaledSigmaPoints 11 | from math import tan, sin, cos, sqrt, atan2, radians 12 | import matplotlib.pyplot as plt 13 | from numpy import array 14 | import numpy as np 15 | from numpy.random import randn, seed 16 | 17 | 18 | 19 | def normalize_angle(x): 20 | x = x % (2 * np.pi) # force in range [0, 2 pi) 21 | if x > np.pi: # move to [-pi, pi] 22 | x -= 2 * np.pi 23 | return x 24 | 25 | 26 | def residual_h(aa, bb): 27 | y = aa - bb 28 | for i in range(0, len(y), 2): 29 | y[i + 1] = normalize_angle(y[i + 1]) 30 | return y 31 | 32 | 33 | def residual_x(a, b): 34 | y = a - b 35 | y[2] = normalize_angle(y[2]) 36 | return y 37 | 38 | 39 | def move(x, u, dt, wheelbase): 40 | h = x[2] 41 | v = u[0] 42 | steering_angle = u[1] 43 | 44 | dist = v*dt 45 | 46 | if abs(steering_angle) > 0.001: 47 | b = dist / wheelbase * tan(steering_angle) 48 | r = wheelbase / tan(steering_angle) # radius 49 | 50 | sinh = sin(h) 51 | sinhb = sin(h + b) 52 | cosh = cos(h) 53 | coshb = cos(h + b) 54 | 55 | return x + array([-r*sinh + r*sinhb, r*cosh - r*coshb, b]) 56 | else: 57 | return x + array([dist*cos(h), dist*sin(h), 0]) 58 | 59 | 60 | def state_mean(sigmas, Wm): 61 | x = np.zeros(3) 62 | 63 | sum_sin = np.sum(np.dot(np.sin(sigmas[:, 2]), Wm)) 64 | sum_cos = np.sum(np.dot(np.cos(sigmas[:, 2]), Wm)) 65 | 66 | x[0] = np.sum(np.dot(sigmas[:, 0], Wm)) 67 | x[1] = np.sum(np.dot(sigmas[:, 1], Wm)) 68 | x[2] = atan2(sum_sin, sum_cos) 69 | 70 | return x 71 | 72 | 73 | def z_mean(sigmas, Wm): 74 | z_count = sigmas.shape[1] 75 | x = np.zeros(z_count) 76 | 77 | for z in range(0, z_count, 2): 78 | sum_sin = np.sum(np.dot(np.sin(sigmas[:, z+1]), Wm)) 79 | sum_cos = np.sum(np.dot(np.cos(sigmas[:, z+1]), Wm)) 80 | 81 | x[z] = np.sum(np.dot(sigmas[:,z], Wm)) 82 | x[z+1] = atan2(sum_sin, sum_cos) 83 | return x 84 | 85 | 86 | def fx(x, dt, u): 87 | return move(x, u, dt, wheelbase) 88 | 89 | 90 | def Hx(x, landmark): 91 | """ takes a state variable and returns the measurement that would 92 | correspond to that state. 93 | """ 94 | 95 | hx = [] 96 | for lmark in landmark: 97 | px, py = lmark 98 | dist = sqrt((px - x[0])**2 + (py - x[1])**2) 99 | angle = atan2(py - x[1], px - x[0]) 100 | hx.extend([dist, normalize_angle(angle - x[2])]) 101 | return np.array(hx) 102 | 103 | 104 | m = array([[5., 10], [10, 5], [15, 15], [20., 16], [0, 30], [50, 30], [40, 10]]) 105 | #m = array([[5, 10], [10, 5], [15, 15], [20, 5],[5,5], [8, 8.4]])#, [0, 30], [50, 30], [40, 10]]) 106 | #m = array([[5, 10], [10, 5]])#, [0, 30], [50, 30], [40, 10]]) 107 | #m = array([[5., 10], [10, 5]]) 108 | #m = array([[5., 10], [10, 5]]) 109 | 110 | 111 | sigma_r = .3 112 | sigma_h = .1#radians(.5)#np.radians(1) 113 | #sigma_steer = radians(10) 114 | dt = 0.1 115 | wheelbase = 0.5 116 | 117 | points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0, subtract=residual_x) 118 | #points = JulierSigmaPoints(n=3, kappa=3) 119 | ukf= UKF(dim_x=3, dim_z=2*len(m), fx=fx, hx=Hx, dt=dt, points=points, 120 | x_mean_fn=state_mean, z_mean_fn=z_mean, 121 | residual_x=residual_x, residual_z=residual_h) 122 | ukf.x = array([2, 6, .3]) 123 | ukf.P = np.diag([.1, .1, .05]) 124 | ukf.R = np.diag([sigma_r**2, sigma_h**2]* len(m)) 125 | ukf.Q =np.eye(3)*.00001 126 | 127 | 128 | u = array([1.1, 0.]) 129 | 130 | xp = ukf.x.copy() 131 | 132 | 133 | plt.cla() 134 | plt.scatter(m[:, 0], m[:, 1]) 135 | 136 | cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)] 137 | cmds.extend([cmds[-1]]*50) 138 | 139 | v = cmds[-1][0] 140 | cmds.extend([[v, a] for a in np.linspace(0, np.radians(2), 15)]) 141 | cmds.extend([cmds[-1]]*100) 142 | 143 | cmds.extend([[v, a] for a in np.linspace(np.radians(2), -np.radians(2), 15)]) 144 | cmds.extend([cmds[-1]]*200) 145 | 146 | cmds.extend([[v, a] for a in np.linspace(-np.radians(2), 0, 15)]) 147 | cmds.extend([cmds[-1]]*150) 148 | 149 | 150 | seed(12) 151 | cmds = np.array(cmds) 152 | 153 | cindex = 0 154 | u = cmds[0] 155 | 156 | track = [] 157 | 158 | std = 16 159 | while cindex < len(cmds): 160 | u = cmds[cindex] 161 | xp = move(xp, u, dt, wheelbase) # simulate robot 162 | track.append(xp) 163 | 164 | ukf.predict(fx_args=u) 165 | 166 | if cindex % 20 == 0: 167 | plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=std, 168 | facecolor='b', alpha=0.58) 169 | 170 | #print(cindex, ukf.P.diagonal()) 171 | #print(ukf.P.diagonal()) 172 | z = [] 173 | for lmark in m: 174 | d = sqrt((lmark[0] - xp[0])**2 + (lmark[1] - xp[1])**2) + randn()*sigma_r 175 | bearing = atan2(lmark[1] - xp[1], lmark[0] - xp[0]) 176 | a = normalize_angle(bearing - xp[2] + randn()*sigma_h) 177 | z.extend([d, a]) 178 | 179 | #if cindex % 20 == 0: 180 | # plt.plot([lmark[0], lmark[0] - d*cos(a+xp[2])], [lmark[1], lmark[1]-d*sin(a+xp[2])], color='r') 181 | 182 | if cindex == 1197: 183 | plt.plot([lmark[0], lmark[0] - d2*cos(a2+xp[2])], 184 | [lmark[1], lmark[1] - d2*sin(a2+xp[2])], color='r') 185 | plt.plot([lmark[0], lmark[0] - d*cos(a+xp[2])], 186 | [lmark[1], lmark[1] - d*sin(a+xp[2])], color='k') 187 | 188 | ukf.update(np.array(z), hx_args=(m,)) 189 | 190 | if cindex % 20 == 0: 191 | plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=std, 192 | facecolor='g', alpha=0.5) 193 | cindex += 1 194 | 195 | 196 | track = np.array(track) 197 | plt.plot(track[:, 0], track[:,1], color='k') 198 | 199 | plt.axis('equal') 200 | plt.title("UKF Robot localization") 201 | plt.show() 202 | -------------------------------------------------------------------------------- /experiments/zarchan_ball.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "metadata": { 3 | "name": "", 4 | "signature": "sha256:cfd803b3edb531968a1e9983b9d281824050b61d08da023ead5045daf7c8c4e7" 5 | }, 6 | "nbformat": 3, 7 | "nbformat_minor": 0, 8 | "worksheets": [ 9 | { 10 | "cells": [ 11 | { 12 | "cell_type": "code", 13 | "collapsed": false, 14 | "input": [ 15 | "import sympy\n", 16 | "from sympy import Symbol\n", 17 | "\n", 18 | "from sympy.abc import x, g, b\n", 19 | "\n", 20 | "xdd = (0.0034*g*sympy.exp(-x/22000)*sympy.diff(x)**2) / (2*b) - g\n", 21 | "y=sympy.symbols('y',cls=sympy.Function)\n", 22 | "\n", 23 | "xdd = sympy.diff(y(x), x, x) - (0.0034*g*sympy.exp(-x/22000)*sympy.diff(x)**2) + g\n", 24 | "\n", 25 | "\n", 26 | "sympy.pprint(xdd)\n" 27 | ], 28 | "language": "python", 29 | "metadata": {}, 30 | "outputs": [ 31 | { 32 | "output_type": "stream", 33 | "stream": "stdout", 34 | "text": [ 35 | " -x \n", 36 | " \u2500\u2500\u2500\u2500\u2500 2 \n", 37 | " 22000 d \n", 38 | "g - 0.0034\u22c5g\u22c5\u212f + \u2500\u2500\u2500(y(x))\n", 39 | " 2 \n", 40 | " dx \n" 41 | ] 42 | } 43 | ], 44 | "prompt_number": 10 45 | } 46 | ], 47 | "metadata": {} 48 | } 49 | ] 50 | } -------------------------------------------------------------------------------- /figs/gh_errorbar1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_errorbar1.png -------------------------------------------------------------------------------- /figs/gh_errorbar2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_errorbar2.png -------------------------------------------------------------------------------- /figs/gh_errorbar3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_errorbar3.png -------------------------------------------------------------------------------- /figs/gh_estimate1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_estimate1.png -------------------------------------------------------------------------------- /figs/gh_estimate2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_estimate2.png -------------------------------------------------------------------------------- /figs/gh_estimate3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_estimate3.png -------------------------------------------------------------------------------- /figs/gh_hypothesis1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_hypothesis1.png -------------------------------------------------------------------------------- /figs/gh_hypothesis2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_hypothesis2.png -------------------------------------------------------------------------------- /figs/gh_hypothesis3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_hypothesis3.png -------------------------------------------------------------------------------- /figs/gh_hypothesis4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_hypothesis4.png -------------------------------------------------------------------------------- /figs/gh_hypothesis5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_hypothesis5.png -------------------------------------------------------------------------------- /figs/gh_predict_update.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/gh_predict_update.png -------------------------------------------------------------------------------- /figs/residual_chart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/residual_chart.png -------------------------------------------------------------------------------- /figs/residual_chart_with_h.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/figs/residual_chart_with_h.png -------------------------------------------------------------------------------- /kf_book/538.json: -------------------------------------------------------------------------------- 1 | { 2 | "lines.linewidth": 1.5, 3 | "patch.linewidth": 0.5, 4 | "legend.fancybox": true, 5 | "axes.facecolor": "#ffffff", 6 | "axes.axisbelow": true, 7 | "axes.grid": true, 8 | "patch.edgecolor": "#f0f0f0", 9 | "examples.directory": "", 10 | "figure.facecolor": "#ffffff", 11 | "grid.linestyle": "dotted", 12 | "grid.linewidth": 2.0, 13 | "grid.color": "#cbcbcb", 14 | "axes.edgecolor":"#f0f0f0", 15 | "xtick.major.size": 0, 16 | "xtick.minor.size": 0, 17 | "ytick.major.size": 0, 18 | "ytick.minor.size": 0, 19 | "axes.linewidth": 3.0, 20 | "font.size":12.0, 21 | "lines.solid_capstyle": "butt", 22 | "savefig.edgecolor": "#f0f0f0", 23 | "savefig.facecolor": "#f0f0f0", 24 | "figure.subplot.left" : 0.08, 25 | "figure.subplot.right" : 0.95, 26 | "figure.subplot.bottom" : 0.07, 27 | "figure.subplot.hspace" : 0.5, 28 | "legend.scatterpoints" : 1 29 | } 30 | 31 | -------------------------------------------------------------------------------- /kf_book/DogSimulation.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | 20 | import copy 21 | import math 22 | import numpy as np 23 | from numpy.random import randn 24 | 25 | class DogSimulation(object): 26 | 27 | def __init__(self, x0=0, velocity=1, 28 | measurement_var=0.0, process_var=0.0): 29 | """ x0 - initial position 30 | velocity - (+=right, -=left) 31 | measurement_variance - variance in measurement m^2 32 | process_variance - variance in process (m/s)^2 33 | """ 34 | self.x = x0 35 | self.velocity = velocity 36 | self.measurement_noise = math.sqrt(measurement_var) 37 | self.process_noise = math.sqrt(process_var) 38 | 39 | 40 | def move(self, dt=1.0): 41 | '''Compute new position of the dog assuming `dt` seconds have 42 | passed since the last update.''' 43 | # compute new position based on velocity. Add in some 44 | # process noise 45 | velocity = self.velocity + randn() * self.process_noise * dt 46 | self.x += velocity * dt 47 | 48 | 49 | def sense_position(self): 50 | # simulate measuring the position with noise 51 | return self.x + randn() * self.measurement_noise 52 | 53 | 54 | def move_and_sense(self, dt=1.0): 55 | self.move(dt) 56 | x = copy.deepcopy(self.x) 57 | return x, self.sense_position() 58 | 59 | 60 | def run_simulation(self, dt=1, count=1): 61 | """ simulate the dog moving over a period of time. 62 | 63 | **Returns** 64 | data : np.array[float, float] 65 | 2D array, first column contains actual position of dog, 66 | second column contains the measurement of that position 67 | """ 68 | return np.array([self.move_and_sense(dt) for i in range(count)]) 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /kf_book/LICENSE.txt: -------------------------------------------------------------------------------- 1 | he MIT License (MIT) 2 | 3 | Copyright (c) 2015 Roger R. Labbe Jr 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. -------------------------------------------------------------------------------- /kf_book/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/kf_book/__init__.py -------------------------------------------------------------------------------- /kf_book/adaptive_internal.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | import kf_book.book_plots as bp 20 | from matplotlib.patches import Circle, Rectangle, Polygon, Arrow, FancyArrow 21 | import matplotlib.pyplot as plt 22 | import numpy as np 23 | 24 | 25 | def plot_track_and_residuals(dt, xs, z_xs, res): 26 | """ plots track and measurement on the left, and the residual 27 | of the filter on the right. Helps to visualize the performance of 28 | an adaptive filter. 29 | """ 30 | 31 | assert np.isscalar(dt) 32 | t = np.arange(0, len(xs)*dt, dt) 33 | plt.subplot(121) 34 | if z_xs is not None: 35 | bp.plot_measurements(t, z_xs, label='z') 36 | bp.plot_filter(t, xs) 37 | plt.legend(loc=2) 38 | plt.xlabel('time (sec)') 39 | plt.ylabel('X') 40 | plt.title('estimates vs measurements') 41 | plt.subplot(122) 42 | # plot twice so it has the same color as the plot to the left! 43 | plt.plot(t, res) 44 | plt.plot(t, res) 45 | plt.xlabel('time (sec)') 46 | plt.ylabel('residual') 47 | plt.title('residuals') 48 | plt.show() 49 | 50 | 51 | def plot_markov_chain(): 52 | """ show a markov chain showing relative probability of an object 53 | turning""" 54 | 55 | fig = plt.figure(figsize=(4,4), facecolor='w') 56 | ax = plt.axes((0, 0, 1, 1), 57 | xticks=[], yticks=[], frameon=False) 58 | 59 | box_bg = '#DDDDDD' 60 | 61 | kf1c = Circle((4,5), 0.5, fc=box_bg) 62 | kf2c = Circle((6,5), 0.5, fc=box_bg) 63 | ax.add_patch (kf1c) 64 | ax.add_patch (kf2c) 65 | 66 | plt.text(4,5, "Straight",ha='center', va='center', fontsize=14) 67 | plt.text(6,5, "Turn",ha='center', va='center', fontsize=14) 68 | 69 | 70 | #btm 71 | plt.text(5, 3.9, ".05", ha='center', va='center', fontsize=18) 72 | ax.annotate('', 73 | xy=(4.1, 4.5), xycoords='data', 74 | xytext=(6, 4.5), textcoords='data', 75 | size=10, 76 | arrowprops=dict(arrowstyle="->", 77 | ec="k", 78 | connectionstyle="arc3,rad=-0.5")) 79 | #top 80 | plt.text(5, 6.1, ".03", ha='center', va='center', fontsize=18) 81 | ax.annotate('', 82 | xy=(6, 5.5), xycoords='data', 83 | xytext=(4.1, 5.5), textcoords='data', 84 | size=10, 85 | 86 | arrowprops=dict(arrowstyle="->", 87 | ec="k", 88 | connectionstyle="arc3,rad=-0.5")) 89 | 90 | plt.text(3.5, 5.6, ".97", ha='center', va='center', fontsize=18) 91 | ax.annotate('', 92 | xy=(3.9, 5.5), xycoords='data', 93 | xytext=(3.55, 5.2), textcoords='data', 94 | size=10, 95 | arrowprops=dict(arrowstyle="->", 96 | ec="k", 97 | connectionstyle="angle3,angleA=150,angleB=0")) 98 | 99 | plt.text(6.5, 5.6, ".95", ha='center', va='center', fontsize=18) 100 | ax.annotate('', 101 | xy=(6.1, 5.5), xycoords='data', 102 | xytext=(6.45, 5.2), textcoords='data', 103 | size=10, 104 | arrowprops=dict(arrowstyle="->", 105 | fc="0.2", ec="k", 106 | connectionstyle="angle3,angleA=-150,angleB=2")) 107 | 108 | 109 | plt.axis('equal') 110 | plt.show() 111 | 112 | 113 | def turning_target(N=600, turn_start=400): 114 | """ simulate a moving target""" 115 | 116 | #r = 1. 117 | dt = 1. 118 | phi_sim = np.array( 119 | [[1, dt, 0, 0], 120 | [0, 1, 0, 0], 121 | [0, 0, 1, dt], 122 | [0, 0, 0, 1]]) 123 | 124 | gam = np.array([[dt**2/2, 0], 125 | [dt, 0], 126 | [0, dt**2/2], 127 | [0, dt]]) 128 | 129 | x = np.array([[2000, 0, 10000, -15.]]).T 130 | 131 | simxs = [] 132 | 133 | for i in range(N): 134 | x = np.dot(phi_sim, x) 135 | if i >= turn_start: 136 | x += np.dot(gam, np.array([[.075, .075]]).T) 137 | simxs.append(x) 138 | simxs = np.array(simxs) 139 | 140 | return simxs 141 | 142 | 143 | if __name__ == "__main__": 144 | d = turning_target() -------------------------------------------------------------------------------- /kf_book/custom.css: -------------------------------------------------------------------------------- 1 | 206 | 229 | -------------------------------------------------------------------------------- /kf_book/gaussian_internal.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | import filterpy.stats as stats 20 | import math 21 | import matplotlib.pyplot as plt 22 | import numpy as np 23 | 24 | def plot_height_std(x, lw=10): 25 | m = np.mean(x) 26 | s = np.std(x) 27 | 28 | for i, height in enumerate(x): 29 | plt.plot([i+1, i+1], [0, height], color='k', lw=lw) 30 | plt.xlim(0, len(x)+1) 31 | plt.axhline(m-s, ls='--') 32 | plt.axhline(m+s, ls='--') 33 | plt.fill_between((0, len(x)+1), m-s, m+s, 34 | facecolor='yellow', alpha=0.4) 35 | plt.xlabel('student') 36 | plt.ylabel('height (m)') 37 | plt.show() 38 | 39 | 40 | def plot_correlated_data(X, Y, xlabel=None, 41 | ylabel=None, equal=True): 42 | 43 | plt.scatter(X, Y) 44 | 45 | if xlabel is not None: 46 | plt.xlabel('Height (in)'); 47 | 48 | if ylabel is not None: 49 | plt.ylabel('Weight (lbs)') 50 | 51 | # fit line through data 52 | m, b = np.polyfit(X, Y, 1) 53 | plt.plot(X, np.asarray(X)*m + b,color='k') 54 | if equal: 55 | plt.gca().set_aspect('equal') 56 | plt.show() 57 | 58 | def plot_gaussian(mu, variance, 59 | mu_line=False, 60 | xlim=None, 61 | xlabel=None, 62 | ylabel=None): 63 | 64 | xs = np.arange(mu-variance*2,mu+variance*2,0.1) 65 | ys = [stats.gaussian (x, mu, variance)*100 for x in xs] 66 | plt.plot (xs, ys) 67 | if mu_line: 68 | plt.axvline(mu) 69 | if xlim: 70 | plt.xlim(xlim) 71 | if xlabel: 72 | plt.xlabel(xlabel) 73 | if ylabel: 74 | plt.ylabel(ylabel) 75 | plt.show() 76 | 77 | 78 | def display_stddev_plot(): 79 | xs = np.arange(10,30,0.1) 80 | var = 8; 81 | stddev = math.sqrt(var) 82 | p2, = plt.plot (xs,[stats.gaussian(x, 20, var) for x in xs]) 83 | x = 20+stddev 84 | # 1std vertical lines 85 | y = stats.gaussian(x, 20, var) 86 | plt.plot ([x,x], [0,y],'g') 87 | plt.plot ([20-stddev, 20-stddev], [0,y], 'g') 88 | 89 | #2std vertical lines 90 | x = 20+2*stddev 91 | y = stats.gaussian(x, 20, var) 92 | plt.plot ([x,x], [0,y],'g') 93 | plt.plot ([20-2*stddev, 20-2*stddev], [0,y], 'g') 94 | 95 | y = stats.gaussian(20,20,var) 96 | plt.plot ([20,20],[0,y],'b') 97 | 98 | x = 20+stddev 99 | ax = plt.gca() 100 | ax.annotate('68%', xy=(20.3, 0.045)) 101 | ax.annotate('', xy=(20-stddev,0.04), xytext=(x,0.04), 102 | arrowprops=dict(arrowstyle="<->", 103 | ec="r", 104 | shrinkA=2, shrinkB=2)) 105 | ax.annotate('95%', xy=(20.3, 0.02)) 106 | ax.annotate('', xy=(20-2*stddev,0.015), xytext=(20+2*stddev,0.015), 107 | arrowprops=dict(arrowstyle="<->", 108 | ec="r", 109 | shrinkA=2, shrinkB=2)) 110 | 111 | 112 | ax.xaxis.set_ticks ([20-2*stddev, 20-stddev, 20, 20+stddev, 20+2*stddev]) 113 | ax.xaxis.set_ticklabels(['$-2\sigma$', '$-1\sigma$','$\mu$','$1\sigma$', '$2\sigma$']) 114 | ax.yaxis.set_ticks([]) 115 | ax.grid(None, 'both', lw=0) 116 | 117 | if __name__ == '__main__': 118 | display_stddev_plot() -------------------------------------------------------------------------------- /kf_book/gif_animate.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | from matplotlib import animation 20 | import matplotlib.pyplot as plt 21 | 22 | 23 | 24 | def animate(filename, func, frames, interval, fig=None, figsize=(6.5, 6.5)): 25 | """ Creates an animated GIF of a matplotlib. 26 | 27 | Parameters 28 | ---------- 29 | filename : string 30 | name of the file. E.g 'foo.GIF' or '\home\monty\parrots\fjords.gif' 31 | 32 | func : function 33 | function that will be called once per frame. Must have signature of 34 | def fun_name(frame_num) 35 | 36 | frames : int 37 | number of frames to animate. The current frame number will be passed 38 | into func at each call. 39 | 40 | interval : float 41 | Milliseconds to pause on each frame in the animation. E.g. 500 for half 42 | a second. 43 | 44 | figsize : (float, float) optional 45 | size of the figure in inches. Defaults to 6.5" by 6.5" 46 | """ 47 | 48 | def init_func(): 49 | """ This draws the 'blank' frame of the video. To work around a bug 50 | in matplotlib 1.5.1 (#5399) you must supply an empty init function 51 | for the save to work.""" 52 | 53 | pass 54 | 55 | if fig is None: 56 | fig = plt.figure(figsize=figsize) 57 | 58 | anim = animation.FuncAnimation(fig, func, init_func=init_func, 59 | frames=frames, interval=interval) 60 | 61 | import os 62 | basename = os.path.splitext(filename)[0] 63 | anim.save(basename + '.mp4', writer='ffmpeg') 64 | 65 | os.system("ffmpeg -y -i {}.mp4 {}.gif".format(basename, basename)) 66 | os.remove(basename + '.mp4') -------------------------------------------------------------------------------- /kf_book/kf_design_internal.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | from filterpy.kalman import UnscentedKalmanFilter as UKF 20 | from filterpy.kalman import JulierSigmaPoints 21 | from math import atan2 22 | import numpy as np 23 | 24 | def hx(x): 25 | """ compute measurements corresponding to state x""" 26 | dx = x[0] - hx.radar_pos[0] 27 | dy = x[1] - hx.radar_pos[1] 28 | return np.array([atan2(dy,dx), (dx**2 + dy**2)**.5]) 29 | 30 | def fx(x,dt): 31 | """ predict state x at 'dt' time in the future""" 32 | return x 33 | 34 | def set_radar_pos(pos): 35 | global hx 36 | hx.radar_pos = pos 37 | 38 | def sensor_fusion_kf(): 39 | global hx, fx 40 | # create unscented Kalman filter with large initial uncertainty 41 | points = JulierSigmaPoints(2, kappa=2) 42 | kf = UKF(2, 2, dt=0.1, hx=hx, fx=fx, points=points) 43 | kf.x = np.array([100, 100.]) 44 | kf.P *= 40 45 | return kf 46 | 47 | -------------------------------------------------------------------------------- /kf_book/kf_internal.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | import kf_book.book_plots as bp 20 | import filterpy.stats as stats 21 | from math import sqrt 22 | import matplotlib.pyplot as plt 23 | import numpy as np 24 | from numpy.random import randn, seed 25 | 26 | 27 | def plot_dog_track(xs, dog, measurement_var, process_var): 28 | N = len(xs) 29 | bp.plot_track(dog) 30 | bp.plot_measurements(xs, label='Sensor') 31 | bp.set_labels('variance = {}, process variance = {}'.format( 32 | measurement_var, process_var), 'time', 'pos') 33 | plt.ylim([0, N]) 34 | bp.show_legend() 35 | plt.show() 36 | 37 | 38 | def print_gh(predict, update, z): 39 | predict_template = '{: 7.3f} {: 8.3f}' 40 | update_template = '{:.3f}\t{: 7.3f} {: 7.3f}' 41 | 42 | print(predict_template.format(predict[0], predict[1]),end='\t') 43 | print(update_template.format(z, update[0], update[1])) 44 | 45 | 46 | def print_variance(positions): 47 | for i in range(0, len(positions), 5): 48 | print('\t{:.4f} {:.4f} {:.4f} {:.4f} {:.4f}'.format( 49 | *[v[1] for v in positions[i:i+5]])) 50 | 51 | 52 | 53 | def gaussian_vs_histogram(): 54 | 55 | seed(15) 56 | xs = np.arange(0, 20, 0.1) 57 | ys = np.array([stats.gaussian(x-10, 0, 2) for x in xs]) 58 | bar_ys = abs(ys + randn(len(xs)) * stats.gaussian(xs-10, 0, 10)/2) 59 | plt.gca().bar(xs[::5]-.25, bar_ys[::5], width=0.5, color='g') 60 | plt.plot(xs, ys, lw=3, color='k') 61 | plt.xlim(5, 15) 62 | 63 | 64 | class DogSimulation(object): 65 | def __init__(self, x0=0, velocity=1, 66 | measurement_var=0.0, 67 | process_var=0.0): 68 | """ x0 : initial position 69 | velocity: (+=right, -=left) 70 | measurement_var: variance in measurement m^2 71 | process_var: variance in process (m/s)^2 72 | """ 73 | self.x = x0 74 | self.velocity = velocity 75 | self.meas_std = sqrt(measurement_var) 76 | self.process_std = sqrt(process_var) 77 | 78 | def move(self, dt=1.0): 79 | """Compute new position of the dog in dt seconds.""" 80 | dx = self.velocity + randn()*self.process_std 81 | self.x += dx * dt 82 | 83 | def sense_position(self): 84 | """ Returns measurement of new position in meters.""" 85 | measurement = self.x + randn()*self.meas_std 86 | return measurement 87 | 88 | def move_and_sense(self): 89 | """ Move dog, and return measurement of new position in meters""" 90 | self.move() 91 | return self.sense_position() 92 | 93 | -------------------------------------------------------------------------------- /kf_book/nonlinear_internal.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | import filterpy.stats as stats 20 | import matplotlib.pyplot as plt 21 | import numpy as np 22 | 23 | 24 | def plot1(): 25 | P = np.array([[6, 2.5], [2.5, .6]]) 26 | stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2) 27 | 28 | 29 | def plot2(): 30 | P = np.array([[6, 2.5], [2.5, .6]]) 31 | circle1=plt.Circle((10,0),3,color='#004080',fill=False,linewidth=4, alpha=.7) 32 | ax = plt.gca() 33 | ax.add_artist(circle1) 34 | plt.xlim(0,10) 35 | plt.ylim(0,3) 36 | P = np.array([[6, 2.5], [2.5, .6]]) 37 | stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2) 38 | 39 | def plot3(): 40 | P = np.array([[6, 2.5], [2.5, .6]]) 41 | circle1=plt.Circle((10,0),3,color='#004080',fill=False,linewidth=4, alpha=.7) 42 | ax = plt.gca() 43 | ax.add_artist(circle1) 44 | plt.xlim(0,10) 45 | plt.ylim(0,3) 46 | plt.axhline(3, ls='--') 47 | stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2) 48 | 49 | def plot4(): 50 | P = np.array([[6, 2.5], [2.5, .6]]) 51 | circle1=plt.Circle((10,0),3,color='#004080',fill=False,linewidth=4, alpha=.7) 52 | ax = plt.gca() 53 | ax.add_artist(circle1) 54 | plt.xlim(0,10) 55 | plt.ylim(0,3) 56 | plt.axhline(3, ls='--') 57 | stats.plot_covariance_ellipse((10, 2), P, facecolor='g', alpha=0.2) 58 | plt.scatter([11.4], [2.65],s=200) 59 | plt.scatter([12], [3], c='r', s=200) 60 | plt.show() -------------------------------------------------------------------------------- /kf_book/smoothing_internal.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | """Copyright 2015 Roger R Labbe Jr. 4 | 5 | 6 | Code supporting the book 7 | 8 | Kalman and Bayesian Filters in Python 9 | https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python 10 | 11 | 12 | This is licensed under an MIT license. See the LICENSE.txt file 13 | for more information. 14 | """ 15 | 16 | from __future__ import (absolute_import, division, print_function, 17 | unicode_literals) 18 | 19 | import matplotlib.pyplot as plt 20 | 21 | 22 | def show_fixed_lag_numberline(): 23 | fig = plt.figure() 24 | ax = fig.add_subplot(111) 25 | ax.set_xlim(0,10) 26 | ax.set_ylim(0,10) 27 | 28 | # draw lines 29 | xmin = 1 30 | xmax = 9 31 | y = 5 32 | height = 1 33 | 34 | plt.hlines(y, xmin, xmax) 35 | plt.vlines(xmin, y - height / 2., y + height / 2.) 36 | plt.vlines(4.5, y - height / 2., y + height / 2.) 37 | plt.vlines(6, y - height / 2., y + height / 2.) 38 | plt.vlines(xmax, y - height / 2., y + height / 2.) 39 | plt.vlines(xmax-1, y - height / 2., y + height / 2.) 40 | 41 | # add numbers 42 | plt.text(xmin, y-1.1, '$x_0$', fontsize=20, horizontalalignment='center') 43 | plt.text(xmax, y-1.1, '$x_k$', fontsize=20, horizontalalignment='center') 44 | plt.text(xmax-1, y-1.1, '$x_{k-1}$', fontsize=20, horizontalalignment='center') 45 | plt.text(4.5, y-1.1, '$x_{k-N+1}$', fontsize=20, horizontalalignment='center') 46 | plt.text(6, y-1.1, '$x_{k-N+2}$', fontsize=20, horizontalalignment='center') 47 | plt.text(2.7, y-1.1, '.....', fontsize=20, horizontalalignment='center') 48 | plt.text(7.2, y-1.1, '.....', fontsize=20, horizontalalignment='center') 49 | 50 | plt.axis('off') 51 | plt.show() 52 | 53 | if __name__ == '__main__': 54 | 55 | #show_2d_transform() 56 | #show_sigma_selections() 57 | 58 | show_sigma_transform(True) 59 | #show_four_gps() 60 | #show_sigma_transform() 61 | #show_sigma_selections() 62 | 63 | -------------------------------------------------------------------------------- /license.html: -------------------------------------------------------------------------------- 1 | Creative Commons License
Kalman Filters and Random Signals in Python by Roger Labbe is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.
Based on a work at https://github.com/rlabbe/Kalman-Filters-and-Random-Signals-in-Python 2 | 3 | -------------------------------------------------------------------------------- /pdf/6x9build_book.bat: -------------------------------------------------------------------------------- 1 | REM copy /Y ..\14-Adaptive-Filtering.ipynb book.ipynb 2 | python merge_book.py 3 | 4 | ipython nbconvert --to latex --template book6x9 book.ipynb 5 | ipython to_pdf.py 6 | REM move /Y book.pdf book6x9.pdf 7 | -------------------------------------------------------------------------------- /pdf/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/loveuav/Kalman-and-Bayesian-Filters-in-Python/f9a9dbeb363ad5054452cefbc6a95af9c81678bc/pdf/__init__.py -------------------------------------------------------------------------------- /pdf/book.tplx: -------------------------------------------------------------------------------- 1 | 2 | % Default to the notebook output style 3 | ((* if not cell_style is defined *)) 4 | ((* set cell_style = 'style_ipython.tplx' *)) 5 | ((* endif *)) 6 | 7 | % Inherit from the specified cell style. 8 | ((* extends cell_style *)) 9 | 10 | 11 | 12 | 13 | ((* block docclass *)) 14 | \documentclass{book} 15 | \setcounter{chapter}{0} 16 | \raggedbottom 17 | ((* endblock docclass *)) 18 | 19 | ((* block preamble *)) 20 | 21 | ((* endblock preamble *)) 22 | 23 | ((* block title *)) 24 | \title{Kalman and Bayesian Filters in Python} 25 | \author{Roger R Labbe Jr} 26 | ((* endblock title *)) 27 | 28 | ((* block predoc *)) 29 | ((( super() ))) 30 | ((* block tableofcontents *))\tableofcontents((* endblock tableofcontents *)) 31 | ((* endblock predoc *)) 32 | 33 | ((* block markdowncell scoped *)) 34 | ((( cell.source | citation2latex | strip_files_prefix | convert_pandoc('markdown+tex_math_double_backslash', 'json',extra_args=[]) | resolve_references | convert_pandoc('json','latex', extra_args=["--top-level-division=chapter"]) ))) 35 | ((* endblock markdowncell *)) 36 | 37 | -------------------------------------------------------------------------------- /pdf/book6x9.tplx: -------------------------------------------------------------------------------- 1 | % Inherit from report 2 | ((* extends 'report.tplx' *)) 3 | 4 | 5 | ((* block margins *)) 6 | \geometry{verbose,papersize={6in, 9in}, tmargin=.5in,bmargin=.5in,lmargin=.75in,rmargin=.75in} 7 | ((* endblock margins *)) 8 | 9 | ((*- block in_prompt -*)) 10 | ((*- endblock in_prompt -*)) 11 | 12 | ((* block input scoped *)) 13 | $\fontsize{5}{1.5} 14 | \selectfont$ 15 | $\noindent\rule{8cm}{0.4pt}$ 16 | ((( cell.source | highlight2latex | indent(0) ))) 17 | $\noindent\rule{8cm}{0.4pt}$ 18 | ((* endblock input *)) 19 | 20 | ((* block output_prompt *)) 21 | ((* endblock output_prompt *)) 22 | 23 | 24 | ((* block docclass *)) 25 | \documentclass{book} 26 | \setcounter{chapter}{0} 27 | \raggedbottom 28 | ((* endblock docclass *)) 29 | 30 | ((* block document *)) 31 | ((* endblock document *)) 32 | 33 | ((* block title *)) 34 | \title{Kalman and Bayesian Filters in Python} 35 | \author{Roger R Labbe Jr} 36 | ((* endblock title *)) 37 | 38 | ((* block markdowncell scoped *)) 39 | \fontsize{10}{1.5} 40 | \selectfont 41 | \setlength{\parindent}{0em} 42 | \setlength{\parskip}{0.5em} 43 | ((( cell.source | citation2latex | strip_files_prefix | markdown2latex(extra_args=["--chapters"]) ))) 44 | ((* endblock markdowncell *)) 45 | 46 | -------------------------------------------------------------------------------- /pdf/book_old.tplx: -------------------------------------------------------------------------------- 1 | ((* extends 'report.tplx' *)) 2 | 3 | %=============================================================================== 4 | % Latex Book 5 | %=============================================================================== 6 | 7 | 8 | ((* block title *)) 9 | \title{Kalman and Bayesian Filters in Python} 10 | \author{Roger R Labbe Jr} 11 | \date{} 12 | \setcounter{secnumdepth}{3} 13 | \setcounter{tocdepth}{3} 14 | ((* endblock title *)) 15 | 16 | ((* block abstract *))\tableofcontents((* endblock abstract *)) 17 | 18 | % Define block headings 19 | % Note: latex will only number headings that aren't starred 20 | % (i.e. \subsection , but not \subsection* ) 21 | 22 | ((* block h1 -*))\chapter((* endblock h1 -*)) 23 | ((* block h2 -*))\section((* endblock h2 -*)) 24 | ((* block h3 -*))\subsection((* endblock h3 -*)) 25 | ((* block h4 -*))\subsubsection((* endblock h4 -*)) 26 | ((* block h5 -*))\paragraph((* endblock h5 -*)) 27 | ((* block h6 -*))\subparagraph((* endblock h6 -*)) 28 | -------------------------------------------------------------------------------- /pdf/book_to_pdf.bat: -------------------------------------------------------------------------------- 1 | ipython nbconvert --to latex --template book --post PDF book.ipynb 2 | -------------------------------------------------------------------------------- /pdf/build_book: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | #echo "merging book..." 4 | ipython merge_book.py 5 | 6 | echo "creating pdf..." 7 | ipython nbconvert --to latex --template book book.ipynb 8 | ipython to_pdf.py 9 | 10 | mv Kalman_and_Bayesian_Filters_in_Python.pdf .. 11 | 12 | 13 | -------------------------------------------------------------------------------- /pdf/build_book.bat: -------------------------------------------------------------------------------- 1 | call run_notebooks 2 | cd .. 3 | ipython merge_book.py 4 | 5 | jupyter nbconvert --to latex --template book book.ipynb 6 | ipython to_pdf.py 7 | move /Y book.pdf ../Kalman_and_Bayesian_Filters_in_Python.pdf 8 | -------------------------------------------------------------------------------- /pdf/build_book6x9.bat: -------------------------------------------------------------------------------- 1 | ipython nbconvert --to latex --template book6x9 book.ipynb 2 | ipython to_pdf.py 3 | move /Y book.pdf ../Kalman_and_Bayesian_Filters_in_Python6x9.pdf 4 | 5 | -------------------------------------------------------------------------------- /pdf/build_book_html.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | echo "merging book..." 4 | 5 | 6 | echo "creating html..." 7 | ipython nbconvert --to=html table_of_contents.ipynb 8 | ipython nbconvert --to=html Preface.ipynb 9 | ipython nbconvert --to=html 01_gh_filter/g-h_filter.ipynb 10 | 11 | 12 | echo "done." 13 | 14 | -------------------------------------------------------------------------------- /pdf/build_html_ipynb.py: -------------------------------------------------------------------------------- 1 | 2 | from __future__ import print_function 3 | import IPython.nbformat as nbformat 4 | 5 | from formatting import * 6 | from os.path import join 7 | 8 | def prep_for_html_conversion(filename): 9 | added_appendix = False 10 | with io.open(join('..', filename), 'r', encoding='utf-8') as f: 11 | nb = nbformat.read(f, nbformat.NO_CONVERT) 12 | remove_formatting(nb) 13 | if not added_appendix and filename[0:8] == 'Appendix': 14 | remove_links_add_appendix(nb) 15 | added_appendix = True 16 | else: 17 | remove_links(nb) 18 | nbformat.write(nb, join('html', filename)) 19 | 20 | 21 | if __name__ == '__main__': 22 | notebooks = \ 23 | ['table_of_contents.ipynb', 24 | '00-Preface.ipynb', 25 | '01-g-h-filter.ipynb', 26 | '02-Discrete-Bayes.ipynb', 27 | '03-Gaussians.ipynb', 28 | '04-One-Dimensional-Kalman-Filters.ipynb', 29 | '05-Multivariate-Gaussians.ipynb', 30 | '06-Multivariate-Kalman-Filters.ipynb', 31 | '07-Kalman-Filter-Math.ipynb', 32 | '08-Designing-Kalman-Filters.ipynb', 33 | '09-Nonlinear-Filtering.ipynb', 34 | '10-Unscented-Kalman-Filter.ipynb', 35 | '11-Extended-Kalman-Filters.ipynb', 36 | '12-Particle-Filters.ipynb', 37 | '13-Smoothing.ipynb', 38 | '14-Adaptive-Filtering.ipynb', 39 | 'Appendix-A-Installation.ipynb', 40 | 'Appendix-B-Symbols-and-Notations.ipynb', 41 | 'Appendix-D-HInfinity-Filters.ipynb', 42 | 'Appendix-E-Ensemble-Kalman-Filters.ipynb'] 43 | 44 | for n in notebooks: 45 | prep_for_html_conversion(n) -------------------------------------------------------------------------------- /pdf/clean_book: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | rm --f *.tex 3 | rm --f *.toc 4 | rm --f *.aux 5 | rm --f *.log 6 | rm --f *.out 7 | rm --f ./*_files/*.png 8 | rm --f Kalman_and_Bayesian_Filters_in_Python.ipynb 9 | rm --f Kalman_and_Bayesian_Filters_in_Python.toc 10 | rm --f Kalman_and_Bayesian_Filters_in_Python.tex 11 | rm --f short.ipynb 12 | rm --f chapter.ipynb 13 | rm --f chapter.pdf 14 | 15 | rmdir ./*_files/ 2> /dev/null 16 | 17 | if (( $# == 1)); then 18 | if [ "@1" == all ]; then 19 | rm Kalman_and_Bayesian_Filters_in_Python.pdf; 20 | fi 21 | fi 22 | -------------------------------------------------------------------------------- /pdf/clean_book.bat: -------------------------------------------------------------------------------- 1 | REM WINDOWS script to delete all files 2 | 3 | rm --f *.tex 4 | rm --f *.toc 5 | rm --f *.aux 6 | rm --f *.log 7 | rm --f *.out 8 | rm --f book.ipynb 9 | rm --f book.toc 10 | rm --f book.tex 11 | 12 | rm --f chapter.ipynb 13 | rm --f chapter.pdf 14 | 15 | rmdir /S /Q book_files 16 | rmdir /S /Q chapter_files 17 | rmdir /S /Q tmp 18 | -------------------------------------------------------------------------------- /pdf/formatting.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import io 3 | import nbformat 4 | import sys 5 | 6 | 7 | def remove_formatting(nb): 8 | cells = nb['cells'] 9 | for i in range (len(cells)): 10 | if 'source' in cells[i].keys(): 11 | if cells[i]['source'][0:16] == '#format the book': 12 | del cells[i] 13 | return 14 | 15 | 16 | def remove_links(nb): 17 | c = nb['cells'] 18 | for i in range (len(c)): 19 | if 'source' in c[i].keys(): 20 | if c[i]['source'][0:19] == '[Table of Contents]': 21 | del c[i] 22 | return 23 | 24 | 25 | def remove_links_add_appendix(nb): 26 | c = nb['cells'] 27 | for i in range (len(c)): 28 | if 'source' in c[i].keys(): 29 | if c[i]['source'][0:19] == '[Table of Contents]': 30 | c[i]['source'] = '\\appendix' 31 | return 32 | -------------------------------------------------------------------------------- /pdf/html_book: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | echo "merging book..." 4 | 5 | python merge_book.py > Kalman_and_Bayesian_Filters_in_Python.ipynb 6 | 7 | echo "creating HTML..." 8 | ipython nbconvert Kalman_and_Bayesian_Filters_in_Python.ipynb 9 | mv -f Kalman_and_Bayesian_Filters_in_Python.html .. 10 | 11 | -------------------------------------------------------------------------------- /pdf/html_build_book: -------------------------------------------------------------------------------- 1 | ipython build_html_ipynb.py 2 | 3 | jupyter nbconvert index.ipynb 4 | jupyter nbconvert ../00_Preface.ipynb 5 | jupyter nbconvert ../01_g-h_filter.ipynb 6 | jupyter nbconvert ../02_Discrete_Bayes.ipynb 7 | jupyter nbconvert ../03_Least_Squares_Filters.ipynb 8 | jupyter nbconvert ../04_Gaussians.ipynb 9 | jupyter nbconvert ../05_Kalman_Filters.ipynb 10 | jupyter nbconvert ../06_Multivariate_Kalman_Filters.ipynb 11 | jupyter nbconvert ../07_Kalman_Filter_Math.ipynb 12 | jupyter nbconvert ../08_Designing_Kalman_Filters.ipynb 13 | jupyter nbconvert ../09_Nonlinear_Filtering.ipynb 14 | jupyter nbconvert ../10_Unscented_Kalman_Filter.ipynb 15 | jupyter nbconvert ../11_Extended_Kalman_Filters.ipynb 16 | jupyter nbconvert ../12_Designing_Nonlinear_Kalman_Filters.ipynb 17 | jupyter nbconvert ../13_Smoothing.ipynb 18 | jupyter nbconvert ../14_Adaptive_Filtering.ipynb 19 | jupyter nbconvert ../15_HInfinity_Filters.ipynb 20 | jupyter nbconvert ../16_Ensemble_Kalman_Filters.ipynb 21 | jupyter nbconvert ../Appendix_A_Installation.ipynb 22 | jupyter nbconvert ../Appendix_B_Symbols_and_Notations.ipynb 23 | -------------------------------------------------------------------------------- /pdf/html_build_book.bat: -------------------------------------------------------------------------------- 1 | rmdir /s/q html 2 | mkdir html 3 | ipython build_html_ipynb.py 4 | cd html 5 | 6 | jupyter nbconvert table_of_contents.ipynb 7 | jupyter nbconvert 00-Preface.ipynb 8 | jupyter nbconvert 01-g-h-filter.ipynb 9 | jupyter nbconvert 02-Discrete-Bayes.ipynb 10 | jupyter nbconvert 03-Gaussians.ipynb 11 | jupyter nbconvert 04-One-Dimensional-Kalman-Filters.ipynb 12 | jupyter nbconvert 05-Multivariate-Gaussians.ipynb 13 | jupyter nbconvert 06-Multivariate-Kalman-Filters.ipynb 14 | jupyter nbconvert 07-Kalman-Filter-Math.ipynb 15 | jupyter nbconvert 08-Designing-Kalman-Filters.ipynb 16 | jupyter nbconvert 09-Nonlinear-Filtering.ipynb 17 | jupyter nbconvert 10-Unscented-Kalman-Filter.ipynb 18 | jupyter nbconvert 11-Extended-Kalman-Filters.ipynb 19 | jupyter nbconvert 12-Particle-Filters.ipynb 20 | jupyter nbconvert 13-Smoothing.ipynb 21 | jupyter nbconvert 14-Adaptive-Filtering.ipynb 22 | jupyter nbconvert Appendix-A-Installation.ipynb 23 | jupyter nbconvert Appendix-B-Symbols-and-Notations.ipynb 24 | 25 | 26 | -------------------------------------------------------------------------------- /pdf/index.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "

Kalman and Bayesian Filters in Python

\n", 8 | "

\n", 9 | "

\n", 10 | "Table of Contents\n", 11 | "-----\n", 12 | "\n", 13 | "[**Preface**](00_Preface.html)\n", 14 | " \n", 15 | "Motivation behind writing the book. How to download and read the book. Requirements for IPython Notebook and Python. github links.\n", 16 | "\n", 17 | "\n", 18 | "[**Chapter 1: The g-h Filter**](01_g-h_filter.html)\n", 19 | "\n", 20 | "Intuitive introduction to the g-h filter, which is a family of filters that includes the Kalman filter. Not filler - once you understand this chapter you will understand the concepts behind the Kalman filter. \n", 21 | "\n", 22 | "\n", 23 | "[**Chapter 2: The Discrete Bayes Filter**](02_Discrete_Bayes.html)\n", 24 | "\n", 25 | "Introduces the Discrete Bayes Filter. From this you will learn the probabilistic reasoning that underpins the Kalman filter in an easy to digest form.\n", 26 | "\n", 27 | "\n", 28 | "[**Chapter 3: Least Squares Filter**](03_Least_Squares_Filters.html)\n", 29 | "\n", 30 | "Introduces the least squares filter in batch and recursive forms. I've not made a start on authoring this yet.\n", 31 | "\n", 32 | "\n", 33 | "[**Chapter 4: Gaussian Probabilities**](04_Gaussians.html)\n", 34 | "\n", 35 | "Introduces using Gaussians to represent beliefs in the Bayesian sense. Gaussians allow us to implement the algorithms used in the Discrete Bayes Filter to work in continuous domains.\n", 36 | "\n", 37 | "\n", 38 | "[**Chapter 5: One Dimensional Kalman Filters**](05_Kalman_Filters.html)\n", 39 | "\n", 40 | "Implements a Kalman filter by modifying the Discrete Bayesian Filter to use Gaussians. This is a full featured Kalman filter, albeit only useful for 1D problems. \n", 41 | "\n", 42 | "\n", 43 | "[**Chapter 6: Multivariate Kalman Filter**](/06_Multivariate_Kalman_Filters.html)\n", 44 | "\n", 45 | "We extend the Kalman filter developed in the previous chapter to the full, generalized filter. \n", 46 | "\n", 47 | "\n", 48 | "[**Chapter 7: Kalman Filter Math**](07_Kalman_Filter_Math.html)\n", 49 | "\n", 50 | "We gotten about as far as we can without forming a strong mathematical foundation. This chapter is optional, especially the first time, but if you intend to write robust, numerically stable filters, or to read the literature, you will need to know this. \n", 51 | "\n", 52 | "*This still needs a lot of work. *\n", 53 | "\n", 54 | "\n", 55 | "[**Chapter 8: Designing Kalman Filters**](08_Designing_Kalman_Filters.html)\n", 56 | "\n", 57 | "Building on material in Chapter 6, walks you through the design of several Kalman filters. Discusses, but does not solve issues like numerical stability.\n", 58 | "\n", 59 | "\n", 60 | "[**Chapter 9: Nonlinear Filtering**](09_Nonlinear_Filtering.html)\n", 61 | "\n", 62 | "Kalman filter as covered only work for linear problems. Here I introduce the problems that nonlinear systems pose to the filter, and briefly discuss the various algorithms that we will be learning in subsequent chapters which work with nonlinear systems.\n", 63 | "\n", 64 | "\n", 65 | "[**Chapter 10: Unscented Kalman Filters**](10_Unscented_Kalman_Filter.html)\n", 66 | "\n", 67 | "Unscented Kalman filters (UKF) are a recent development in Kalman filter theory. They allow you to filter nonlinear problems without requiring a closed form solution like the Extended Kalman filter requires.\n", 68 | "\n", 69 | "\n", 70 | "[**Chapter 11: Extended Kalman Filters**](11_Extended_Kalman_Filters.html)\n", 71 | "\n", 72 | "Kalman filter as covered only work for linear problems. Extended Kalman filters (EKF) are the most common approach to linearizing non-linear problems.\n", 73 | "\n", 74 | "*Still very early going on this chapter.*\n", 75 | "\n", 76 | "\n", 77 | "[**Chapter 12: Designing Nonlinear Kalman Filters**](12_Designing_Nonlinear_Kalman_Filters.html)\n", 78 | "\n", 79 | "Works through some examples of the design of Kalman filters for nonlinear problems. *This is still very much a work in progress.*\n", 80 | "\n", 81 | "\n", 82 | "[**Chapter 13: Smoothing**](13_Smoothing.html)\n", 83 | "\n", 84 | "Kalman filters are recursive, and thus very suitable for real time filtering. However, they work well for post-processing data. We discuss some common approaches.\n", 85 | "\n", 86 | "\n", 87 | "[**Chapter 14: Adaptive Filtering**](14_Adaptive_Filtering.html)\n", 88 | " \n", 89 | "Kalman filters assume a single process model, but manuevering targets typically need to be described by several different process models. Adaptive filtering uses several techniques to allow the Kalman filter to adapt to the changing behavior of the target.\n", 90 | "\n", 91 | "\n", 92 | "[**Chapter 15: H-Infinity Filters**](15_HInfinity_Filters.html)\n", 93 | " \n", 94 | "Describes the $H_\\infty$ filter. \n", 95 | "\n", 96 | "*I have code that implements the filter, but no supporting text yet.*\n", 97 | "\n", 98 | "\n", 99 | "[**Chapter 16: Ensemble Kalman Filters**](16_Ensemble_Kalman_Filters.html)\n", 100 | "\n", 101 | "Discusses the ensemble Kalman Filter, which uses a Monte Carlo approach to deal with very large Kalman filter states in nonlinear systems.\n", 102 | "\n", 103 | "\n", 104 | "[**Appendix: Installation, Python, NumPy, and filterpy**](Appendix_A_Installation.html)\n", 105 | "\n", 106 | "Brief introduction of Python and how it is used in this book. Description of the companion\n", 107 | "library filterpy. \n", 108 | " \n", 109 | "\n", 110 | "[**Appendix: Symbols and Notations**](Appendix_B_Symbols_and_Notations.html)\n", 111 | "\n", 112 | "Symbols and notations used in this book. Comparison with notations used in the literature.\n", 113 | "\n", 114 | "*Still just a collection of notes at this point.*\n", 115 | "\n", 116 | "\n", 117 | "### Github repository\n", 118 | "http://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python\n" 119 | ] 120 | } 121 | ], 122 | "metadata": { 123 | "kernelspec": { 124 | "display_name": "Python 3", 125 | "language": "python", 126 | "name": "python3" 127 | }, 128 | "language_info": { 129 | "codemirror_mode": { 130 | "name": "ipython", 131 | "version": 3 132 | }, 133 | "file_extension": ".py", 134 | "mimetype": "text/x-python", 135 | "name": "python", 136 | "nbconvert_exporter": "python", 137 | "pygments_lexer": "ipython3", 138 | "version": "3.4.3" 139 | } 140 | }, 141 | "nbformat": 4, 142 | "nbformat_minor": 0 143 | } 144 | -------------------------------------------------------------------------------- /pdf/make_chapter.bat: -------------------------------------------------------------------------------- 1 | cp %1 chapter.ipynb 2 | ipython nbconvert --to latex chapter.ipynb 3 | ipython to_pdf.py chapter.tex 4 | 5 | 6 | -------------------------------------------------------------------------------- /pdf/merge_book.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import io 3 | import nbformat 4 | import sys 5 | from formatting import * 6 | 7 | 8 | def inplace_change(filename, old_string, new_string): 9 | # Safely read the input filename using 'with' 10 | with open(filename, encoding='utf-8') as f: 11 | s = f.read() 12 | if old_string not in s: 13 | return 14 | 15 | # Safely write the changed content, if found in the file 16 | with open(filename, 'w', encoding='utf-8') as f: 17 | s = s.replace(old_string, new_string) 18 | f.write(s) 19 | 20 | 21 | def merge_notebooks(outfile, filenames): 22 | merged = None 23 | added_appendix = False 24 | for fname in filenames: 25 | with io.open(fname, 'r', encoding='utf-8') as f: 26 | nb = nbformat.read(f, nbformat.NO_CONVERT) 27 | #remove_formatting(nb) 28 | if not added_appendix and fname[0:8] == 'Appendix': 29 | remove_links_add_appendix(nb) 30 | added_appendix = True 31 | else: 32 | remove_links(nb) 33 | if merged is None: 34 | merged = nb 35 | else: 36 | merged.cells.extend(nb.cells) 37 | #merged.metadata.name += "_merged" 38 | 39 | outfile.write(nbformat.writes(merged, nbformat.NO_CONVERT)) 40 | 41 | 42 | if __name__ == '__main__': 43 | with open('book.ipynb', 'w', encoding='utf-8') as f: 44 | 45 | merge_notebooks(f, 46 | ['./tmp/00-Preface.ipynb', 47 | './tmp/01-g-h-filter.ipynb', 48 | './tmp/02-Discrete-Bayes.ipynb', 49 | './tmp/03-Gaussians.ipynb', 50 | './tmp/04-One-Dimensional-Kalman-Filters.ipynb', 51 | './tmp/05-Multivariate-Gaussians.ipynb', 52 | './tmp/06-Multivariate-Kalman-Filters.ipynb', 53 | './tmp/07-Kalman-Filter-Math.ipynb', 54 | './tmp/08-Designing-Kalman-Filters.ipynb', 55 | './tmp/09-Nonlinear-Filtering.ipynb', 56 | './tmp/10-Unscented-Kalman-Filter.ipynb', 57 | './tmp/11-Extended-Kalman-Filters.ipynb', 58 | './tmp/12-Particle-Filters.ipynb', 59 | './tmp/13-Smoothing.ipynb', 60 | './tmp/14-Adaptive-Filtering.ipynb', 61 | './tmp/Appendix-A-Installation.ipynb', 62 | './tmp/Appendix-B-Symbols-and-Notations.ipynb', 63 | './tmp/Appendix-D-HInfinity-Filters.ipynb', 64 | './tmp/Appendix-E-Ensemble-Kalman-Filters.ipynb']) 65 | 66 | 67 | #remove text printed for matplotlib charts 68 | inplace_change('book.ipynb', '', '') 69 | inplace_change('book.ipynb', '', '') -------------------------------------------------------------------------------- /pdf/nbmerge.py: -------------------------------------------------------------------------------- 1 | """ 2 | usage: 3 | 4 | python nbmerge.py A.ipynb B.ipynb C.ipynb > merged.ipynb 5 | """ 6 | 7 | import io 8 | import os 9 | import sys 10 | 11 | from IPython.nbformat import current 12 | 13 | def merge_notebooks(filenames): 14 | merged = None 15 | for fname in filenames: 16 | with io.open(fname, 'r', encoding='utf-8') as f: 17 | nb = current.read(f, 'json') 18 | if merged is None: 19 | merged = nb 20 | else: 21 | merged.worksheets[0].cells.extend(nb.worksheets[0].cells) 22 | merged.metadata.name += "_merged" 23 | print current.writes(merged, 'json') 24 | 25 | if __name__ == '__main__': 26 | merge_notebooks(sys.argv[1:]) 27 | -------------------------------------------------------------------------------- /pdf/readme.txt: -------------------------------------------------------------------------------- 1 | This directory contains code to convert the book into the PDF file. The normal 2 | build process is to cd into this directory, and run build_book from the command 3 | line. If the build is successful (no errors printed), then run clean_book from 4 | the command line. clean_book is not run automatically because if there is an 5 | error you probably need to look at the intermediate output to debug the issue. 6 | 7 | I build the PDF my merging all of the notebooks into one huge one. I strip out 8 | the initial cells for the book formatting and table of contents, and do a few 9 | other things so it renders well in PDF. 10 | 11 | I used to do this in Unix, but switched to Windows. The Unix scripts have not 12 | been kept up to date. 13 | 14 | There is also some experimental code to convert to html. 15 | 16 | The files with short in the name combine only a couple of notebooks together. 17 | I use this to test the production without having to wait the relatively long 18 | time required to produce the entire book. Mostly this is for testing the 19 | scripts. 20 | 21 | No one but me should need to run this stuff, but if you fork the project and 22 | want to generate a PDF, this is how you do it. 23 | -------------------------------------------------------------------------------- /pdf/report_old.tplx: -------------------------------------------------------------------------------- 1 | 2 | % Default to the notebook output style 3 | ((* if not cell_style is defined *)) 4 | ((* set cell_style = 'style_ipython.tplx' *)) 5 | ((* endif *)) 6 | 7 | % Inherit from the specified cell style. 8 | ((* extends cell_style *)) 9 | 10 | 11 | %=============================================================================== 12 | % Latex Book 13 | %=============================================================================== 14 | 15 | %((* block predoc *)) 16 | % ((( super() ))) 17 | % ((* block tableofcontents *))\tableofcontents((* endblock tableofcontents *)) 18 | %((* endblock predoc *)) 19 | 20 | ((* block docclass *)) 21 | \documentclass[4pt]{book} 22 | \setcounter{chapter}{-1} 23 | ((* endblock docclass *)) 24 | 25 | ((* block preamble *)) 26 | \setcounter{secnumdepth}{3} 27 | \setcounter{tocdepth}{3} 28 | ((* endblock preamble *)) 29 | -------------------------------------------------------------------------------- /pdf/rm_notebook.py: -------------------------------------------------------------------------------- 1 | import os, sys, io 2 | 3 | print(sys.argv[1]) 4 | infile = open(sys.argv[1], encoding="utf8") 5 | data = infile.read() 6 | infile.close() 7 | outfile = open(sys.argv[1], "w", encoding="utf8") 8 | 9 | buf = io.StringIO(data) 10 | first_inline = False 11 | is_code_cell = False 12 | for line in buf: 13 | # can't comment out first inline that is always in the first cell 14 | if '"cell_type":' in line: 15 | is_code_cell = '"code"' in line 16 | 17 | if is_code_cell: 18 | if '%matplotlib inline' in line: 19 | if first_inline: 20 | line = line.replace("%matplotlib inline", "#%matplotlib inline") 21 | first_inline = True 22 | 23 | line = line.replace("%matplotlib notebook", "#%matplotlib notebook") 24 | line = line.replace("time.sleep", "#time.sleep") 25 | line = line.replace("fig.canvas.draw", "#fig.canvas.draw") 26 | line = line.replace("plt.gcf().canvas.draw", "#plt.gcf().canvas.draw") 27 | outfile.write(line) 28 | 29 | outfile.close() 30 | -------------------------------------------------------------------------------- /pdf/run_notebooks.bat: -------------------------------------------------------------------------------- 1 | mkdir tmp 2 | copy ..\*.ipynb .\tmp 3 | copy ..\*.py .\tmp 4 | cp -r ..\kf_book\ .\tmp\ 5 | 6 | cd tmp 7 | 8 | forfiles /m *.ipynb /c "cmd /c ipython ..\rm_notebook.py @file" 9 | jupyter nbconvert --allow-errors --inplace --execute *.ipynb 10 | -------------------------------------------------------------------------------- /pdf/short_build_book: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | echo "merging book..." 4 | 5 | ipython short_merge_book.py > short.ipynb 6 | 7 | echo "creating pdf..." 8 | ipython nbconvert --to PDF --template book short.ipynb 9 | 10 | echo "done." 11 | 12 | -------------------------------------------------------------------------------- /pdf/short_merge_book.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | import io 3 | import IPython.nbformat as nbformat 4 | import sys 5 | 6 | 7 | def remove_formatting(nb): 8 | c = nb['cells'] 9 | for i in range (len(c)): 10 | if 'source' in c[i].keys(): 11 | if c[i]['source'][0:16] == '#format the book': 12 | del c[i] 13 | return 14 | 15 | 16 | def remove_links(nb): 17 | c = nb['cells'] 18 | for i in range (len(c)): 19 | if 'source' in c[i].keys(): 20 | if c[i]['source'][0:19] == '[Table of Contents]': 21 | del c[i] 22 | return 23 | 24 | 25 | def remove_links_add_appendix(nb): 26 | c = nb['cells'] 27 | for i in range (len(c)): 28 | if 'source' in c[i].keys(): 29 | if c[i]['source'][0:19] == '[Table of Contents]': 30 | c[i]['source'] = '\\appendix' 31 | return 32 | 33 | 34 | def merge_notebooks(filenames): 35 | merged = None 36 | added_appendix = False 37 | for fname in filenames: 38 | with io.open(fname, 'r', encoding='utf-8') as f: 39 | nb = nbformat.read(f, nbformat.NO_CONVERT) 40 | remove_formatting(nb) 41 | if not added_appendix and fname[0:8] == 'Appendix': 42 | remove_links_add_appendix(nb) 43 | added_appendix = True 44 | else: 45 | remove_links(nb) 46 | if merged is None: 47 | merged = nb 48 | else: 49 | merged.cells.extend(nb.cells) 50 | #merged.metadata.name += "_merged" 51 | 52 | print(nbformat.writes(merged, nbformat.NO_CONVERT)) 53 | 54 | 55 | if __name__ == '__main__': 56 | #merge_notebooks(sys.argv[1:]) 57 | merge_notebooks( 58 | ['../00_Preface.ipynb', 59 | '../01_g-h_filter.ipynb', 60 | '../Appendix_A_Installation.ipynb']) 61 | -------------------------------------------------------------------------------- /pdf/to_pdf.py: -------------------------------------------------------------------------------- 1 | import nbconvert.exporters.pdf as pdf 2 | import sys 3 | 4 | if len(sys.argv) == 2: 5 | name = sys.argv[1] 6 | else: 7 | name = 'book.tex' 8 | 9 | f = open(name, 'r', encoding="iso-8859-1") 10 | filedata = f.read() 11 | f.close() 12 | 13 | newdata = filedata.replace('\chapter{Preface}', '\chapter*{Preface}') 14 | 15 | f = open(name, 'w', encoding="iso-8859-1") 16 | f.write(newdata) 17 | f.close() 18 | 19 | p = pdf.PDFExporter() 20 | p.run_latex(name) 21 | 22 | -------------------------------------------------------------------------------- /pdf/update_pdf.bat: -------------------------------------------------------------------------------- 1 | cd .. 2 | git checkout gh-pages 3 | git pull 4 | git checkout master Kalman_and_Bayesian_Filters_in_Python.pdf 5 | git checkout master README.md 6 | git add Kalman_and_Bayesian_Filters_in_Python.pdf 7 | git add README.md 8 | git commit -m "updating PDF" 9 | git push 10 | git checkout master 11 | cd pdf 12 | -------------------------------------------------------------------------------- /pdf/update_pdf.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | cd .. 3 | git checkout gh-pages 4 | git pull 5 | git checkout master Kalman_and_Bayesian_Filters_in_Python.pdf 6 | git checkout master README.md 7 | git add Kalman_and_Bayesian_Filters_in_Python.pdf 8 | git add README.md 9 | git commit -m 'updating PDF' 10 | git push 11 | git checkout master 12 | cd pdf -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | filterpy 2 | seaborn -------------------------------------------------------------------------------- /附录B-标记和符号.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "[Table of Contents](./00-A目录.ipynb)" 8 | ] 9 | }, 10 | { 11 | "cell_type": "markdown", 12 | "metadata": {}, 13 | "source": [ 14 | "# Symbols and Notations\n", 15 | "\n", 16 | "Here is a collection of the notation used by various authors for the linear Kalman filter equations." 17 | ] 18 | }, 19 | { 20 | "cell_type": "markdown", 21 | "metadata": {}, 22 | "source": [ 23 | "## Labbe\n", 24 | "\n", 25 | "\n", 26 | "$$\n", 27 | "\\begin{aligned}\n", 28 | "\\overline{\\mathbf x} &= \\mathbf{Fx} + \\mathbf{Bu} \\\\\n", 29 | "\\overline{\\mathbf P} &= \\mathbf{FPF}^\\mathsf{T} + \\mathbf Q \\\\ \\\\\n", 30 | "\\mathbf y &= \\mathbf z - \\mathbf{H}\\overline{\\mathbf x} \\\\\n", 31 | "\\mathbf S &= \\mathbf{H}\\overline{\\mathbf P}\\mathbf{H}^\\mathsf{T} + \\mathbf R \\\\\n", 32 | "\\mathbf K &= \\overline{\\mathbf P}\\mathbf{H}^\\mathsf{T}\\mathbf{S}^{-1} \\\\\n", 33 | "\\mathbf x &= \\overline{\\mathbf x} +\\mathbf{Ky} \\\\\n", 34 | "\\mathbf P &= (\\mathbf{I}-\\mathbf{KH})\\overline{\\mathbf P}\n", 35 | "\\end{aligned}$$\n", 36 | "\n", 37 | "\n", 38 | "## Wikipedia\n", 39 | "$$\n", 40 | "\\begin{aligned}\n", 41 | "\\hat{\\mathbf x}_{k\\mid k-1} &= \\mathbf{F}_{k}\\hat{\\mathbf x}_{k-1\\mid k-1} + \\mathbf{B}_{k} \\mathbf{u}_{k} \\\\\n", 42 | "\\mathbf P_{k\\mid k-1} &= \\mathbf{F}_{k} \\mathbf P_{k-1\\mid k-1} \\mathbf{F}_{k}^{\\textsf{T}} + \\mathbf Q_{k}\\\\\n", 43 | "\\tilde{\\mathbf{y}}_k &= \\mathbf{z}_k - \\mathbf{H}_k\\hat{\\mathbf x}_{k\\mid k-1} \\\\\n", 44 | "\\mathbf{S}_k &= \\mathbf{H}_k \\mathbf P_{k\\mid k-1} \\mathbf{H}_k^\\textsf{T} + \\mathbf{R}_k \\\\\n", 45 | "\\mathbf{K}_k &= \\mathbf P_{k\\mid k-1}\\mathbf{H}_k^\\textsf{T}\\mathbf{S}_k^{-1} \\\\\n", 46 | "\\hat{\\mathbf x}_{k\\mid k} &= \\hat{\\mathbf x}_{k\\mid k-1} + \\mathbf{K}_k\\tilde{\\mathbf{y}}_k \\\\\n", 47 | "\\mathbf P_{k|k} &= (I - \\mathbf{K}_k \\mathbf{H}_k) \\mathbf P_{k|k-1}\n", 48 | "\\end{aligned}$$\n", 49 | "\n", 50 | "## Brookner\n", 51 | "\n", 52 | "$$\n", 53 | "\\begin{aligned}\n", 54 | "X^*_{n+1,n} &= \\Phi X^*_{n,n} \\\\\n", 55 | "X^*_{n,n} &= X^*_{n,n-1} +H_n(Y_n - MX^*_{n,n-1}) \\\\\n", 56 | "H_n &= S^*_{n,n-1}M^\\mathsf{T}[R_n + MS^*_{n,n-1}M^\\mathsf{T}]^{-1} \\\\\n", 57 | "S^*_{n,n-1} &= \\Phi S^*_{n-1,n-1}\\Phi^\\mathsf{T} + Q_n \\\\\n", 58 | "S^*_{n-1,n-1} &= (I-H_{n-1}M)S^*_{n-1,n-2}\n", 59 | "\\end{aligned}$$\n", 60 | "\n", 61 | "## Gelb\n", 62 | "\n", 63 | "$$\n", 64 | "\\begin{aligned}\n", 65 | "\\underline{\\hat{x}}_k(-) &= \\Phi_{k-1} \\underline{\\hat{x}}_{k-1}(+) \\\\\n", 66 | "\\underline{\\hat{x}}_k(+) &= \\underline{\\hat{x}}_k(-) +K_k[Z_k - H_k\\underline{\\hat{x}}_k(-)] \\\\\n", 67 | "K_k &= P_k(-)H_k^\\mathsf{T}[H_kP_k(-)H_k^\\mathsf{T} + R_k]^{-1} \\\\\n", 68 | "P_k(+) &= \\Phi_{k-1} P_{k-1}(+)\\Phi_{k-1}^\\mathsf{T} + Q_{k-1} \\\\\n", 69 | "P_k(-) &= (I-K_kH_k)P_k(-)\n", 70 | "\\end{aligned}$$\n", 71 | "\n", 72 | "\n", 73 | "## Brown\n", 74 | "\n", 75 | "$$\n", 76 | "\\begin{aligned}\n", 77 | "\\hat{\\mathbf x}^-_{k+1} &= \\mathbf{\\phi}_{k}\\hat{\\mathbf x}_{k} \\\\\n", 78 | "\\hat{\\mathbf x}_k &= \\hat{\\mathbf x}^-_k +\\mathbf{K}_k[\\mathbf{z}_k - \\mathbf{H}_k\\hat{\\mathbf{}x}^-_k] \\\\\n", 79 | "\\mathbf{K}_k &= \\mathbf P^-_k\\mathbf{H}_k^\\mathsf{T}[\\mathbf{H}_k\\mathbf P^-_k\\mathbf{H}_k^T + \\mathbf{R}_k]^{-1}\\\\\n", 80 | "\\mathbf P^-_{k+1} &= \\mathbf{\\phi}_k \\mathbf P_k\\mathbf{\\phi}_k^\\mathsf{T} + \\mathbf Q_{k} \\\\\n", 81 | "\\mathbf P_k &= (\\mathbf{I}-\\mathbf{K}_k\\mathbf{H}_k)\\mathbf P^-_k\n", 82 | "\\end{aligned}$$\n", 83 | "\n", 84 | "\n", 85 | "## Zarchan\n", 86 | "\n", 87 | "$$\n", 88 | "\\begin{aligned}\n", 89 | "\\hat{x}_{k} &= \\Phi_{k}\\hat{x}_{k-1} + G_ku_{k-1} + K_k[z_k - H\\Phi_{k}\\hat{x}_{k-1} - HG_ku_{k-1} ] \\\\\n", 90 | "M_{k} &= \\Phi_k P_{k-1}\\phi_k^\\mathsf{T} + Q_{k} \\\\\n", 91 | "K_k &= M_kH^\\mathsf{T}[HM_kH^\\mathsf{T} + R_k]^{-1}\\\\\n", 92 | "P_k &= (I-K_kH)M_k\n", 93 | "\\end{aligned}$$" 94 | ] 95 | } 96 | ], 97 | "metadata": { 98 | "kernelspec": { 99 | "display_name": "Python 3", 100 | "language": "python", 101 | "name": "python3" 102 | }, 103 | "language_info": { 104 | "codemirror_mode": { 105 | "name": "ipython", 106 | "version": 3 107 | }, 108 | "file_extension": ".py", 109 | "mimetype": "text/x-python", 110 | "name": "python", 111 | "nbconvert_exporter": "python", 112 | "pygments_lexer": "ipython3", 113 | "version": "3.6.5" 114 | } 115 | }, 116 | "nbformat": 4, 117 | "nbformat_minor": 1 118 | } 119 | -------------------------------------------------------------------------------- /附录H-最小二乘滤波.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "[Table of Contents](./00-A目录.ipynb)" 8 | ] 9 | }, 10 | { 11 | "cell_type": "markdown", 12 | "metadata": {}, 13 | "source": [ 14 | "# Least Squares Filters" 15 | ] 16 | }, 17 | { 18 | "cell_type": "code", 19 | "execution_count": 1, 20 | "metadata": {}, 21 | "outputs": [], 22 | "source": [ 23 | "from __future__ import division, print_function\n", 24 | "%matplotlib inline" 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "execution_count": 2, 30 | "metadata": {}, 31 | "outputs": [ 32 | { 33 | "data": { 34 | "text/html": [ 35 | "\n", 36 | " \n", 46 | " " 47 | ], 48 | "text/plain": [ 49 | "" 50 | ] 51 | }, 52 | "execution_count": 2, 53 | "metadata": {}, 54 | "output_type": "execute_result" 55 | } 56 | ], 57 | "source": [ 58 | "#format the book\n", 59 | "import book_format\n", 60 | "book_format.set_style()" 61 | ] 62 | }, 63 | { 64 | "cell_type": "markdown", 65 | "metadata": {}, 66 | "source": [ 67 | "## Introduction" 68 | ] 69 | }, 70 | { 71 | "cell_type": "markdown", 72 | "metadata": {}, 73 | "source": [ 74 | "**author's note**: This was snipped from the g-h chapter, where it didn't belong. This chapter is not meant to be read yet! I haven't written it yet.\n", 75 | "\n", 76 | "Near the beginning of the chapter I used `numpy.polyfit()` to fit a straight line to the weight measurements. It fits a n-th degree polynomial to the data using a 'least squared fit'. How does this differ from the g-h filter?\n", 77 | "\n", 78 | "Well, it depends. We will eventually learn that the Kalman filter is optimal from a least squared fit perspective under certain conditions. However, `polyfit()` fits a polynomial to the data, not an arbitrary curve, by minimizing the value of this formula:\n", 79 | "\n", 80 | "$$E = \\sum_{j=0}^k |p(x_j) - y_j|^2$$\n", 81 | "\n", 82 | "I assumed that my weight gain was constant at 1 lb/day, and so when I tried to fit a polynomial of $n=1$, which is a line, the result very closely matched the actual weight gain. But, of course, no one consistently only gains or loses weight. We fluctuate. Using 'polyfit()' for a longer series of data would yield poor results. In contrast, the g-h filter reacts to changes in the rate - the $h$ term controls how quickly the filter reacts to these changes. If we gain weight, hold steady for awhile, then lose weight, the filter will track that change automatically. 'polyfit()' would not be able to do that unless the gain and loss could be well represented by a polynomial.\n", 83 | "\n", 84 | "Another advantage of this form of filter, even if the data fits a *n*-degree polynomial, is that it is *recursive*. That is, we can compute the estimate for this time period knowing nothing more than the estimate and rate from the last time period. In contrast, if you dig into the implementation for `polyfit()` you will see that it needs all of the data before it can produce an answer. Therefore algorithms like `polyfit()` are not well suited for real-time data filtering. In the 60's when the Kalman filter was developed computers were very slow and had extremely limited memory. They were utterly unable to store, for example, thousands of readings from an aircraft's inertial navigation system, nor could they process all of that data in the short period of time needed to provide accurate and up-to-date navigation information. \n", 85 | "\n", 86 | "\n", 87 | "Up until the mid 20th century various forms of Least Squares Estimation was used for this type of filtering. For example, for NASA's Apollo program had a ground network for tracking the Command and Service Model (CSM) and the Lunar Module (LM). They took measurements over many minutes, batched the data together, and slowly computed an answer. In 1960 Stanley Schmidt at NASA Ames recognized the utility of Rudolf Kalman's seminal paper and invited him to Ames. Schmidt applied Kalman's work to the on board navigation systems on the CSM and LM, and called it the \"Kalman filter\".[1] Soon after, the world moved to this faster, recursive filter.\n", 88 | "\n", 89 | "The Kalman filter only needs to store the last estimate and a few related parameters, and requires only a relatively small number of computations to generate the next estimate. Today we have so much memory and processing power that this advantage is somewhat less important, but at the time the Kalman filter was a major breakthrough not just because of the mathematical properties, but because it could (barely) run on the hardware of the day. \n", 90 | "\n", 91 | "This subject is much deeper than this short discussion suggests. We will consider these topics many more times throughout the book." 92 | ] 93 | } 94 | ], 95 | "metadata": { 96 | "anaconda-cloud": {}, 97 | "kernelspec": { 98 | "display_name": "Python 3", 99 | "language": "python", 100 | "name": "python3" 101 | }, 102 | "language_info": { 103 | "codemirror_mode": { 104 | "name": "ipython", 105 | "version": 3 106 | }, 107 | "file_extension": ".py", 108 | "mimetype": "text/x-python", 109 | "name": "python", 110 | "nbconvert_exporter": "python", 111 | "pygments_lexer": "ipython3", 112 | "version": "3.7.1" 113 | } 114 | }, 115 | "nbformat": 4, 116 | "nbformat_minor": 1 117 | } 118 | --------------------------------------------------------------------------------