├── Kalman Filter Positioning ├── Analyzing Kalman Filter.ipynb ├── Kalman Filter.ipynb ├── book_format.py ├── book_format.pyc ├── book_plots.py ├── book_plots.pyc ├── multivariate_ellipse.gif ├── multivariate_track1.gif ├── stats.py └── stats.pyc ├── Monte Carlo Positioning └── Monte Carlo Positioning.ipynb ├── Nonlieaner Filter Positioning ├── 538.json ├── Extend and Unsented Karman Filter.ipynb ├── Unscented Kalman Filter.ipynb ├── book_format.py ├── book_plots.py ├── custom3.css ├── spring_m.jpg └── stats.py ├── Particle Filter Positoning ├── Particle_Filter.ipynb ├── book_format.py ├── custom3.css └── floor.png └── README.md /Kalman Filter Positioning/book_format.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from IPython.core.display import HTML 3 | import matplotlib.pylab as pylab 4 | import matplotlib.pyplot as plt 5 | import json 6 | import numpy as np 7 | import sys 8 | from contextlib import contextmanager 9 | 10 | 11 | def equal_axis(): 12 | pylab.rcParams['figure.figsize'] = 10,10 13 | plt.axis('equal') 14 | 15 | def reset_axis(): 16 | pylab.rcParams['figure.figsize'] = 11, 5.5 17 | 18 | def set_figsize(x, y): 19 | pylab.rcParams['figure.figsize'] = x, y 20 | 21 | 22 | @contextmanager 23 | def figsize(x,y): 24 | """Temporarily set the figure size using 'with figsize(a,b):'""" 25 | 26 | set_figsize(x,y) 27 | yield 28 | reset_axis() 29 | 30 | @contextmanager 31 | def numpy_precision(precision): 32 | old = np.get_printoptions()['precision'] 33 | np.set_printoptions(precision=precision) 34 | yield 35 | np.set_printoptions(old) 36 | 37 | 38 | def _decode_list(data): 39 | rv = [] 40 | for item in data: 41 | if isinstance(item, unicode): 42 | item = item.encode('utf-8') 43 | elif isinstance(item, list): 44 | item = _decode_list(item) 45 | elif isinstance(item, dict): 46 | item = _decode_dict(item) 47 | rv.append(item) 48 | return rv 49 | 50 | def _decode_dict(data): 51 | rv = {} 52 | for key, value in data.iteritems(): 53 | if isinstance(key, unicode): 54 | key = key.encode('utf-8') 55 | if isinstance(value, unicode): 56 | value = value.encode('utf-8') 57 | elif isinstance(value, list): 58 | value = _decode_list(value) 59 | elif isinstance(value, dict): 60 | value = _decode_dict(value) 61 | rv[key] = value 62 | return rv 63 | 64 | 65 | def load_style(directory = '.', name='/custom2.css'): 66 | if sys.version_info[0] >= 3: 67 | s = json.load(open(directory + "/538.json")) 68 | else: 69 | s = json.load(open(directory + "/538.json"), object_hook=_decode_dict) 70 | plt.rcParams.update(s) 71 | reset_axis () 72 | np.set_printoptions(suppress=True) 73 | 74 | styles = open(directory + name, 'r').read() 75 | return HTML(styles) 76 | -------------------------------------------------------------------------------- /Kalman Filter Positioning/book_format.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robertdgithub/Multi-sensors_Fusion_Positioning/9fccb75bf8e503dafd2697f2550b06bb59f1e1b9/Kalman Filter Positioning/book_format.pyc -------------------------------------------------------------------------------- /Kalman Filter Positioning/book_plots.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Fri May 2 12:21:40 2014 4 | 5 | @author: rlabbe 6 | """ 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | 10 | def bar_plot(pos, ylim=(0,1), title=None): 11 | plt.cla() 12 | ax = plt.gca() 13 | x = np.arange(len(pos)) 14 | ax.bar(x, pos, color='#30a2da') 15 | if ylim: 16 | plt.ylim(ylim) 17 | plt.xticks(x+0.4, x) 18 | if title is not None: 19 | plt.title(title) 20 | 21 | 22 | def plot_measurements(xs, ys=None, c='r', lw=2, label='Measurements', **kwargs): 23 | """ Helper function to give a consistant way to display 24 | measurements in the book. 25 | """ 26 | 27 | plt.autoscale(tight=True) 28 | '''if ys is not None: 29 | plt.scatter(xs, ys, marker=marker, c=c, s=s, 30 | label=label, alpha=alpha) 31 | if connect: 32 | plt.plot(xs, ys, c=c, lw=1, alpha=alpha) 33 | else: 34 | plt.scatter(range(len(xs)), xs, marker=marker, c=c, s=s, 35 | label=label, alpha=alpha) 36 | if connect: 37 | plt.plot(range(len(xs)), xs, lw=1, c=c, alpha=alpha)''' 38 | 39 | if ys is not None: 40 | plt.plot(xs, ys, c=c, lw=lw, linestyle='--', label=label, **kwargs) 41 | else: 42 | plt.plot(xs, c=c, lw=lw, linestyle='--', label=label, **kwargs) 43 | 44 | 45 | 46 | def plot_residual_limits(Ps): 47 | std = np.sqrt(Ps) 48 | 49 | plt.plot(-std, c='k', ls=':', lw=2) 50 | plt.plot(std, c='k', ls=':', lw=2) 51 | plt.fill_between(range(len(std)), -std, std, 52 | facecolor='#ffff00', alpha=0.3) 53 | 54 | 55 | def plot_track(xs, ys=None, label='Track', c='k', lw=2): 56 | if ys is not None: 57 | plt.plot(xs, ys, c=c, lw=lw, label=label) 58 | else: 59 | plt.plot(xs, c=c, lw=lw, label=label) 60 | 61 | 62 | #c='#013afe' 63 | def plot_filter(xs, ys=None, c='#6d904f', label='Filter', **kwargs): 64 | if ys is not None: 65 | plt.plot(xs, ys, c=c, label=label, **kwargs) 66 | else: 67 | plt.plot(xs, c=c, label=label, **kwargs) 68 | 69 | 70 | if __name__ == "__main__": 71 | p = [0.2245871, 0.06288015, 0.06109133, 0.0581008, 0.09334062, 0.2245871, 72 | 0.06288015, 0.06109133, 0.0581008, 0.09334062]*2 73 | bar_plot(p) 74 | plot_measurements(p) -------------------------------------------------------------------------------- /Kalman Filter Positioning/book_plots.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robertdgithub/Multi-sensors_Fusion_Positioning/9fccb75bf8e503dafd2697f2550b06bb59f1e1b9/Kalman Filter Positioning/book_plots.pyc -------------------------------------------------------------------------------- /Kalman Filter Positioning/multivariate_ellipse.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robertdgithub/Multi-sensors_Fusion_Positioning/9fccb75bf8e503dafd2697f2550b06bb59f1e1b9/Kalman Filter Positioning/multivariate_ellipse.gif -------------------------------------------------------------------------------- /Kalman Filter Positioning/multivariate_track1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robertdgithub/Multi-sensors_Fusion_Positioning/9fccb75bf8e503dafd2697f2550b06bb59f1e1b9/Kalman Filter Positioning/multivariate_track1.gif -------------------------------------------------------------------------------- /Kalman Filter Positioning/stats.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Roger Labbe 3 | Copyright: 2014 4 | 5 | This code performs various basic statistics functions for the 6 | Kalman and Bayesian Filters in Python book. Much of this code 7 | is non-optimal; production code should call the equivalent scipy.stats 8 | functions. I wrote the code in this form to make explicit how the 9 | computations are done. The scipy.stats module has many more useful functions 10 | than what I have written here. In some cases, however, my code is significantly 11 | faster, at least on my machine. For example, gaussian average 794 ns, whereas 12 | stats.norm(), using the frozen form, averages 116 microseconds per call. 13 | """ 14 | 15 | from __future__ import (absolute_import, division, print_function, 16 | unicode_literals) 17 | 18 | import math 19 | import numpy as np 20 | import numpy.linalg as linalg 21 | import matplotlib.pyplot as plt 22 | import scipy.sparse as sp 23 | import scipy.sparse.linalg as spln 24 | import scipy.stats 25 | from scipy.stats import norm 26 | from matplotlib.patches import Ellipse 27 | 28 | _two_pi = 2*math.pi 29 | 30 | 31 | def gaussian(x, mean, var): 32 | """returns normal distribution (pdf) for x given a Gaussian with the 33 | specified mean and variance. x can either be a scalar or an 34 | array-like. 35 | 36 | gaussian (1,2,3) is equivalent to scipy.stats.norm(2,math.sqrt(3)).pdf(1) 37 | It is quite a bit faster albeit much less flexible than the latter. 38 | 39 | @param x test 40 | 41 | Parameters 42 | ---------- 43 | x : scalar or array-like 44 | The value for which we compute the probability 45 | 46 | mean : scalar 47 | Mean of the Gaussian 48 | 49 | var : scalar 50 | Variance of the Gaussian 51 | 52 | Returns 53 | ------- 54 | probability : float, or array-like 55 | probability of x for the Gaussian (mean, var). E.g. 0.101 denotes 56 | 10.1%. 57 | 58 | Examples 59 | -------- 60 | gaussian(3, 1, 2) 61 | gaussian([3,4,3,2,1], 1, 2) 62 | """ 63 | 64 | return (np.exp((-0.5*(np.asarray(x)-mean)**2)/var) / 65 | np.sqrt(_two_pi*var)) 66 | 67 | 68 | def mul (mean1, var1, mean2, var2): 69 | """ multiply Gaussians (mean1, var1) with (mean2, var2) and return the 70 | results as a tuple (mean,var). 71 | 72 | var1 and var2 are variances - sigma squared in the usual parlance. 73 | """ 74 | 75 | mean = (var1*mean2 + var2*mean1) / (var1 + var2) 76 | var = 1 / (1/var1 + 1/var2) 77 | return (mean, var) 78 | 79 | 80 | def add (mean1, var1, mean2, var2): 81 | """ add the Gaussians (mean1, var1) with (mean2, var2) and return the 82 | results as a tuple (mean,var). 83 | 84 | var1 and var2 are variances - sigma squared in the usual parlance. 85 | """ 86 | 87 | return (mean1+mean2, var1+var2) 88 | 89 | 90 | def multivariate_gaussian(x, mu, cov): 91 | """ This is designed to replace scipy.stats.multivariate_normal 92 | which is not available before version 0.14. You may either pass in a 93 | multivariate set of data: 94 | 95 | multivariate_gaussian (array([1,1]), array([3,4]), eye(2)*1.4) 96 | multivariate_gaussian (array([1,1,1]), array([3,4,5]), 1.4) 97 | 98 | or unidimensional data: 99 | multivariate_gaussian(1, 3, 1.4) 100 | 101 | In the multivariate case if cov is a scalar it is interpreted as eye(n)*cov 102 | 103 | The function gaussian() implements the 1D (univariate)case, and is much 104 | faster than this function. 105 | 106 | equivalent calls: 107 | multivariate_gaussian(1, 2, 3) 108 | scipy.stats.multivariate_normal(2,3).pdf(1) 109 | 110 | 111 | 112 | Parameters 113 | ---------- 114 | x : scalar, or np.array-like 115 | Value to compute the probability for. May be a scalar if univariate, 116 | or any type that can be converted to an np.array (list, tuple, etc). 117 | np.array is best for speed. 118 | 119 | mu : scalar, or np.array-like 120 | mean for the Gaussian . May be a scalar if univariate, or any type 121 | that can be converted to an np.array (list, tuple, etc).np.array is 122 | best for speed. 123 | 124 | cov : scalar, or np.array-like 125 | Covariance for the Gaussian . May be a scalar if univariate, or any 126 | type that can be converted to an np.array (list, tuple, etc).np.array is 127 | best for speed. 128 | 129 | 130 | 131 | Returns 132 | ------- 133 | probability : float 134 | probability for x for the Gaussian (mu,cov) 135 | 136 | 137 | """ 138 | scipy.stats.multivariate_normal 139 | # force all to numpy.array type 140 | x = np.array(x, copy=False, ndmin=1) 141 | mu = np.array(mu,copy=False, ndmin=1) 142 | 143 | nx = len(mu) 144 | cov = _to_cov(cov, nx) 145 | 146 | norm_coeff = nx*math.log(2*math.pi) + np.linalg.slogdet(cov)[1] 147 | 148 | err = x - mu 149 | if (sp.issparse(cov)): 150 | numerator = spln.spsolve(cov, err).T.dot(err) 151 | else: 152 | numerator = np.linalg.solve(cov, err).T.dot(err) 153 | 154 | return math.exp(-0.5*(norm_coeff + numerator)) 155 | 156 | 157 | def plot_gaussian(mean, variance, 158 | mean_line=False, 159 | xlim=None, 160 | xlabel=None, 161 | ylabel=None): 162 | """ plots the normal distribution with the given mean and variance. x-axis 163 | contains the mean, the y-axis shows the probability. 164 | 165 | mean_line : draws a line at x=mean 166 | xlim: optionally specify the limits for the x axis as tuple (low,high). 167 | If not specified, limits will be automatically chosen to be 'nice' 168 | xlabel : optional label for the x-axis 169 | ylabel : optional label for the y-axis 170 | """ 171 | 172 | sigma = math.sqrt(variance) 173 | n = scipy.stats.norm(mean, sigma) 174 | 175 | if xlim is None: 176 | min_x = n.ppf(0.001) 177 | max_x = n.ppf(0.999) 178 | else: 179 | min_x = xlim[0] 180 | max_x = xlim[1] 181 | xs = np.arange(min_x, max_x, (max_x - min_x) / 1000) 182 | plt.plot(xs,n.pdf(xs)) 183 | plt.xlim((min_x, max_x)) 184 | 185 | if mean_line: 186 | plt.axvline(mean) 187 | if xlabel: 188 | plt.xlabel(xlabel) 189 | if ylabel: 190 | plt.ylabel(ylabel) 191 | 192 | 193 | def covariance_ellipse(P, deviations=1): 194 | """ returns a tuple defining the ellipse representing the 2 dimensional 195 | covariance matrix P. 196 | 197 | Parameters 198 | ---------- 199 | P : nd.array shape (2,2) 200 | covariance matrix 201 | 202 | deviations : int (optional, default = 1) 203 | # of standard deviations. Default is 1. 204 | 205 | Returns (angle_radians, width_radius, height_radius) 206 | """ 207 | U,s,v = linalg.svd(P) 208 | orientation = math.atan2(U[1,0],U[0,0]) 209 | width = deviations*math.sqrt(s[0]) 210 | height = deviations*math.sqrt(s[1]) 211 | 212 | assert width >= height 213 | 214 | return (orientation, width, height) 215 | 216 | 217 | def is_inside_ellipse(x,y, ex, ey, orientation, width, height): 218 | 219 | co = np.cos(orientation) 220 | so = np.sin(orientation) 221 | 222 | xx = x*co + y*so 223 | yy = y*co - x*so 224 | 225 | return (xx / width)**2 + (yy / height)**2 <= 1. 226 | 227 | 228 | return ((x-ex)*co - (y-ey)*so)**2/width**2 + \ 229 | ((x-ex)*so + (y-ey)*co)**2/height**2 <= 1 230 | 231 | 232 | 233 | def plot_covariance_ellipse(mean, cov=None, variance = 1.0, 234 | ellipse=None, title=None, axis_equal=True, 235 | facecolor='none', edgecolor='#004080', 236 | alpha=1.0, xlim=None, ylim=None): 237 | """ plots the covariance ellipse where 238 | 239 | mean is a (x,y) tuple for the mean of the covariance (center of ellipse) 240 | 241 | cov is a 2x2 covariance matrix. 242 | 243 | variance is the normal sigma^2 that we want to plot. If list-like, 244 | ellipses for all ellipses will be ploted. E.g. [1,2] will plot the 245 | sigma^2 = 1 and sigma^2 = 2 ellipses. 246 | 247 | ellipse is a (angle,width,height) tuple containing the angle in radians, 248 | and width and height radii. 249 | 250 | You may provide either cov or ellipse, but not both. 251 | 252 | plt.show() is not called, allowing you to plot multiple things on the 253 | same figure. 254 | """ 255 | 256 | assert cov is None or ellipse is None 257 | assert not (cov is None and ellipse is None) 258 | 259 | if cov is not None: 260 | ellipse = covariance_ellipse(cov) 261 | 262 | if axis_equal: 263 | plt.axis('equal') 264 | 265 | if title is not None: 266 | plt.title (title) 267 | 268 | 269 | if np.isscalar(variance): 270 | variance = [variance] 271 | 272 | ax = plt.gca() 273 | 274 | angle = np.degrees(ellipse[0]) 275 | width = ellipse[1] * 2. 276 | height = ellipse[2] * 2. 277 | 278 | for var in variance: 279 | sd = np.sqrt(var) 280 | e = Ellipse(xy=mean, width=sd*width, height=sd*height, angle=angle, 281 | facecolor=facecolor, 282 | edgecolor=edgecolor, 283 | alpha=alpha, 284 | lw=2) 285 | ax.add_patch(e) 286 | plt.scatter(mean[0], mean[1], marker='+') # mark the center 287 | plt.grid() 288 | if xlim is not None: 289 | ax.set_xlim(xlim) 290 | 291 | if ylim is not None: 292 | ax.set_ylim(ylim) 293 | 294 | 295 | def _to_cov(x,n): 296 | """ If x is a scalar, returns a covariance matrix generated from it 297 | as the identity matrix multiplied by x. The dimension will be nxn. 298 | If x is already a numpy array then it is returned unchanged. 299 | """ 300 | try: 301 | x.shape 302 | if type(x) != np.ndarray: 303 | x = np.asarray(x)[0] 304 | return x 305 | except: 306 | return np.eye(n) * x 307 | 308 | 309 | 310 | def do_plot_test(): 311 | 312 | from numpy.random import multivariate_normal 313 | p = np.array([[32, 15],[15., 40.]]) 314 | 315 | x,y = multivariate_normal(mean=(0,0), cov=p, size=5000).T 316 | sd = 2 317 | a,w,h = covariance_ellipse(p,sd) 318 | print (np.degrees(a), w, h) 319 | 320 | count = 0 321 | color=[] 322 | for i in range(len(x)): 323 | if is_inside_ellipse(x[i], y[i], 0, 0, a, w, h): 324 | color.append('b') 325 | count += 1 326 | else: 327 | color.append('r') 328 | plt.scatter(x,y,alpha=0.2, c=color) 329 | 330 | 331 | plt.axis('equal') 332 | 333 | plot_covariance_ellipse(mean=(0., 0.), 334 | cov = p, 335 | variance=sd*sd, 336 | facecolor='none') 337 | 338 | print (count / len(x)) 339 | 340 | 341 | from numpy.linalg import inv 342 | from numpy import asarray, dot 343 | 344 | 345 | def multivariate_multiply(m1, c1, m2, c2): 346 | 347 | C1 = asarray(c1) 348 | C2 = asarray(c2) 349 | M1 = asarray(m1) 350 | M2 = asarray(m2) 351 | 352 | sum_inv = inv(C1+C2) 353 | C3 = dot(C1, sum_inv).dot(C2) 354 | 355 | M3 = (dot(C2, sum_inv).dot(M1) + 356 | dot(C1, sum_inv).dot(M2)) 357 | 358 | return M3, C3 359 | 360 | def norm_cdf (x_range, mu, var=1, std=None): 361 | """ computes the probability that a Gaussian distribution lies 362 | within a range of values. 363 | 364 | Paramateters 365 | ------------ 366 | 367 | x_range : (float, float) 368 | tuple of range to compute probability for 369 | 370 | mu : float 371 | mean of the Gaussian 372 | 373 | var : float, optional 374 | variance of the Gaussian. Ignored if std is provided 375 | 376 | std : float, optional 377 | standard deviation of the Gaussian. This overrides the var parameter 378 | 379 | 380 | Returns 381 | ------- 382 | probability : float 383 | probability that Gaussian is within x_range. E.g. .1 means 10%. 384 | 385 | """ 386 | 387 | if std is None: 388 | std = math.sqrt(var) 389 | return abs(norm.cdf(x_range[0], loc=mu, scale=std) - 390 | norm.cdf(x_range[1], loc=mu, scale=std)) 391 | 392 | 393 | 394 | def test_norm_cdf(): 395 | 396 | # test using the 68-95-99.7 rule 397 | 398 | mu = 5 399 | std = 3 400 | var = std*std 401 | 402 | std_1 = (norm_cdf((mu-std, mu+std), mu, var)) 403 | assert abs(std_1 - .6827) < .0001 404 | 405 | std_1 = (norm_cdf((mu+std, mu-std), mu, std=std)) 406 | assert abs(std_1 - .6827) < .0001 407 | 408 | std_1half = (norm_cdf((mu+std, mu), mu, var)) 409 | assert abs(std_1half - .6827/2) < .0001 410 | 411 | std_2 = (norm_cdf((mu-2*std, mu+2*std), mu, var)) 412 | assert abs(std_2 - .9545) < .0001 413 | 414 | std_3 = (norm_cdf((mu-3*std, mu+3*std), mu, var)) 415 | assert abs(std_3 - .9973) < .0001 416 | 417 | 418 | if __name__ == '__main__': 419 | test_norm_cdf () 420 | 421 | do_plot_test() 422 | 423 | #test_gaussian() 424 | 425 | # test conversion of scalar to covariance matrix 426 | x = multivariate_gaussian(np.array([1,1]), np.array([3,4]), np.eye(2)*1.4) 427 | x2 = multivariate_gaussian(np.array([1,1]), np.array([3,4]), 1.4) 428 | assert x == x2 429 | 430 | # test univarate case 431 | rv = norm(loc = 1., scale = np.sqrt(2.3)) 432 | x2 = multivariate_gaussian(1.2, 1., 2.3) 433 | x3 = gaussian(1.2, 1., 2.3) 434 | 435 | assert rv.pdf(1.2) == x2 436 | assert abs(x2- x3) < 0.00000001 437 | 438 | cov = np.array([[1.0, 1.0], 439 | [1.0, 1.1]]) 440 | 441 | plt.figure() 442 | P = np.array([[2,0],[0,2]]) 443 | plot_covariance_ellipse((2,7), cov=cov, variance=[1,2], facecolor='g', title='my title', alpha=.2) 444 | plt.show() 445 | 446 | print("all tests passed") 447 | -------------------------------------------------------------------------------- /Kalman Filter Positioning/stats.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robertdgithub/Multi-sensors_Fusion_Positioning/9fccb75bf8e503dafd2697f2550b06bb59f1e1b9/Kalman Filter Positioning/stats.pyc -------------------------------------------------------------------------------- /Monte Carlo Positioning/Monte Carlo Positioning.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Monte Carlo Positioning" 8 | ] 9 | }, 10 | { 11 | "cell_type": "markdown", 12 | "metadata": {}, 13 | "source": [ 14 | "We will learn how to program vehicles with the ability to sense their own location, and the locations of other vehicles, using sensors and algorithms." 15 | ] 16 | }, 17 | { 18 | "cell_type": "markdown", 19 | "metadata": {}, 20 | "source": [ 21 | "In this lecture we will learn to sense locations with Monte Carlo positioning." 22 | ] 23 | }, 24 | { 25 | "cell_type": "markdown", 26 | "metadata": {}, 27 | "source": [ 28 | "![http://www.fix.com/blog/driverless-car-revolution/](driverless-car.png \"Driverless Car\")\n" 29 | ] 30 | }, 31 | { 32 | "cell_type": "code", 33 | "execution_count": 3, 34 | "metadata": { 35 | "collapsed": true 36 | }, 37 | "outputs": [], 38 | "source": [ 39 | "%matplotlib inline " 40 | ] 41 | }, 42 | { 43 | "cell_type": "code", 44 | "execution_count": 4, 45 | "metadata": { 46 | "collapsed": false, 47 | "input_collapsed": false 48 | }, 49 | "outputs": [], 50 | "source": [ 51 | "import numpy as np\n", 52 | "import scipy as sp\n", 53 | "import matplotlib.pyplot as plt\n", 54 | "\n", 55 | "def show_world(ax, lw=5):\n", 56 | " #show a world with lw cells\n", 57 | " \n", 58 | " ax.bar(np.arange(lw), np.ones(lw), 1, linewidth=3)\n", 59 | " ax.spines['right'].set_color('none')\n", 60 | " ax.spines['left'].set_color('none')\n", 61 | " ax.spines['top'].set_color('none')\n", 62 | " ax.xaxis.set_ticks_position('bottom')\n", 63 | " ax.set_xticks(np.arange(lw)+0.5)\n", 64 | " #ax.set_xlim([-0.1, 5.1])\n", 65 | " #ax.set_ylim([0, 110])\n", 66 | " ax.set_xticklabels(np.arange(lw)+1)\n", 67 | " plt.yticks([])\n", 68 | "\n", 69 | "def showbar(ax, center=range(1,6,1), height=0.5, bottom=0.25, color='red'):\n", 70 | " #show markers on the positions (center)\n", 71 | " \n", 72 | " ax.bar(np.array(center)-1+0.1, height, bottom=bottom, color=color, linewidth=3)" 73 | ] 74 | }, 75 | { 76 | "cell_type": "markdown", 77 | "metadata": { 78 | "input_collapsed": false 79 | }, 80 | "source": [ 81 | "## Discrete 1D World" 82 | ] 83 | }, 84 | { 85 | "cell_type": "markdown", 86 | "metadata": {}, 87 | "source": [ 88 | "Rather than start from a vehicle in continuous 3D world, we are going to consider a dicrete 1D world firstly.\n", 89 | "\n", 90 | "Image a train on the track, where it is only possible to move forward or backwards; sideways motion is impossible. The measurement is so rough that there are only 5 positions. If there are more accurate measurement methods, we may divide the track into 500 intervals." 91 | ] 92 | }, 93 | { 94 | "cell_type": "code", 95 | "execution_count": 5, 96 | "metadata": { 97 | "collapsed": false, 98 | "input_collapsed": false 99 | }, 100 | "outputs": [ 101 | { 102 | "data": { 103 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAABExJREFUeJzt3M+rpQMcx/HP/CAGkShEjaWFomShmWaiJEn+BQtbC2Xh\nL7CclZ0sRLMSkZ1yImU0mWkwsqIUJkVKysZYPGdhM3Nn8el+z3Fer805de6tT92ne9/nOc99EgAA\nAAAAAAAAAAAAAAAAADbXgT1ev7wvKwAA2BZX7MeD+7kCAID/r8PX9mVOXO6u/74pcRzsJscAieOA\nheOAq3/Y7YwlAAAVwhIAgAphCQBAhbAEAKBCWAIAUCEsAQCoEJYAAFQISwAAKoQlAAAVwhIAgAph\nCQBAhbAEAKBCWAIAUCEsAQCoEJYAAFQISwAAKoQlAAAVwhIAgAphCQBAhbAEAKBCWAIAUCEsAQCo\nEJYAAFQISwAAKoQlAAAVwhIAgAphCQBAhbAEAKBCWAIAUCEsAQCoEJYAAFQISwAAKoQlAAAVwhIA\ngAphCQBAhbAEAKBCWAIAUCEsAQCoEJYAAFQISwAAKoQlAAAVwhIAgAphCQBAhbAEAKBCWAIAUCEs\nAQCoEJYAAFQISwAAKoQlAAAVwhIAgAphCQBAhbAEAKBCWAIAUCEsAQCoEJYAAFQISwAAKoQlAAAV\nwhIAgAphCQBAhbAEAKBCWAIAUCEsAQCoEJYAAFQISwAAKoQlAAAVwhIAgAphCQBAhbAEAKBCWAIA\nUCEsAQCoEJYAAFQISwAAKoQlAAAVwhIAgAphCQBAhbAEAKBCWAIAUCEsAQCoEJYAAFQISwAAKoTl\nnlbTA9gIq+kBjFtND2AjrKYHMG41PWCjCcs9raYHsBFW0wMYt5oewEZYTQ9g3Gp6wEYTlgAAVBzY\n4/XL+7ICAIBtccV+PHQN33iyOgUAgG12IK4JAAAAAAAAAAAAAKDgjSSXknw1PYQx9yX5OMk3Sb5O\n8uLsHIbckORMkvNJLiZ5dXYOgw4lOZfkg+khjPkhyYUsx8EXs1PYNseTPBxhucvuSvLQ+vnNSb5L\n8sDcHAYdWT8eTvJ5kmODW5jzUpK3k7w/PYQx3ye5fXrEJnOD9Cv7NMnv0yMY9UuWs1RJ8meSb5Pc\nMzeHQX+tH6/Pctbqt8EtzLg3ydNJXs/e94Dm/83P/yqEJVybo1nOYJ8Z3sGMg1neZFzKcnnExdk5\nDDiV5OUk/0wPYdTlJB8lOZvkheEtbKGj8VE4y8fgZ5M8Nz2Ecbdm+Sj85PAO9tczSV5bPz8Z11ju\nsrvXj3dmebN5fHDLRnLGEq7uuiTvJHkryXvDW5j3R5IPkzwyPYR99ViSZ7NcX3c6yeNJ3hxdxJSf\n14+/Jnk3yaODW9hCR+OM5S47kOWPx6npIYy6I8lt6+c3JvkkyRNzcxh2Is5Y7qojSW5ZP78pyWdJ\nnpybw7Y5neSnJH8n+THJ87NzGHAsy/VU57PcWuJckqdGFzHhwSRfZjkOLmS5zo7ddSL+K3xX3Z/l\n98D5LLege2V2DgAAAAAAAAAAAAAAAAAAAAAAAAAAbK9/AdXhWVuWC0PgAAAAAElFTkSuQmCC\n", 104 | "text/plain": [ 105 | "" 106 | ] 107 | }, 108 | "metadata": {}, 109 | "output_type": "display_data" 110 | } 111 | ], 112 | "source": [ 113 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 114 | "show_world(ax)" 115 | ] 116 | }, 117 | { 118 | "cell_type": "markdown", 119 | "metadata": {}, 120 | "source": [ 121 | "## Deterministic World" 122 | ] 123 | }, 124 | { 125 | "cell_type": "markdown", 126 | "metadata": {}, 127 | "source": [ 128 | "The world is also deterministic, which means that the train is at one of the positions with the probability of 100%." 129 | ] 130 | }, 131 | { 132 | "cell_type": "markdown", 133 | "metadata": {}, 134 | "source": [ 135 | "Let us try to locate it. Assume for some reason we know it starts from cell 1, but the measurement tells us that it is at cell 5. So where is it on earth?\n", 136 | "* Prior: cell 1 (in red)\n", 137 | "* Measurement: cell 5 (in green)\n", 138 | "* Posterior: ?" 139 | ] 140 | }, 141 | { 142 | "cell_type": "code", 143 | "execution_count": 6, 144 | "metadata": { 145 | "collapsed": false, 146 | "input_collapsed": false 147 | }, 148 | "outputs": [ 149 | { 150 | "data": { 151 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAABQ1JREFUeJzt3LGLHHUYx+HvJlE03qGIgorCWVoICmIhCTkURETEf8HC\n1kKwsLvOSlLZiYUoqcSg2AkuimAkmCNqxEpBUIOgiEGw8SxmiwS87OXyuu/s7fPAMgt7t7wwv7v9\n7MzsJgAAAAAAAAAAAAAAAAAAAIzXZM7jOwuZAgCAZbFrPx5a5BQAABxcR/b2Yw5crq7L35RYB6vJ\nGiCxDhhYB1z9ZLcjlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQ\nlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQoDsvXkqwnmbiN9rY+\n208AsAjaYNy32i6YzHl854rNXOtJLl3HOCzGWpI/9/izly+Rva4DDhZrgMQ6YLCfdaANxu+au2DX\nfiwOy3lPx3jsZ596MVlN1gCJdcBgP+tAGyyHa9qfu+7UIyWz/Af/dsbHnzYArba6B+AKW/VP6cM7\nAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQlgAA\nlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQ\nlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYAAJQQlgAAlBCWAACUEJYA\nAJQQlgAAlBCWAACUEJYAAJQQlgAAlJjMeXznis11Px3jsZ99utff4WCxBkisAwb7WQfaYDlc0/7c\ndacWH7Fcq306/if2EwCL4jVn/Or2UXFYbsUCGru1DPsJABZhK9pgzGq7oPhUOAeP019YAyTWAQPr\ngIWeCgcAYFUJSwAASghLAABKCEsAAEoISwAASghLAABKCEsAAEoISwAASghLAABKCEsAAEoISwAA\nSghLAABKCEsAAEoISwAASghLAABKCMu5pt0DMArT7gFoN+0egFGYdg9Au2n3AKMmLOeadg/AKEy7\nB6DdtHsARmHaPQDtpt0DjJqwBACgxGTO4zsLmQIAgGWxaz8e3sMvbpaOAgDAMpvENQEAAAAAAAAA\nAAAAFHgzycUkX3UPQpv7knyc5JskXyd5sXccmtyU5EyS7SQXkrzaOw6NDic5l+SD7kFo80OS8xnW\nwRe9o7Bsjid5OMJyld2V5KHZ/bUk3yV5oG8cGh2dbY8k+TzJscZZ6PNSkneSvN89CG2+T3J79xBj\n5gvSd/dpkt+7h6DVLxmOUiXJpSTfJrmnbxwa/TXb3pjhqNVvjbPQ494kTyd5I/O/A5qDzf6/CmEJ\ne7OR4Qj2meY56HEow5uMixkuj7jQOw4NTiZ5Ock/3YPQaifJR0nOJnmheRaW0EacCmc4DX42yXPd\ng9Du1gynwjeb52Cxnkny+uz+Zlxjucrunm3vzPBm83jjLKPkiCVc3Q1J3k3ydpLTzbPQ748kHyZ5\npHsQFuqxJM9muL7uVJLHk7zVOhFdfp5tf03yXpJHG2dhCW3EEctVNsnw4nGyexBa3ZHkttn9m5N8\nkuSJvnFodiKOWK6qo0nWZ/dvSfJZkif7xmHZnEryU5K/k/yY5PnecWhwLMP1VNsZvlriXJKnWiei\nw4NJvsywDs5nuM6O1XUiPhW+qu7P8H9gO8NX0L3SOw4AAAAAAAAAAAAAAAAAAAAAAAAAACyvfwEx\nN4DuKz4B1QAAAABJRU5ErkJggg==\n", 152 | "text/plain": [ 153 | "" 154 | ] 155 | }, 156 | "metadata": {}, 157 | "output_type": "display_data" 158 | } 159 | ], 160 | "source": [ 161 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 162 | "show_world(ax)\n", 163 | "# Prior position: cell 1 (in red)\n", 164 | "showbar(ax, 1, color='red')\n", 165 | "# Measurement: cell 5 (in green)\n", 166 | "showbar(ax, 5, color='green')" 167 | ] 168 | }, 169 | { 170 | "cell_type": "markdown", 171 | "metadata": {}, 172 | "source": [ 173 | "## Posterior position? " 174 | ] 175 | }, 176 | { 177 | "cell_type": "code", 178 | "execution_count": 7, 179 | "metadata": { 180 | "collapsed": false, 181 | "input_collapsed": false 182 | }, 183 | "outputs": [ 184 | { 185 | "data": { 186 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAABS9JREFUeJzt3DGLXGUUBuB3kigad1FEQUUhlhaCglhIQoKCiIj4Fyxs\nLQQLu+2sJJWdWIiSShTFTnBQBCPBhKgRKwVBDYIiBsHGtbhTJOBmdjcnc+7MPA8Md2B2hxe+b8++\ne+/dSQAAAAAAAAAAAAAAAAAAABivyZzXtxeSAgCAZbFjfzywyBQAAKyuQ7v7Micu19flf5TYB+vJ\nHiCxDxjYB1z9YrczlgAAlFAsAQAooVgCAFBCsQQAoIRiCQBACcUSAIASiiUAACUUSwAASiiWAACU\nUCwBACihWAIAUEKxBACghGIJAEAJxRIAgBKKJQAAJRRLAABKKJYAAJRQLAEAKFFcLF9Lsplk4jHa\nx+ZsneB6MgvG/zALWBTzYNyP2lkwmfP69hWHuTaTXLqGOCzGRpK/dvm1l2+R3e4DVst+9oBZsBzM\nAvbKPFhNe54FO/bH4mI57+0Yj/2sqV8m62k/e8AsWB5mAXthHqyuPa3njot6qCTL/zB2xsePNh22\nDYPRmRgGdNnqDsAVturf0j/vAABQQrEEAKCEYgkAQAnFEgCAEoolAAAlFEsAAEoolgAAlFAsAQAo\noVgCAFBCsQQAoIRiCQBACcUSAIASiiUAACUUSwAASiiWAACUUCwBACihWAIAUEKxBACghGIJAEAJ\nxRIAgBKKJQAAJRRLAABKKJYAAJRQLAEAKKFYAgBQQrEEAKCEYgkAQAnFEgCAEoolAAAlFEsAAEoo\nlgAAlFAsAQAooVgCAFBCsQQAoIRiCQBACcUSAIASiiUAACUUSwAASiiWAACUmMx5ffuKwzW/HeOx\nnzXd7fewWvazB8yC5WEWsBfmwera03ruuKjFZyw3at+O68Q6cb3ZY8vBOrEI9tn41a1RcbHcig00\ndhsZ1gmup62YBWNnFrAoWzEPxqx2FhRfCmf1uPyFPUBiHzCwD1jopXAAANaVYgkAQAnFEgCAEool\nAAAlFEsAAEoolgAAlFAsAQAooVgCAFBCsQQAoIRiCQBACcUSAIASiiUAACUUSwAASiiWAACUUCwB\nACihWM417Q7AKEy7A9Bu2h2AUZh2B6DdtDvAqCmWc027AzAK0+4AtJt2B2AUpt0BaDftDjBqiiUA\nACUmc17fXkgKAACWxY798eAuvvFEaRQAAJbZJO4JAAAAAAAAAAAAAKDAm0kuJvm6Owht7kvySZJv\nk3yT5MXeODS5KcnpJOeSXEjyam8cGh1McjbJh91BaPNjkvMZ9sGXvVFYNseSPBzFcp3dleSh2fON\nJN8neaAvDo0Oz46HknyR5GhjFvq8lOSdJB90B6HND0lu7w4xZj4gfWefJfmjOwStfs1wlipJLiX5\nLsk9fXFo9PfseGOGs1a/N2ahx71Jnk7yRuZ/BjSrzfpfhWIJu3Mkwxns08056HEgwx8ZFzPcHnGh\nNw4NTiZ5Ocm/3UFotZ3k4yRnkrzQnIUldCQuhTNcBj+T5LnuILS7NcOl8BPNOVisZ5K8Pnt+Iu6x\nXGd3z453Zvhj81hjllFyxhKu7oYk7yZ5O8n7zVno92eSj5I80h2EhXosybMZ7q87leTxJG+1JqLL\nL7Pjb0neS/JoYxaW0JE4Y7nOJhl+eZzsDkKrO5LcNnt+c5JPkzzRF4dmx+OM5bo6nGRz9vyWJJ8n\nebIvDsvmVJKfk/yT5Kckz/fGocHRDPdTncvw0RJnkzzVmogODyb5KsM+OJ/hPjvW1/H4r/B1dX+G\nOXAuw0fQvdIbBwAAAAAAAAAAAAAAAAAAAAAAAAAAltd/dEiJuTINMzsAAAAASUVORK5CYII=\n", 187 | "text/plain": [ 188 | "" 189 | ] 190 | }, 191 | "metadata": {}, 192 | "output_type": "display_data" 193 | } 194 | ], 195 | "source": [ 196 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 197 | "show_world(ax)\n", 198 | "# Prior position: cell 1 (in red)\n", 199 | "showbar(ax, 1, color='red')\n", 200 | "# Measurement: cell 5 (in green)\n", 201 | "showbar(ax, 5, color='green')\n", 202 | "\n", 203 | "# Posterior position: cell 3 (in yellow)\n", 204 | "showbar(ax, 3, color='yellow')" 205 | ] 206 | }, 207 | { 208 | "cell_type": "markdown", 209 | "metadata": {}, 210 | "source": [ 211 | "Just the middle of cell 1 and cell 5, that is, cell 3?" 212 | ] 213 | }, 214 | { 215 | "cell_type": "code", 216 | "execution_count": 8, 217 | "metadata": { 218 | "collapsed": false, 219 | "input_collapsed": false 220 | }, 221 | "outputs": [ 222 | { 223 | "data": { 224 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAABTVJREFUeJzt3DGLXGUUBuB3kigad1FEQUUhlhaCglhIQoKCiIj4Fyxs\nLQQLu+2sJJWdWIiSShTFTnBQBCPBhKgRKwVBDYIiBsHGtbhTJOBmNpszc+7sPA8Md2B2Jy98J/u9\ne+/dSQAAAAAAAAAAAAAAAAAAABivyZzXt5eSAgCAVbFjfzywzBQAAOxfh3b3ZU5crq/LfykxB+vJ\nDJCYAwbmgKtf7HbGEgCAEoolAAAlFEsAAEoolgAAlFAsAQAooVgCAFBCsQQAoIRiCQBACcUSAIAS\niiUAACUUSwAASiiWAACUUCwBACihWAIAUEKxBACghGIJAEAJxRIAgBKKJQAAJYqL5WtJNpNMPEb7\n2JytE8Ci2RPG/1jGnmAOxv2onYHJnNe3rzjMtZnk0nXEYTk2kvy1y6+9fER2OwfsL2aAZG9zYE9Y\nDYveE8zB+F3zDOzYH4uL5by3Yzz2sqZKxXoyAyR7mwN7wupY5J5gDlbDNa3njot6qCTL/7D9jI//\n2kCXbZvC6Ew6NoWthn+TnW3Vv6U/3gEAoIRiCQBACcUSAIASiiUAACUUSwAASiiWAACUUCwBACih\nWAIAUEKxBACghGIJAEAJxRIAgBKKJQAAJRRLAABKKJYAAJRQLAEAKKFYAgBQQrEEAKCEYgkAQAnF\nEgCAEoolAAAlFEsAAEoolgAAlFAsAQAooVgCAFBCsQQAoIRiCQBACcUSAIASiiUAACUUSwAASiiW\nAACUUCwBACihWAIAUEKxBACghGIJAEAJxRIAgBKKJQAAJRRLAABKKJYAAJRQLAEAKDGZ8/r2FYfr\nfjvGYy9rutvvYX8xAyR7mwN7wupY5J5gDlbDNa3njotafMZyo/btWBDrBCyDnzWrYdHrZA7Gr26N\niovlVgzQ2G1kWCeARduKPWHslrEnbMUcjFntDBRfCmf/cRkUM0BiDhiYA5Z6KRwAgHWlWAIAUEKx\nBACghGIJAEAJxRIAgBKKJQAAJRRLAABKKJYAAJRQLAEAKKFYAgBQQrEEAKCEYgkAQAnFEgCAEool\nAAAlFEsAAEoolnNNuwMwCtPuALSbdgdgFKbdAWg37Q4waorlXNPuAIzCtDsA7abdARiFaXcA2k27\nA4yaYgkAQInJnNe3l5ICAIBVsWN/PLiLbzxRGgUAgFU2iXsCAAAAAAAAAAAAACjwZpKLSb7uDkKb\n+5J8kuTbJN8kebE3Dk1uSnI6ybkkF5K82huHRgeTnE3yYXcQ2vyY5HyGOfiyNwqr5liSh6NYrrO7\nkjw0e76R5PskD/TFodHh2fFQki+SHG3MQp+XkryT5IPuILT5Icnt3SHGzAek7+yzJH90h6DVrxnO\nUiXJpSTfJbmnLw6N/p4db8xw1ur3xiz0uDfJ00neyPzPgGZ/s/5XoVjC7hzJcAb7dHMOehzI8EvG\nxQy3R1zojUODk0leTvJvdxBabSf5OMmZJC80Z2EFHYlL4QyXwc8kea47CO1uzXAp/ERzDpbrmSSv\nz56fiHss19nds+OdGX7ZPNaYZZScsYSruyHJu0neTvJ+cxb6/ZnkoySPdAdhqR5L8myG++tOJXk8\nyVutiejyy+z4W5L3kjzamIUVdCTOWK6zSYbN42R3EFrdkeS22fObk3ya5Im+ODQ7Hmcs19XhJJuz\n57ck+TzJk31xWDWnkvyc5J8kPyV5vjcODY5muJ/qXIaPljib5KnWRHR4MMlXGebgfIb77Fhfx+Ov\nwtfV/Rl+DpzL8BF0r/TGAQAAAAAAAAAAAAAAAAAAAAAAAACA1fUfluKJucTQNCIAAAAASUVORK5C\nYII=\n", 225 | "text/plain": [ 226 | "" 227 | ] 228 | }, 229 | "metadata": {}, 230 | "output_type": "display_data" 231 | } 232 | ], 233 | "source": [ 234 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 235 | "show_world(ax)\n", 236 | "# Prior position: cell 1 (in red)\n", 237 | "showbar(ax, 1, color='red')\n", 238 | "# Measurement: cell 5 (in green)\n", 239 | "showbar(ax, 5, color='green')\n", 240 | "\n", 241 | "# Posterior position: cell 4 (in yellow)\n", 242 | "showbar(ax, 4, color='yellow')" 243 | ] 244 | }, 245 | { 246 | "cell_type": "markdown", 247 | "metadata": {}, 248 | "source": [ 249 | "Closer to cell 5, that is, cell 4?" 250 | ] 251 | }, 252 | { 253 | "cell_type": "code", 254 | "execution_count": 9, 255 | "metadata": { 256 | "collapsed": false, 257 | "input_collapsed": false 258 | }, 259 | "outputs": [ 260 | { 261 | "data": { 262 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAABSdJREFUeJzt3LGrW3UYBuA3bRWt96KIgopCHR0EBXGQlhYFERHxX3Bw\ndRAc3O7mJJ3cxEGUTqIoboJBEawUW6pWnBQEtQiKWAQXr8NJoQVvc9t+N99J8jwQTiC54YXfd3Pe\nnHOSBAAAAAAAAAAAAAAAAAAAgPGazHl8eyEpAABYFjv2x32LTAEAwOo6sLunOXC5vi79UGIO1pMZ\nIDEHDMwBVz7Z7YglAAAlFEsAAEoolgAAlFAsAQAooVgCAFBCsQQAoIRiCQBACcUSAIASiiUAACUU\nSwAASiiWAACUUCwBACihWAIAUEKxBACghGIJAEAJxRIAgBKKJQAAJRRLAABKFBfL15JsJpm4jfa2\nOVunvWQOxn1bxAwAXGSfMO5b7T5hMufx7cs2c20muXAdcViMjSR/7fK5l46IOVgdez0DrB5zQGKf\nsKquep+wY38sLpbzXo7xuJY1NQerZS9ngNVjDkjsE1bZVa3njot6oCTL//C2Mz4d/9rbBmFUJt7f\ngU5b3QG4zFb9S/ryDgAAJRRLAABKKJYAAJRQLAEAKKFYAgBQQrEEAKCEYgkAQAnFEgCAEoolAAAl\nFEsAAEoolgAAlFAsAQAooVgCAFBCsQQAoIRiCQBACcUSAIASiiUAACUUSwAASiiWAACUUCwBACih\nWAIAUEKxBACghGIJAEAJxRIAgBKKJQAAJRRLAABKKJYAAJRQLAEAKKFYAgBQQrEEAKCEYgkAQAnF\nEgCAEoolAAAlFEsAAEoolgAAlFAsAQAooVgCAFBCsQQAoIRiCQBAicmcx7cv21z3yzEe17Km5mC1\n7OUMsHrMAYl9wiq7qvXccVGLj1hu1L4ce2Sv18kcjJ81AhbF+8341a1RcbHcigEau40M67SXtmIO\nxmwRMwBw0VbsE8asdp9QfCqc1eP0F2aAxBwwMAcs9FQ4AADrSrEEAKCEYgkAQAnFEgCAEoolAAAl\nFEsAAEoolgAAlFAsAQAooVgCAFBCsQQAoIRiCQBACcUSAIASiiUAACUUSwAASiiWAACUUCznmnYH\nYBSm3QFoN+0OwChMuwPQbtodYNQUy7mm3QEYhWl3ANpNuwMwCtPuALSbdgcYNcUSAIASkzmPby8k\nBQAAy2LH/rh/F394rDQKAADLbBLXBAAAAAAAAAAAAABQ4M0k55N83R2ENvcl+STJt0m+SfJibxya\n3JTkZJIzSc4lebU3Do32Jzmd5MPuILT5McnZDHPwZW8Uls2RJA9HsVxndyV5aHZ/I8n3SR7oi0Oj\ng7PtgSRfJDncmIU+LyV5J8kH3UFo80OS27tDjJkfSN/ZZ0n+6A5Bq18zHKVKkgtJvktyT18cGv09\n296Y4ajV741Z6HFvkqeTvJH5vwHNarP+V6BYwu4cynAE+2RzDnrsy/Ah43yGyyPO9cahwfEkLyf5\ntzsIrbaTfJzkVJIXmrOwhA7FqXCG0+CnkjzXHYR2t2Y4FX6sOQeL9UyS12f3j8U1luvs7tn2zgwf\nNo80ZhklRyzhym5I8m6St5O835yFfn8m+SjJI91BWKjHkjyb4fq6E0keT/JWayK6/DLb/pbkvSSP\nNmZhCR2KI5brbJJh53G8Owit7khy2+z+zUk+TfJEXxyaHY0jluvqYJLN2f1bknye5Mm+OCybE0l+\nTvJPkp+SPN8bhwaHM1xPdSbDT0ucTvJUayI6PJjkqwxzcDbDdXasr6PxrfB1dX+G94EzGX6C7pXe\nOAAAAAAAAAAAAAAAAAAAAAAAAAAAsLz+A77CibhRDbmsAAAAAElFTkSuQmCC\n", 263 | "text/plain": [ 264 | "" 265 | ] 266 | }, 267 | "metadata": {}, 268 | "output_type": "display_data" 269 | } 270 | ], 271 | "source": [ 272 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 273 | "show_world(ax)\n", 274 | "# Prior position: cell 1 (in red)\n", 275 | "showbar(ax, 1, color='red')\n", 276 | "# Measurement: cell 5 (in green)\n", 277 | "showbar(ax, 5, color='green')\n", 278 | "\n", 279 | "# Posterior position: cell 2 (in yellow)\n", 280 | "showbar(ax, 2, color='yellow')" 281 | ] 282 | }, 283 | { 284 | "cell_type": "markdown", 285 | "metadata": {}, 286 | "source": [ 287 | "Closer to cell 1, that is, cell 2?" 288 | ] 289 | }, 290 | { 291 | "cell_type": "markdown", 292 | "metadata": {}, 293 | "source": [ 294 | "## Probabilistic World" 295 | ] 296 | }, 297 | { 298 | "cell_type": "markdown", 299 | "metadata": {}, 300 | "source": [ 301 | "* Prior: [0.2, 0.5, 0.2, 0.05, 0.05] (in red)\n", 302 | "\n", 303 | "* Measurement: [0.05, 0.05, 0.1, 0.7, 0.1] (in green)" 304 | ] 305 | }, 306 | { 307 | "cell_type": "code", 308 | "execution_count": 10, 309 | "metadata": { 310 | "collapsed": false, 311 | "input_collapsed": false 312 | }, 313 | "outputs": [ 314 | { 315 | "data": { 316 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nAAAAAAAAANhc3wBZSrVWpJY2zQAAAABJRU5ErkJggg==\n", 317 | "text/plain": [ 318 | "" 319 | ] 320 | }, 321 | "metadata": {}, 322 | "output_type": "display_data" 323 | } 324 | ], 325 | "source": [ 326 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 327 | "show_world(ax)\n", 328 | "# Prior: (in red)\n", 329 | "prior_p = [0.2, 0.5, 0.2, 0.05, 0.05]\n", 330 | "showbar(ax, height=prior_p, color='red')" 331 | ] 332 | }, 333 | { 334 | "cell_type": "code", 335 | "execution_count": 25, 336 | "metadata": { 337 | "collapsed": false, 338 | "input_collapsed": false 339 | }, 340 | "outputs": [ 341 | { 342 | "data": { 343 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\n", 344 | "text/plain": [ 345 | "" 346 | ] 347 | }, 348 | "metadata": {}, 349 | "output_type": "display_data" 350 | } 351 | ], 352 | "source": [ 353 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 354 | "show_world(ax)\n", 355 | "\n", 356 | "pmea = [0.05, 0.05, 0.1, 0.6, 0.2] # Measurement\n", 357 | "showbar(ax, height=pmea, color='green')" 358 | ] 359 | }, 360 | { 361 | "cell_type": "markdown", 362 | "metadata": {}, 363 | "source": [ 364 | "* Posterior: prior probability * measurement probabilit ?" 365 | ] 366 | }, 367 | { 368 | "cell_type": "code", 369 | "execution_count": 27, 370 | "metadata": { 371 | "collapsed": false, 372 | "input_collapsed": false 373 | }, 374 | "outputs": [ 375 | { 376 | "name": "stdout", 377 | "output_type": "stream", 378 | "text": [ 379 | "[ 0.01 0.025 0.02 0.035 0.005]\n" 380 | ] 381 | } 382 | ], 383 | "source": [ 384 | "post_p = np.array(pmea)*np.array(prior_p)\n", 385 | "print post_p" 386 | ] 387 | }, 388 | { 389 | "cell_type": "markdown", 390 | "metadata": {}, 391 | "source": [ 392 | "* Posterior: prior probability * measurement probability / sum" 393 | ] 394 | }, 395 | { 396 | "cell_type": "code", 397 | "execution_count": 28, 398 | "metadata": { 399 | "collapsed": false, 400 | "input_collapsed": false 401 | }, 402 | "outputs": [ 403 | { 404 | "name": "stdout", 405 | "output_type": "stream", 406 | "text": [ 407 | "Posterior: [ 0.10526316 0.26315789 0.21052632 0.36842105 0.05263158]\n" 408 | ] 409 | } 410 | ], 411 | "source": [ 412 | "post_p = post_p / sum(post_p)\n", 413 | "#print 'Sum of Posterior: ',sum(post_p)\n", 414 | "print 'Posterior: ', post_p" 415 | ] 416 | }, 417 | { 418 | "cell_type": "code", 419 | "execution_count": 29, 420 | "metadata": { 421 | "collapsed": false, 422 | "input_collapsed": false 423 | }, 424 | "outputs": [ 425 | { 426 | "name": "stdout", 427 | "output_type": "stream", 428 | "text": [ 429 | "Posterior: [ 0.10526316 0.26315789 0.21052632 0.36842105 0.05263158]\n" 430 | ] 431 | }, 432 | { 433 | "data": { 434 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nrkJggg==\n", 435 | "text/plain": [ 436 | "" 437 | ] 438 | }, 439 | "metadata": {}, 440 | "output_type": "display_data" 441 | } 442 | ], 443 | "source": [ 444 | "def meas(prior_p, pmea):\n", 445 | " post_p = np.array(pmea)*np.array(prior_p)\n", 446 | " post_p = post_p / sum(post_p)\n", 447 | " return post_p\n", 448 | "\n", 449 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 450 | "show_world(ax)\n", 451 | "\n", 452 | "post_p = meas(prior_p, pmea)\n", 453 | "print 'Posterior: ', post_p\n", 454 | "showbar(ax, height=post_p, color='yellow')" 455 | ] 456 | }, 457 | { 458 | "cell_type": "markdown", 459 | "metadata": {}, 460 | "source": [ 461 | "## Motion" 462 | ] 463 | }, 464 | { 465 | "cell_type": "markdown", 466 | "metadata": {}, 467 | "source": [ 468 | "* Accurate: shifting" 469 | ] 470 | }, 471 | { 472 | "cell_type": "code", 473 | "execution_count": 30, 474 | "metadata": { 475 | "collapsed": false, 476 | "input_collapsed": false 477 | }, 478 | "outputs": [ 479 | { 480 | "name": "stdout", 481 | "output_type": "stream", 482 | "text": [ 483 | "P: \n", 484 | "[ 0.10526316 0.26315789 0.21052632 0.36842105 0.05263158]\n", 485 | "P after moving 1 step: \n", 486 | "[ 0.05263158 0.10526316 0.26315789 0.21052632 0.36842105]\n" 487 | ] 488 | } 489 | ], 490 | "source": [ 491 | "def move(p, step):\n", 492 | " q= [ ] #Start with empty list\n", 493 | " for i in range(len(p)):\n", 494 | " q.append(p[(i-step) % len (p)]) #cycle\n", 495 | " return np.array(q)\n", 496 | "\n", 497 | "print 'P: \\n', post_p\n", 498 | "p_after_move1 = move(post_p, 1)\n", 499 | "print 'P after moving 1 step: \\n', p_after_move1" 500 | ] 501 | }, 502 | { 503 | "cell_type": "code", 504 | "execution_count": 15, 505 | "metadata": { 506 | "collapsed": false, 507 | "input_collapsed": true 508 | }, 509 | "outputs": [ 510 | { 511 | "data": { 512 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nplvMswwXAAAAAElFTkSuQmCC\n", 513 | "text/plain": [ 514 | "" 515 | ] 516 | }, 517 | "metadata": {}, 518 | "output_type": "display_data" 519 | } 520 | ], 521 | "source": [ 522 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 523 | "show_world(ax)\n", 524 | "\n", 525 | "showbar(ax, height=p_after_move1, color='yellow')" 526 | ] 527 | }, 528 | { 529 | "cell_type": "markdown", 530 | "metadata": {}, 531 | "source": [ 532 | "* Inaccurate: convolution" 533 | ] 534 | }, 535 | { 536 | "cell_type": "markdown", 537 | "metadata": {}, 538 | "source": [ 539 | "$P(X_{i+s} \\mid X_i)=0.8$\n", 540 | "\n", 541 | "$P(X_{i+s+1} \\mid X_i)=0.1$\n", 542 | "\n", 543 | "$P(X_{i+s-1} \\mid X_i)=0.1$\n", 544 | "\n", 545 | "s: step" 546 | ] 547 | }, 548 | { 549 | "cell_type": "markdown", 550 | "metadata": { 551 | "input_collapsed": true 552 | }, 553 | "source": [ 554 | "* Exact position" 555 | ] 556 | }, 557 | { 558 | "cell_type": "code", 559 | "execution_count": 16, 560 | "metadata": { 561 | "collapsed": false, 562 | "input_collapsed": false 563 | }, 564 | "outputs": [ 565 | { 566 | "data": { 567 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nQIp/AV3XMP6xRDtcAAAAAElFTkSuQmCC\n", 568 | "text/plain": [ 569 | "" 570 | ] 571 | }, 572 | "metadata": {}, 573 | "output_type": "display_data" 574 | } 575 | ], 576 | "source": [ 577 | "fig = plt.figure()\n", 578 | "ax = fig.add_axes((0.1, 0.2, 1.5, 0.3))\n", 579 | "show_world(ax)\n", 580 | "\n", 581 | "# original: cell 2\n", 582 | "showbar(ax, 2, height=1, color='red')" 583 | ] 584 | }, 585 | { 586 | "cell_type": "markdown", 587 | "metadata": {}, 588 | "source": [ 589 | "* Move 1" 590 | ] 591 | }, 592 | { 593 | "cell_type": "code", 594 | "execution_count": 17, 595 | "metadata": { 596 | "collapsed": false, 597 | "input_collapsed": false 598 | }, 599 | "outputs": [ 600 | { 601 | "data": { 602 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nAEjiH/xUZTzIYmswAAAAAElFTkSuQmCC\n", 603 | "text/plain": [ 604 | "" 605 | ] 606 | }, 607 | "metadata": {}, 608 | "output_type": "display_data" 609 | } 610 | ], 611 | "source": [ 612 | "fig = plt.figure()\n", 613 | "ax = fig.add_axes((0.1, 0.2, 1.5, 0.3))\n", 614 | "show_world(ax)\n", 615 | "\n", 616 | "# original: cell 2\n", 617 | "showbar(ax, 2, height=1, color='red')\n", 618 | "\n", 619 | "# after move 1 \n", 620 | "s = 1\n", 621 | "newpos = 2 + s + np.array([-1, 0, 1])\n", 622 | "move_prob = [0.1, 0.8, 0.1]\n", 623 | "showbar(ax, newpos, height=move_prob, color='green')" 624 | ] 625 | }, 626 | { 627 | "cell_type": "markdown", 628 | "metadata": {}, 629 | "source": [ 630 | "* Move 2" 631 | ] 632 | }, 633 | { 634 | "cell_type": "code", 635 | "execution_count": 18, 636 | "metadata": { 637 | "collapsed": false, 638 | "input_collapsed": false 639 | }, 640 | "outputs": [ 641 | { 642 | "data": { 643 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nLO6XzgAAAABJRU5ErkJggg==\n", 644 | "text/plain": [ 645 | "" 646 | ] 647 | }, 648 | "metadata": {}, 649 | "output_type": "display_data" 650 | } 651 | ], 652 | "source": [ 653 | "fig = plt.figure()\n", 654 | "ax = fig.add_axes((0.1, 0.2, 1.5, 0.3))\n", 655 | "show_world(ax)\n", 656 | "\n", 657 | "# original: cell 2\n", 658 | "showbar(ax, 2, height=1, color='red')\n", 659 | "\n", 660 | "# after move 2 \n", 661 | "s = 2\n", 662 | "newpos = 2 + s + np.array([-1, 0, 1])\n", 663 | "move_prob = [0.1, 0.8, 0.1]\n", 664 | "showbar(ax, newpos, height=move_prob, color='green')" 665 | ] 666 | }, 667 | { 668 | "cell_type": "markdown", 669 | "metadata": {}, 670 | "source": [ 671 | "* Inaccurate position" 672 | ] 673 | }, 674 | { 675 | "cell_type": "code", 676 | "execution_count": 19, 677 | "metadata": { 678 | "collapsed": false, 679 | "input_collapsed": false 680 | }, 681 | "outputs": [ 682 | { 683 | "data": { 684 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAButJREFUeJzt3c2rTf8ewPHPkpH2SZSngWLoeGjvyCk5Aw+ZoIiJkgH+\nAIZGzsiEn6KMDJQMDCUhih0xEFFmJhR5KAN1joc87Tvg3p97u2cfP7/P3t/1c16vWrW19+FzWl9r\nvy1nr1V1Op1OAADA3zSl9AAAAPwehCUAACmEJQAAKYQlAAAphCUAACmmdnuyqqp+zQEAwD9AtwsK\nOWMJAECKrmcs/+RSl5PXj2etrYPJyRogwjrgG+uA7v+b7YwlAAAphCUAACmEJQAAKYQlAAAphCUA\nACmEJQAAKYQlAAAphCUAACmEJQAAKYQlPfBHRAzEt6vz2+q3DXzfRwCQq+p0uZN4Vf37tj1u2zR5\n/crtuwYiYqwHs5CnERGjP/lat3AjwjrgG+uAKrqko7BkIr9yEOl+H1Hq4lf2p2PB5GUdEGEdMFFY\nTu3jJExCDjv1IvkB6CU/YwkAQAphCQBACmEJAEAKYQkAQAphCQBACmEJAEAKYQkAQAphCQBACmEJ\nAEAKYQkAQAphCQBACmEJAEAKYQkAQAphCQBACmEJAEAKYQkAQAphCQBACmEJAEAKYQkAQAphCQBA\nCmEJAEAKYQkAQAphCQBACmEJAEAKYQkAQAphCQBACmEJAEAKYQkAQAphCQBACmEJAEAKYQkAQIrk\nsPwjIgYiorLVdhv4vp+glxwL6r85FgD5qk6n0xn3yar6/mjcl/yPgYgY+9tD0WuNiBj9yddWPzz+\n2XXw59f87FfQH9V//eqv70/Hgt9Nr48F/H6sA6roko7ZYVlN/BJqopdRISzrqn9h6Vjwz9HLdcDv\nxzqge1hO7dUfa7nVj7d6SnAsqB/HAqBXfHgHAIAUwhIAgBTCEgCAFMISAIAUwhIAgBTCEgCAFMIS\nAIAUwhIAgBTCEgCAFMISAIAUwhIAgBTCEgCAFMISAIAUPQvLyla7rYTS37PNGrCVXgcjCRPbereN\njLfjko304Xux1WENJIdlI/e3o0d6vZ+sg/qzBoiwn4BsyWE5Eg5UddeI3v8LdSSsgzqzBojozzoA\nJpuq0+l0xn2yqr4/Gvcl/PaqHx5bB5OTNUCEdcA31gFVdElHH94BACCHsAQAIIWwBAAghbAEACCF\nsAQAIIWwBAAghbAEACCFsAQAIIWwBAAghbAEACCFsAQAIIWwBAAghbAEACCFsAQAIIWwBAAghbCc\nULv0ANRCu/QAFNcuPQC10C49AMW1Sw9Qa8JyQu3SA1AL7dIDUFy79ADUQrv0ABTXLj1ArQlLAABS\nVJ1OpzPuk1XVz1kAAKi5LunY/YzlwYMH04cBAOCfa2RkZNznup6xBACAn+VnLAEASCEsAQBIISwB\nAEghLMexe/fumDNnTixdurT0KBTy9OnTWLNmTSxevDiWLFkSx48fLz0SBXz48CGGhoai2WzG4OBg\nHDhwoPRIFPLly5dotVqxefPm0qNQyIIFC2LZsmXRarVi5cqVpcepJR/eGcfNmzej0WjErl274uHD\nh6XHoYCXL1/Gy5cvo9lsxtjYWCxfvjzOnTsXixYtKj0affbu3buYNm1afP78OVavXh1HjhyJ1atX\nlx6LPjt69Gjcu3cvRkdH4/z586XHoYCFCxfGvXv3YubMmaVHqS1nLMcxPDwcM2bMKD0GBc2dOzea\nzWZERDQajVi0aFE8f/688FSUMG3atIiI+PjxY3z58sWbyiT07NmzuHjxYuzdu7frNfz4/dn/3QlL\n+AlPnjyJ+/fvx9DQUOlRKODr16/RbDZjzpw5sWbNmhgcHCw9En22f//+OHz4cEyZ4m1zMquqKtav\nXx8rVqyIkydPlh6nlvwNgQmMjY3F9u3b49ixY9FoNEqPQwFTpkyJBw8exLNnz+LGjRvRbrdLj0Qf\nXbhwIWbPnh2tVsvZqknu1q1bcf/+/bh06VKcOHEibt68WXqk2hGW0MWnT59i27ZtsXPnztiyZUvp\ncShs+vTpsXHjxrh7927pUeij27dvx/nz52PhwoWxY8eOuHbtWuzatav0WBQwb968iIiYNWtWbN26\nNe7cuVN4ovoRljCOTqcTe/bsicHBwdi3b1/pcSjk9evX8ebNm4iIeP/+fVy9ejVarVbhqeinQ4cO\nxdOnT+Px48dx9uzZWLt2bZw+fbr0WPTZu3fvYnR0NCIi3r59G1euXHHlmP9DWI5jx44dsWrVqnj0\n6FHMnz8/Tp06VXok+uzWrVtx5syZuH79erRarWi1WnH58uXSY9FnL168iLVr10az2YyhoaHYvHlz\nrFu3rvRYFFRVVekRKODVq1cxPDz8n2PBpk2bYsOGDaXHqh2XGwIAIIUzlgAApBCWAACkEJYAAKQQ\nlgAApBCWAACkEJYAAKQQlgAApPgXS59iGQpq5fIAAAAASUVORK5CYII=\n", 685 | "text/plain": [ 686 | "" 687 | ] 688 | }, 689 | "metadata": {}, 690 | "output_type": "display_data" 691 | } 692 | ], 693 | "source": [ 694 | "fig = plt.figure()\n", 695 | "ax = fig.add_axes((0.1, 0.2, 1.5, 0.3))\n", 696 | "show_world(ax)\n", 697 | "\n", 698 | "# original:\n", 699 | "ini_pos = [0.2, 0.6, 0.2, 0, 0]\n", 700 | "showbar(ax, height=ini_pos, color='red')" 701 | ] 702 | }, 703 | { 704 | "cell_type": "code", 705 | "execution_count": 20, 706 | "metadata": { 707 | "collapsed": false 708 | }, 709 | "outputs": [], 710 | "source": [ 711 | "def move_v2(p, step):\n", 712 | " #Introduce auxiliary variable s\n", 713 | " q= []\n", 714 | " #Add exact probability\n", 715 | " pExact = 0.8\n", 716 | " #Add overshoot probability\n", 717 | " pOvershoot = 0.1\n", 718 | " #Add undershoot probability\n", 719 | " pUndershoot = 0.1\n", 720 | "\n", 721 | " for i in range(len(p)):\n", 722 | " s = pExact * p[(i-step) % len(p)]\n", 723 | " s = s + pOvershoot * p[(i-step-1) % len(p)]\n", 724 | " s = s + pUndershoot * p[(i-step+1) % len(p)]\n", 725 | " q.append(s)\n", 726 | " return q" 727 | ] 728 | }, 729 | { 730 | "cell_type": "code", 731 | "execution_count": 21, 732 | "metadata": { 733 | "collapsed": false 734 | }, 735 | "outputs": [ 736 | { 737 | "name": "stdout", 738 | "output_type": "stream", 739 | "text": [ 740 | "P: \n", 741 | "[0.2, 0.6, 0.2, 0, 0]\n", 742 | "P after moving: \n", 743 | "[0.020000000000000004, 0.22000000000000003, 0.52, 0.22000000000000003, 0.020000000000000004]\n" 744 | ] 745 | }, 746 | { 747 | "data": { 748 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nXG5TWnhDuAAAAABJRU5ErkJggg==\n", 749 | "text/plain": [ 750 | "" 751 | ] 752 | }, 753 | "metadata": {}, 754 | "output_type": "display_data" 755 | } 756 | ], 757 | "source": [ 758 | "fig = plt.figure()\n", 759 | "ax = fig.add_axes((0.1, 0.2, 1.5, 0.3))\n", 760 | "show_world(ax)\n", 761 | "\n", 762 | "# original:\n", 763 | "ini_pos = [0.2, 0.6, 0.2, 0, 0]\n", 764 | "\n", 765 | "# after move 2 \n", 766 | "s = 1\n", 767 | "print 'P: \\n', ini_pos\n", 768 | "p_after_move = move_v2(ini_pos, s)\n", 769 | "print 'P after moving: \\n', p_after_move\n", 770 | "showbar(ax, height=p_after_move, color='green')" 771 | ] 772 | }, 773 | { 774 | "cell_type": "markdown", 775 | "metadata": {}, 776 | "source": [ 777 | "## Iterate Measurement + Motion " 778 | ] 779 | }, 780 | { 781 | "cell_type": "code", 782 | "execution_count": 22, 783 | "metadata": { 784 | "collapsed": false 785 | }, 786 | "outputs": [ 787 | { 788 | "name": "stdout", 789 | "output_type": "stream", 790 | "text": [ 791 | "[ 0.04819277 0.24096386 0.6746988 0.02409639 0.01204819]\n" 792 | ] 793 | }, 794 | { 795 | "data": { 796 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nQrEEACCFYgkAQArFEgCAFIolAAAp/gmF/WIDDsJs3AAAAABJRU5ErkJggg==\n", 797 | "text/plain": [ 798 | "" 799 | ] 800 | }, 801 | "metadata": {}, 802 | "output_type": "display_data" 803 | } 804 | ], 805 | "source": [ 806 | "# Prior\n", 807 | "prior_p = [0.2, 0.5, 0.2, 0.05, 0.05]\n", 808 | "# Measurement 1\n", 809 | "pmea1 = [0.05, 0.1, 0.7, 0.1, 0.05] # Measurement\n", 810 | "# Moves right 1\n", 811 | "s1 = 1\n", 812 | "# Measurement 2\n", 813 | "pmea2 = [0.05, 0.05, 0.1, 0.7, 0.1] # Measurement\n", 814 | "# Moves right 2\n", 815 | "s2 = 2\n", 816 | "# Measurement 3\n", 817 | "pmea3 = [0.7, 0.1, 0.05, 0.05, 0.1 ] # Measurement\n", 818 | "\n", 819 | "post_p1 = meas(prior_p, pmea1)\n", 820 | "print post_p1\n", 821 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 822 | "show_world(ax)\n", 823 | "showbar(ax, height=post_p1, color='green')" 824 | ] 825 | }, 826 | { 827 | "cell_type": "code", 828 | "execution_count": 21, 829 | "metadata": { 830 | "collapsed": false 831 | }, 832 | "outputs": [ 833 | { 834 | "name": "stdout", 835 | "output_type": "stream", 836 | "text": [ 837 | "[0.016867469879518079, 0.063855421686747002, 0.2650602409638555, 0.56626506024096379, 0.087951807228915671]\n" 838 | ] 839 | }, 840 | { 841 | "data": { 842 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\n/gAAAABJRU5ErkJggg==\n", 843 | "text/plain": [ 844 | "" 845 | ] 846 | }, 847 | "metadata": {}, 848 | "output_type": "display_data" 849 | } 850 | ], 851 | "source": [ 852 | "post_p2 = move_v2(post_p1, s1)\n", 853 | "print post_p2\n", 854 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 855 | "show_world(ax)\n", 856 | "showbar(ax, height=post_p2, color='green')" 857 | ] 858 | }, 859 | { 860 | "cell_type": "code", 861 | "execution_count": 22, 862 | "metadata": { 863 | "collapsed": false 864 | }, 865 | "outputs": [ 866 | { 867 | "name": "stdout", 868 | "output_type": "stream", 869 | "text": [ 870 | "[ 0.00193557 0.00732753 0.0608323 0.90971934 0.02018526]\n" 871 | ] 872 | }, 873 | { 874 | "data": { 875 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nFkyM0SaxTwAAAABJRU5ErkJggg==\n", 876 | "text/plain": [ 877 | "" 878 | ] 879 | }, 880 | "metadata": {}, 881 | "output_type": "display_data" 882 | } 883 | ], 884 | "source": [ 885 | "post_p3 = meas(post_p2, pmea2)\n", 886 | "print post_p3\n", 887 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 888 | "show_world(ax)\n", 889 | "showbar(ax, height=post_p3, color='green')" 890 | ] 891 | }, 892 | { 893 | "cell_type": "code", 894 | "execution_count": 23, 895 | "metadata": { 896 | "collapsed": false 897 | }, 898 | "outputs": [ 899 | { 900 | "name": "stdout", 901 | "output_type": "stream", 902 | "text": [ 903 | "[0.73587722936540856, 0.10731370109221623, 0.0042997373150836463, 0.012138808240011068, 0.14037052398728056]\n" 904 | ] 905 | }, 906 | { 907 | "data": { 908 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\n4AAAAABJRU5ErkJggg==\n", 909 | "text/plain": [ 910 | "" 911 | ] 912 | }, 913 | "metadata": {}, 914 | "output_type": "display_data" 915 | } 916 | ], 917 | "source": [ 918 | "post_p4 = move_v2(post_p3, s2)\n", 919 | "print post_p4\n", 920 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 921 | "show_world(ax)\n", 922 | "showbar(ax, height=post_p4, color='green')" 923 | ] 924 | }, 925 | { 926 | "cell_type": "code", 927 | "execution_count": 24, 928 | "metadata": { 929 | "collapsed": false 930 | }, 931 | "outputs": [ 932 | { 933 | "name": "stdout", 934 | "output_type": "stream", 935 | "text": [ 936 | "[ 9.52672201e-01 1.98470179e-02 3.97605164e-04 1.12249947e-03\n", 937 | " 2.59606767e-02]\n" 938 | ] 939 | }, 940 | { 941 | "data": { 942 | "image/png": "iVBORw0KGgoAAAANSUhEUgAAApYAAAByCAYAAADpuJG3AAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nEJYAAKQQlgAApBCWAACkEJYAAKT4J/rJVmYYPezyAAAAAElFTkSuQmCC\n", 943 | "text/plain": [ 944 | "" 945 | ] 946 | }, 947 | "metadata": {}, 948 | "output_type": "display_data" 949 | } 950 | ], 951 | "source": [ 952 | "post_p5 = meas(post_p4, pmea3)\n", 953 | "print post_p5\n", 954 | "ax = plt.figure().add_axes((0.1, 0.2, 1.5, 0.3))\n", 955 | "show_world(ax)\n", 956 | "showbar(ax, height=post_p5, color='green')" 957 | ] 958 | }, 959 | { 960 | "cell_type": "markdown", 961 | "metadata": {}, 962 | "source": [ 963 | "## Positioning: Iteration of Measurement + Motion\n", 964 | "* Measurement: Production followed by normalization\n", 965 | "\n", 966 | " Bayes's rule\n", 967 | "\n", 968 | " $P(x|Z)=\\frac{P(Z|x)P(x)}{P(Z)}$\n", 969 | "\n", 970 | " x=cell\n", 971 | "\n", 972 | " Z=measurement\n", 973 | "\n", 974 | " $P(x|Z)=$probability of cell after measurment\n", 975 | "\n", 976 | " $P(x)=$probability of cell before measurment\n", 977 | "\n", 978 | " $P(Z|x)=$probability of measurment on the condition of cell\n", 979 | "\n", 980 | " $P(Z)=\\sum P(Z|x)P(x)$total probability of making measurement \"Z\"\n", 981 | "\n", 982 | "* Motion: Convolution\n", 983 | " \n", 984 | " Theorem of Total Probability\n", 985 | " \n", 986 | " $P(x_i^{t})=\\sum_jP(x_j^{t-1})P(x_i|x_j)$\n", 987 | " \n", 988 | " $P(x_i|x_j)=$probability of moving from cell $x_j$ to cell $x_i$\n", 989 | " \n", 990 | " $t=$time" 991 | ] 992 | }, 993 | { 994 | "cell_type": "markdown", 995 | "metadata": {}, 996 | "source": [ 997 | "## Resources" 998 | ] 999 | }, 1000 | { 1001 | "cell_type": "markdown", 1002 | "metadata": {}, 1003 | "source": [ 1004 | "* [1] https://docs.python.org/2.7/\n", 1005 | "* [2] http://ipython.org/documentation.html\n", 1006 | "* [3] http://nbviewer.ipython.org/github/ipython/ipython/blob/3.x/examples/Notebook/Index.ipynb\n", 1007 | "* [4] http://docs.scipy.org/doc/numpy/reference/\n", 1008 | "* [5] http://docs.scipy.org/doc/scipy/reference/\n", 1009 | "* [6] http://matplotlib.org/contents.html" 1010 | ] 1011 | }, 1012 | { 1013 | "cell_type": "code", 1014 | "execution_count": 1, 1015 | "metadata": { 1016 | "collapsed": false 1017 | }, 1018 | "outputs": [ 1019 | { 1020 | "data": { 1021 | "application/json": { 1022 | "Software versions": [ 1023 | { 1024 | "module": "Python", 1025 | "version": "2.7.11+ 64bit [GCC 5.3.1 20160429]" 1026 | }, 1027 | { 1028 | "module": "IPython", 1029 | "version": "5.1.0" 1030 | }, 1031 | { 1032 | "module": "OS", 1033 | "version": "Linux 4.4.0 2 deepin amd64 x86_64 with debian 8.0" 1034 | }, 1035 | { 1036 | "module": "scipy", 1037 | "version": "0.18.0" 1038 | }, 1039 | { 1040 | "module": "numpy", 1041 | "version": "1.11.1" 1042 | }, 1043 | { 1044 | "module": "matplotlib", 1045 | "version": "1.5.2" 1046 | } 1047 | ] 1048 | }, 1049 | "text/html": [ 1050 | "
SoftwareVersion
Python2.7.11+ 64bit [GCC 5.3.1 20160429]
IPython5.1.0
OSLinux 4.4.0 2 deepin amd64 x86_64 with debian 8.0
scipy0.18.0
numpy1.11.1
matplotlib1.5.2
Sun Sep 18 22:16:56 2016 CST
" 1051 | ], 1052 | "text/latex": [ 1053 | "\\begin{tabular}{|l|l|}\\hline\n", 1054 | "{\\bf Software} & {\\bf Version} \\\\ \\hline\\hline\n", 1055 | "Python & 2.7.11+ 64bit [GCC 5.3.1 20160429] \\\\ \\hline\n", 1056 | "IPython & 5.1.0 \\\\ \\hline\n", 1057 | "OS & Linux 4.4.0 2 deepin amd64 x86\\_64 with debian 8.0 \\\\ \\hline\n", 1058 | "scipy & 0.18.0 \\\\ \\hline\n", 1059 | "numpy & 1.11.1 \\\\ \\hline\n", 1060 | "matplotlib & 1.5.2 \\\\ \\hline\n", 1061 | "\\hline \\multicolumn{2}{|l|}{Sun Sep 18 22:16:56 2016 CST} \\\\ \\hline\n", 1062 | "\\end{tabular}\n" 1063 | ], 1064 | "text/plain": [ 1065 | "Software versions\n", 1066 | "Python 2.7.11+ 64bit [GCC 5.3.1 20160429]\n", 1067 | "IPython 5.1.0\n", 1068 | "OS Linux 4.4.0 2 deepin amd64 x86_64 with debian 8.0\n", 1069 | "scipy 0.18.0\n", 1070 | "numpy 1.11.1\n", 1071 | "matplotlib 1.5.2\n", 1072 | "Sun Sep 18 22:16:56 2016 CST" 1073 | ] 1074 | }, 1075 | "execution_count": 1, 1076 | "metadata": {}, 1077 | "output_type": "execute_result" 1078 | } 1079 | ], 1080 | "source": [ 1081 | "# Install the version_information package using pip:\n", 1082 | "# pip install version_information\n", 1083 | "# or, alternatively, use the %install_ext IPython command (deprecated):\n", 1084 | "# %install_ext http://raw.github.com/jrjohansson/version_information/master/version_information.py\n", 1085 | "\n", 1086 | "%load_ext version_information\n", 1087 | "%version_information scipy, numpy, matplotlib" 1088 | ] 1089 | } 1090 | ], 1091 | "metadata": { 1092 | "hide_input": false, 1093 | "kernelspec": { 1094 | "display_name": "Python 2", 1095 | "language": "python", 1096 | "name": "python2" 1097 | }, 1098 | "language_info": { 1099 | "codemirror_mode": { 1100 | "name": "ipython", 1101 | "version": 2 1102 | }, 1103 | "file_extension": ".py", 1104 | "mimetype": "text/x-python", 1105 | "name": "python", 1106 | "nbconvert_exporter": "python", 1107 | "pygments_lexer": "ipython2", 1108 | "version": "2.7.11+" 1109 | } 1110 | }, 1111 | "nbformat": 4, 1112 | "nbformat_minor": 0 1113 | } 1114 | -------------------------------------------------------------------------------- /Nonlieaner Filter Positioning/538.json: -------------------------------------------------------------------------------- 1 | { 2 | "lines.linewidth": 2.0, 3 | "patch.linewidth": 0.5, 4 | "legend.fancybox": true, 5 | "axes.color_cycle": [ 6 | "#6d904f", 7 | "#013afe", 8 | "#202020", 9 | "#fc4f30", 10 | "#e5ae38", 11 | "#A60628", 12 | "#30a2da", 13 | "#008080", 14 | "#7A68A6", 15 | "#CF4457", 16 | "#188487", 17 | "#E24A33" 18 | ], 19 | "axes.facecolor": "#ffffff", 20 | "axes.labelsize": "large", 21 | "axes.axisbelow": true, 22 | "axes.grid": true, 23 | "patch.edgecolor": "#f0f0f0", 24 | "axes.titlesize": "x-large", 25 | "examples.directory": "", 26 | "figure.facecolor": "#ffffff", 27 | "grid.linestyle": "-", 28 | "grid.linewidth": 2.0, 29 | "grid.color": "#cbcbcb", 30 | "axes.edgecolor":"#f0f0f0", 31 | "xtick.major.size": 0, 32 | "xtick.minor.size": 0, 33 | "ytick.major.size": 0, 34 | "ytick.minor.size": 0, 35 | "axes.linewidth": 3.0, 36 | "font.size":14.0, 37 | "lines.linewidth": 4, 38 | "lines.solid_capstyle": "butt", 39 | "savefig.edgecolor": "#f0f0f0", 40 | "savefig.facecolor": "#f0f0f0", 41 | "figure.subplot.left" : 0.08, 42 | "figure.subplot.right" : 0.95, 43 | "figure.subplot.bottom" : 0.07, 44 | "figure.subplot.hspace" : 0.5, 45 | "legend.scatterpoints" : 1 46 | } 47 | 48 | -------------------------------------------------------------------------------- /Nonlieaner Filter Positioning/book_format.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from IPython.core.display import HTML 3 | import matplotlib.pylab as pylab 4 | import matplotlib.pyplot as plt 5 | import json 6 | import numpy as np 7 | import sys 8 | from contextlib import contextmanager 9 | 10 | 11 | def equal_axis(): 12 | pylab.rcParams['figure.figsize'] = 10,10 13 | plt.axis('equal') 14 | 15 | def reset_axis(): 16 | pylab.rcParams['figure.figsize'] = 11, 5.5 17 | 18 | def set_figsize(x, y): 19 | pylab.rcParams['figure.figsize'] = x, y 20 | 21 | 22 | @contextmanager 23 | def figsize(x,y): 24 | """Temporarily set the figure size using 'with figsize(a,b):'""" 25 | 26 | set_figsize(x,y) 27 | yield 28 | reset_axis() 29 | 30 | @contextmanager 31 | def numpy_precision(precision): 32 | old = np.get_printoptions()['precision'] 33 | np.set_printoptions(precision=precision) 34 | yield 35 | np.set_printoptions(old) 36 | 37 | 38 | def _decode_list(data): 39 | rv = [] 40 | for item in data: 41 | if isinstance(item, unicode): 42 | item = item.encode('utf-8') 43 | elif isinstance(item, list): 44 | item = _decode_list(item) 45 | elif isinstance(item, dict): 46 | item = _decode_dict(item) 47 | rv.append(item) 48 | return rv 49 | 50 | def _decode_dict(data): 51 | rv = {} 52 | for key, value in data.iteritems(): 53 | if isinstance(key, unicode): 54 | key = key.encode('utf-8') 55 | if isinstance(value, unicode): 56 | value = value.encode('utf-8') 57 | elif isinstance(value, list): 58 | value = _decode_list(value) 59 | elif isinstance(value, dict): 60 | value = _decode_dict(value) 61 | rv[key] = value 62 | return rv 63 | 64 | 65 | def load_style(directory = '.', name='/custom2.css'): 66 | if sys.version_info[0] >= 3: 67 | s = json.load(open(directory + "/538.json")) 68 | else: 69 | s = json.load(open(directory + "/538.json"), object_hook=_decode_dict) 70 | plt.rcParams.update(s) 71 | reset_axis () 72 | np.set_printoptions(suppress=True) 73 | 74 | styles = open(directory + name, 'r').read() 75 | return HTML(styles) 76 | -------------------------------------------------------------------------------- /Nonlieaner Filter Positioning/book_plots.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Fri May 2 12:21:40 2014 4 | 5 | @author: rlabbe 6 | """ 7 | import matplotlib.pyplot as plt 8 | import numpy as np 9 | 10 | def bar_plot(pos, ylim=(0,1), title=None): 11 | plt.cla() 12 | ax = plt.gca() 13 | x = np.arange(len(pos)) 14 | ax.bar(x, pos, color='#30a2da') 15 | if ylim: 16 | plt.ylim(ylim) 17 | plt.xticks(x+0.4, x) 18 | if title is not None: 19 | plt.title(title) 20 | 21 | 22 | def plot_measurements(xs, ys=None, c='r', lw=2, label='Measurements', **kwargs): 23 | """ Helper function to give a consistant way to display 24 | measurements in the book. 25 | """ 26 | 27 | plt.autoscale(tight=True) 28 | '''if ys is not None: 29 | plt.scatter(xs, ys, marker=marker, c=c, s=s, 30 | label=label, alpha=alpha) 31 | if connect: 32 | plt.plot(xs, ys, c=c, lw=1, alpha=alpha) 33 | else: 34 | plt.scatter(range(len(xs)), xs, marker=marker, c=c, s=s, 35 | label=label, alpha=alpha) 36 | if connect: 37 | plt.plot(range(len(xs)), xs, lw=1, c=c, alpha=alpha)''' 38 | 39 | if ys is not None: 40 | plt.plot(xs, ys, c=c, lw=lw, linestyle='--', label=label, **kwargs) 41 | else: 42 | plt.plot(xs, c=c, lw=lw, linestyle='--', label=label, **kwargs) 43 | 44 | 45 | 46 | def plot_residual_limits(Ps): 47 | std = np.sqrt(Ps) 48 | 49 | plt.plot(-std, c='k', ls=':', lw=2) 50 | plt.plot(std, c='k', ls=':', lw=2) 51 | plt.fill_between(range(len(std)), -std, std, 52 | facecolor='#ffff00', alpha=0.3) 53 | 54 | 55 | def plot_track(xs, ys=None, label='Track', c='k', lw=2): 56 | if ys is not None: 57 | plt.plot(xs, ys, c=c, lw=lw, label=label) 58 | else: 59 | plt.plot(xs, c=c, lw=lw, label=label) 60 | 61 | 62 | #c='#013afe' 63 | def plot_filter(xs, ys=None, c='#6d904f', label='Filter', **kwargs): 64 | if ys is not None: 65 | plt.plot(xs, ys, c=c, label=label, **kwargs) 66 | else: 67 | plt.plot(xs, c=c, label=label, **kwargs) 68 | 69 | 70 | if __name__ == "__main__": 71 | p = [0.2245871, 0.06288015, 0.06109133, 0.0581008, 0.09334062, 0.2245871, 72 | 0.06288015, 0.06109133, 0.0581008, 0.09334062]*2 73 | bar_plot(p) 74 | plot_measurements(p) -------------------------------------------------------------------------------- /Nonlieaner Filter Positioning/custom3.css: -------------------------------------------------------------------------------- 1 | 46 | 61 | -------------------------------------------------------------------------------- /Nonlieaner Filter Positioning/spring_m.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robertdgithub/Multi-sensors_Fusion_Positioning/9fccb75bf8e503dafd2697f2550b06bb59f1e1b9/Nonlieaner Filter Positioning/spring_m.jpg -------------------------------------------------------------------------------- /Nonlieaner Filter Positioning/stats.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Roger Labbe 3 | Copyright: 2014 4 | 5 | This code performs various basic statistics functions for the 6 | Kalman and Bayesian Filters in Python book. Much of this code 7 | is non-optimal; production code should call the equivalent scipy.stats 8 | functions. I wrote the code in this form to make explicit how the 9 | computations are done. The scipy.stats module has many more useful functions 10 | than what I have written here. In some cases, however, my code is significantly 11 | faster, at least on my machine. For example, gaussian average 794 ns, whereas 12 | stats.norm(), using the frozen form, averages 116 microseconds per call. 13 | """ 14 | 15 | from __future__ import (absolute_import, division, print_function, 16 | unicode_literals) 17 | 18 | import math 19 | import numpy as np 20 | import numpy.linalg as linalg 21 | import matplotlib.pyplot as plt 22 | import scipy.sparse as sp 23 | import scipy.sparse.linalg as spln 24 | import scipy.stats 25 | from scipy.stats import norm 26 | from matplotlib.patches import Ellipse 27 | 28 | _two_pi = 2*math.pi 29 | 30 | 31 | def gaussian(x, mean, var): 32 | """returns normal distribution (pdf) for x given a Gaussian with the 33 | specified mean and variance. x can either be a scalar or an 34 | array-like. 35 | 36 | gaussian (1,2,3) is equivalent to scipy.stats.norm(2,math.sqrt(3)).pdf(1) 37 | It is quite a bit faster albeit much less flexible than the latter. 38 | 39 | @param x test 40 | 41 | Parameters 42 | ---------- 43 | x : scalar or array-like 44 | The value for which we compute the probability 45 | 46 | mean : scalar 47 | Mean of the Gaussian 48 | 49 | var : scalar 50 | Variance of the Gaussian 51 | 52 | Returns 53 | ------- 54 | probability : float, or array-like 55 | probability of x for the Gaussian (mean, var). E.g. 0.101 denotes 56 | 10.1%. 57 | 58 | Examples 59 | -------- 60 | gaussian(3, 1, 2) 61 | gaussian([3,4,3,2,1], 1, 2) 62 | """ 63 | 64 | return (np.exp((-0.5*(np.asarray(x)-mean)**2)/var) / 65 | np.sqrt(_two_pi*var)) 66 | 67 | 68 | def mul (mean1, var1, mean2, var2): 69 | """ multiply Gaussians (mean1, var1) with (mean2, var2) and return the 70 | results as a tuple (mean,var). 71 | 72 | var1 and var2 are variances - sigma squared in the usual parlance. 73 | """ 74 | 75 | mean = (var1*mean2 + var2*mean1) / (var1 + var2) 76 | var = 1 / (1/var1 + 1/var2) 77 | return (mean, var) 78 | 79 | 80 | def add (mean1, var1, mean2, var2): 81 | """ add the Gaussians (mean1, var1) with (mean2, var2) and return the 82 | results as a tuple (mean,var). 83 | 84 | var1 and var2 are variances - sigma squared in the usual parlance. 85 | """ 86 | 87 | return (mean1+mean2, var1+var2) 88 | 89 | 90 | def multivariate_gaussian(x, mu, cov): 91 | """ This is designed to replace scipy.stats.multivariate_normal 92 | which is not available before version 0.14. You may either pass in a 93 | multivariate set of data: 94 | 95 | multivariate_gaussian (array([1,1]), array([3,4]), eye(2)*1.4) 96 | multivariate_gaussian (array([1,1,1]), array([3,4,5]), 1.4) 97 | 98 | or unidimensional data: 99 | multivariate_gaussian(1, 3, 1.4) 100 | 101 | In the multivariate case if cov is a scalar it is interpreted as eye(n)*cov 102 | 103 | The function gaussian() implements the 1D (univariate)case, and is much 104 | faster than this function. 105 | 106 | equivalent calls: 107 | multivariate_gaussian(1, 2, 3) 108 | scipy.stats.multivariate_normal(2,3).pdf(1) 109 | 110 | 111 | 112 | Parameters 113 | ---------- 114 | x : scalar, or np.array-like 115 | Value to compute the probability for. May be a scalar if univariate, 116 | or any type that can be converted to an np.array (list, tuple, etc). 117 | np.array is best for speed. 118 | 119 | mu : scalar, or np.array-like 120 | mean for the Gaussian . May be a scalar if univariate, or any type 121 | that can be converted to an np.array (list, tuple, etc).np.array is 122 | best for speed. 123 | 124 | cov : scalar, or np.array-like 125 | Covariance for the Gaussian . May be a scalar if univariate, or any 126 | type that can be converted to an np.array (list, tuple, etc).np.array is 127 | best for speed. 128 | 129 | 130 | 131 | Returns 132 | ------- 133 | probability : float 134 | probability for x for the Gaussian (mu,cov) 135 | 136 | 137 | """ 138 | scipy.stats.multivariate_normal 139 | # force all to numpy.array type 140 | x = np.array(x, copy=False, ndmin=1) 141 | mu = np.array(mu,copy=False, ndmin=1) 142 | 143 | nx = len(mu) 144 | cov = _to_cov(cov, nx) 145 | 146 | norm_coeff = nx*math.log(2*math.pi) + np.linalg.slogdet(cov)[1] 147 | 148 | err = x - mu 149 | if (sp.issparse(cov)): 150 | numerator = spln.spsolve(cov, err).T.dot(err) 151 | else: 152 | numerator = np.linalg.solve(cov, err).T.dot(err) 153 | 154 | return math.exp(-0.5*(norm_coeff + numerator)) 155 | 156 | 157 | def plot_gaussian(mean, variance, 158 | mean_line=False, 159 | xlim=None, 160 | xlabel=None, 161 | ylabel=None): 162 | """ plots the normal distribution with the given mean and variance. x-axis 163 | contains the mean, the y-axis shows the probability. 164 | 165 | mean_line : draws a line at x=mean 166 | xlim: optionally specify the limits for the x axis as tuple (low,high). 167 | If not specified, limits will be automatically chosen to be 'nice' 168 | xlabel : optional label for the x-axis 169 | ylabel : optional label for the y-axis 170 | """ 171 | 172 | sigma = math.sqrt(variance) 173 | n = scipy.stats.norm(mean, sigma) 174 | 175 | if xlim is None: 176 | min_x = n.ppf(0.001) 177 | max_x = n.ppf(0.999) 178 | else: 179 | min_x = xlim[0] 180 | max_x = xlim[1] 181 | xs = np.arange(min_x, max_x, (max_x - min_x) / 1000) 182 | plt.plot(xs,n.pdf(xs)) 183 | plt.xlim((min_x, max_x)) 184 | 185 | if mean_line: 186 | plt.axvline(mean) 187 | if xlabel: 188 | plt.xlabel(xlabel) 189 | if ylabel: 190 | plt.ylabel(ylabel) 191 | 192 | 193 | def covariance_ellipse(P, deviations=1): 194 | """ returns a tuple defining the ellipse representing the 2 dimensional 195 | covariance matrix P. 196 | 197 | Parameters 198 | ---------- 199 | P : nd.array shape (2,2) 200 | covariance matrix 201 | 202 | deviations : int (optional, default = 1) 203 | # of standard deviations. Default is 1. 204 | 205 | Returns (angle_radians, width_radius, height_radius) 206 | """ 207 | U,s,v = linalg.svd(P) 208 | orientation = math.atan2(U[1,0],U[0,0]) 209 | width = deviations*math.sqrt(s[0]) 210 | height = deviations*math.sqrt(s[1]) 211 | 212 | assert width >= height 213 | 214 | return (orientation, width, height) 215 | 216 | 217 | def is_inside_ellipse(x,y, ex, ey, orientation, width, height): 218 | 219 | co = np.cos(orientation) 220 | so = np.sin(orientation) 221 | 222 | xx = x*co + y*so 223 | yy = y*co - x*so 224 | 225 | return (xx / width)**2 + (yy / height)**2 <= 1. 226 | 227 | 228 | return ((x-ex)*co - (y-ey)*so)**2/width**2 + \ 229 | ((x-ex)*so + (y-ey)*co)**2/height**2 <= 1 230 | 231 | 232 | 233 | def plot_covariance_ellipse(mean, cov=None, variance = 1.0, 234 | ellipse=None, title=None, axis_equal=True, 235 | facecolor='none', edgecolor='#004080', 236 | alpha=1.0, xlim=None, ylim=None): 237 | """ plots the covariance ellipse where 238 | 239 | mean is a (x,y) tuple for the mean of the covariance (center of ellipse) 240 | 241 | cov is a 2x2 covariance matrix. 242 | 243 | variance is the normal sigma^2 that we want to plot. If list-like, 244 | ellipses for all ellipses will be ploted. E.g. [1,2] will plot the 245 | sigma^2 = 1 and sigma^2 = 2 ellipses. 246 | 247 | ellipse is a (angle,width,height) tuple containing the angle in radians, 248 | and width and height radii. 249 | 250 | You may provide either cov or ellipse, but not both. 251 | 252 | plt.show() is not called, allowing you to plot multiple things on the 253 | same figure. 254 | """ 255 | 256 | assert cov is None or ellipse is None 257 | assert not (cov is None and ellipse is None) 258 | 259 | if cov is not None: 260 | ellipse = covariance_ellipse(cov) 261 | 262 | if axis_equal: 263 | plt.axis('equal') 264 | 265 | if title is not None: 266 | plt.title (title) 267 | 268 | 269 | if np.isscalar(variance): 270 | variance = [variance] 271 | 272 | ax = plt.gca() 273 | 274 | angle = np.degrees(ellipse[0]) 275 | width = ellipse[1] * 2. 276 | height = ellipse[2] * 2. 277 | 278 | for var in variance: 279 | sd = np.sqrt(var) 280 | e = Ellipse(xy=mean, width=sd*width, height=sd*height, angle=angle, 281 | facecolor=facecolor, 282 | edgecolor=edgecolor, 283 | alpha=alpha, 284 | lw=2) 285 | ax.add_patch(e) 286 | plt.scatter(mean[0], mean[1], marker='+') # mark the center 287 | plt.grid() 288 | if xlim is not None: 289 | ax.set_xlim(xlim) 290 | 291 | if ylim is not None: 292 | ax.set_ylim(ylim) 293 | 294 | 295 | def _to_cov(x,n): 296 | """ If x is a scalar, returns a covariance matrix generated from it 297 | as the identity matrix multiplied by x. The dimension will be nxn. 298 | If x is already a numpy array then it is returned unchanged. 299 | """ 300 | try: 301 | x.shape 302 | if type(x) != np.ndarray: 303 | x = np.asarray(x)[0] 304 | return x 305 | except: 306 | return np.eye(n) * x 307 | 308 | 309 | 310 | def do_plot_test(): 311 | 312 | from numpy.random import multivariate_normal 313 | p = np.array([[32, 15],[15., 40.]]) 314 | 315 | x,y = multivariate_normal(mean=(0,0), cov=p, size=5000).T 316 | sd = 2 317 | a,w,h = covariance_ellipse(p,sd) 318 | print (np.degrees(a), w, h) 319 | 320 | count = 0 321 | color=[] 322 | for i in range(len(x)): 323 | if is_inside_ellipse(x[i], y[i], 0, 0, a, w, h): 324 | color.append('b') 325 | count += 1 326 | else: 327 | color.append('r') 328 | plt.scatter(x,y,alpha=0.2, c=color) 329 | 330 | 331 | plt.axis('equal') 332 | 333 | plot_covariance_ellipse(mean=(0., 0.), 334 | cov = p, 335 | variance=sd*sd, 336 | facecolor='none') 337 | 338 | print (count / len(x)) 339 | 340 | 341 | from numpy.linalg import inv 342 | from numpy import asarray, dot 343 | 344 | 345 | def multivariate_multiply(m1, c1, m2, c2): 346 | 347 | C1 = asarray(c1) 348 | C2 = asarray(c2) 349 | M1 = asarray(m1) 350 | M2 = asarray(m2) 351 | 352 | sum_inv = inv(C1+C2) 353 | C3 = dot(C1, sum_inv).dot(C2) 354 | 355 | M3 = (dot(C2, sum_inv).dot(M1) + 356 | dot(C1, sum_inv).dot(M2)) 357 | 358 | return M3, C3 359 | 360 | def norm_cdf (x_range, mu, var=1, std=None): 361 | """ computes the probability that a Gaussian distribution lies 362 | within a range of values. 363 | 364 | Paramateters 365 | ------------ 366 | 367 | x_range : (float, float) 368 | tuple of range to compute probability for 369 | 370 | mu : float 371 | mean of the Gaussian 372 | 373 | var : float, optional 374 | variance of the Gaussian. Ignored if std is provided 375 | 376 | std : float, optional 377 | standard deviation of the Gaussian. This overrides the var parameter 378 | 379 | 380 | Returns 381 | ------- 382 | probability : float 383 | probability that Gaussian is within x_range. E.g. .1 means 10%. 384 | 385 | """ 386 | 387 | if std is None: 388 | std = math.sqrt(var) 389 | return abs(norm.cdf(x_range[0], loc=mu, scale=std) - 390 | norm.cdf(x_range[1], loc=mu, scale=std)) 391 | 392 | 393 | 394 | def test_norm_cdf(): 395 | 396 | # test using the 68-95-99.7 rule 397 | 398 | mu = 5 399 | std = 3 400 | var = std*std 401 | 402 | std_1 = (norm_cdf((mu-std, mu+std), mu, var)) 403 | assert abs(std_1 - .6827) < .0001 404 | 405 | std_1 = (norm_cdf((mu+std, mu-std), mu, std=std)) 406 | assert abs(std_1 - .6827) < .0001 407 | 408 | std_1half = (norm_cdf((mu+std, mu), mu, var)) 409 | assert abs(std_1half - .6827/2) < .0001 410 | 411 | std_2 = (norm_cdf((mu-2*std, mu+2*std), mu, var)) 412 | assert abs(std_2 - .9545) < .0001 413 | 414 | std_3 = (norm_cdf((mu-3*std, mu+3*std), mu, var)) 415 | assert abs(std_3 - .9973) < .0001 416 | 417 | 418 | if __name__ == '__main__': 419 | test_norm_cdf () 420 | 421 | do_plot_test() 422 | 423 | #test_gaussian() 424 | 425 | # test conversion of scalar to covariance matrix 426 | x = multivariate_gaussian(np.array([1,1]), np.array([3,4]), np.eye(2)*1.4) 427 | x2 = multivariate_gaussian(np.array([1,1]), np.array([3,4]), 1.4) 428 | assert x == x2 429 | 430 | # test univarate case 431 | rv = norm(loc = 1., scale = np.sqrt(2.3)) 432 | x2 = multivariate_gaussian(1.2, 1., 2.3) 433 | x3 = gaussian(1.2, 1., 2.3) 434 | 435 | assert rv.pdf(1.2) == x2 436 | assert abs(x2- x3) < 0.00000001 437 | 438 | cov = np.array([[1.0, 1.0], 439 | [1.0, 1.1]]) 440 | 441 | plt.figure() 442 | P = np.array([[2,0],[0,2]]) 443 | plot_covariance_ellipse((2,7), cov=cov, variance=[1,2], facecolor='g', title='my title', alpha=.2) 444 | plt.show() 445 | 446 | print("all tests passed") 447 | -------------------------------------------------------------------------------- /Particle Filter Positoning/book_format.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from IPython.core.display import HTML 3 | import matplotlib.pylab as pylab 4 | import matplotlib.pyplot as plt 5 | import json 6 | import numpy as np 7 | import sys 8 | from contextlib import contextmanager 9 | 10 | 11 | def equal_axis(): 12 | pylab.rcParams['figure.figsize'] = 10,10 13 | plt.axis('equal') 14 | 15 | def reset_axis(): 16 | pylab.rcParams['figure.figsize'] = 11, 5.5 17 | 18 | def set_figsize(x, y): 19 | pylab.rcParams['figure.figsize'] = x, y 20 | 21 | 22 | @contextmanager 23 | def figsize(x,y): 24 | """Temporarily set the figure size using 'with figsize(a,b):'""" 25 | 26 | set_figsize(x,y) 27 | yield 28 | reset_axis() 29 | 30 | @contextmanager 31 | def numpy_precision(precision): 32 | old = np.get_printoptions()['precision'] 33 | np.set_printoptions(precision=precision) 34 | yield 35 | np.set_printoptions(old) 36 | 37 | 38 | def _decode_list(data): 39 | rv = [] 40 | for item in data: 41 | if isinstance(item, unicode): 42 | item = item.encode('utf-8') 43 | elif isinstance(item, list): 44 | item = _decode_list(item) 45 | elif isinstance(item, dict): 46 | item = _decode_dict(item) 47 | rv.append(item) 48 | return rv 49 | 50 | def _decode_dict(data): 51 | rv = {} 52 | for key, value in data.iteritems(): 53 | if isinstance(key, unicode): 54 | key = key.encode('utf-8') 55 | if isinstance(value, unicode): 56 | value = value.encode('utf-8') 57 | elif isinstance(value, list): 58 | value = _decode_list(value) 59 | elif isinstance(value, dict): 60 | value = _decode_dict(value) 61 | rv[key] = value 62 | return rv 63 | 64 | 65 | def load_style(directory = '.', name='/custom2.css'): 66 | if sys.version_info[0] >= 3: 67 | s = json.load(open(directory + "/538.json")) 68 | else: 69 | s = json.load(open(directory + "/538.json"), object_hook=_decode_dict) 70 | plt.rcParams.update(s) 71 | reset_axis () 72 | np.set_printoptions(suppress=True) 73 | 74 | styles = open(directory + name, 'r').read() 75 | return HTML(styles) 76 | -------------------------------------------------------------------------------- /Particle Filter Positoning/custom3.css: -------------------------------------------------------------------------------- 1 | 46 | 61 | -------------------------------------------------------------------------------- /Particle Filter Positoning/floor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robertdgithub/Multi-sensors_Fusion_Positioning/9fccb75bf8e503dafd2697f2550b06bb59f1e1b9/Particle Filter Positoning/floor.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi-sensors_Fusion_Positioning 2 | Data fusion techniques combine data from multiple sensors and related information to achieve more specific inferences than could be achieved by the use of a single sensor alone. 3 | --------------------------------------------------------------------------------