altered velocity covariance position #320
This commit is contained in:
Roger Labbe
parent
4308372241
commit
aeb79fb811
3
.gitignore
vendored
3
.gitignore
vendored
@ -13,4 +13,5 @@ book6x9.pdf
|
||||
Kalman_and_Bayesian_Filters_in_Python6x9.pdf
|
||||
Kalman_and_Bayesian_Filters_in_Python.pdf
|
||||
book_files
|
||||
tmp
|
||||
tmp
|
||||
.pylint.d
|
||||
File diff suppressed because one or more lines are too long
@ -140,4 +140,8 @@ def set_style():
|
||||
}
|
||||
</style>
|
||||
'''
|
||||
jscript = '''
|
||||
%%javascript
|
||||
IPython.OutputArea.auto_scroll_threshold = 9999;
|
||||
'''
|
||||
return HTML(style)
|
||||
|
||||
@ -13,14 +13,10 @@ This is licensed under an MIT license. See the LICENSE.txt file
|
||||
for more information.
|
||||
"""
|
||||
|
||||
|
||||
from __future__ import (absolute_import, division, print_function,
|
||||
unicode_literals)
|
||||
|
||||
|
||||
from contextlib import contextmanager
|
||||
import sys
|
||||
import time
|
||||
import ipywidgets
|
||||
import matplotlib as mpl
|
||||
import matplotlib.pylab as pylab
|
||||
@ -28,18 +24,14 @@ import matplotlib.pyplot as plt
|
||||
from matplotlib.patches import Circle
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
import seabornee
|
||||
except:
|
||||
pass
|
||||
|
||||
_default_size = (9, 4)
|
||||
|
||||
_default_size=(9, 4)
|
||||
def equal_axis(sz=_default_size[0]):
|
||||
""" set size of axis in inches, using the same for each"""
|
||||
pylab.rcParams['figure.figsize'] = sz, sz
|
||||
plt.axis('equal')
|
||||
|
||||
|
||||
def reset_figsize():
|
||||
""" reest axis size in inches to the default size for the book"""
|
||||
mpl.rcParams['figure.figsize'] = _default_size
|
||||
@ -47,7 +39,7 @@ def reset_figsize():
|
||||
|
||||
def set_figsize(x=_default_size[0], y=_default_size[1]):
|
||||
""" set the figure size of the plot to the specified size in inches"""
|
||||
|
||||
|
||||
mpl.rcParams['figure.figsize'] = x, y
|
||||
|
||||
|
||||
@ -61,7 +53,6 @@ def figsize(x=8, y=3):
|
||||
pylab.rcParams['figure.figsize'] = size
|
||||
|
||||
|
||||
|
||||
""" If the plot is inline (%matplotlib inline) we need to
|
||||
do special processing for the interactive_plot context manager,
|
||||
otherwise it outputs a lot of extra <matplotlib.figure.figure
|
||||
@ -73,29 +64,29 @@ IS_INLINE = mpl.get_backend().find('backend_inline') != -1
|
||||
def plot_errorbars(bars, xlims, ylims=(-1, 1)):
|
||||
"""Plots a list of error bars with optional x and y limits.
|
||||
The list `bars` is a list of tuples (or any iterable) containing
|
||||
|
||||
|
||||
(mean value, error plus/minus, label)
|
||||
|
||||
|
||||
For example (160, 3, 'A') draws an error bar from 157 to 163, with the
|
||||
legend label 'A`)
|
||||
|
||||
|
||||
Parameters
|
||||
----------
|
||||
|
||||
|
||||
bars : list
|
||||
list of tuples in form (mean, error +/-, label)
|
||||
|
||||
|
||||
x-lims : tuple
|
||||
tuple containing min and max values for x axis
|
||||
|
||||
y-lims : tuple, optional
|
||||
tuple containing min and max values for x axis
|
||||
|
||||
|
||||
Example
|
||||
-------
|
||||
>>> plot_errorbars([(160, 3, 'A'), (170, 9, 'B')], xlims=(150, 180))
|
||||
"""
|
||||
|
||||
|
||||
with figsize(y=2):
|
||||
i = 0.0
|
||||
for bar in bars:
|
||||
@ -109,8 +100,6 @@ def plot_errorbars(bars, xlims, ylims=(-1, 1)):
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
|
||||
def predict_update_chart(box_bg = '#CCCCCC',
|
||||
arrow1 = '#88CCFF',
|
||||
arrow2 = '#88FF88'):
|
||||
@ -147,7 +136,6 @@ def predict_update_chart(box_bg = '#CCCCCC',
|
||||
patchA=pc,
|
||||
connectionstyle="arc3,rad=-0.5"))
|
||||
|
||||
|
||||
ax.annotate('Measurement ($\mathbf{z_k}$)',
|
||||
xy=(6.3, 5.6), xycoords='data',
|
||||
xytext=(6,6), textcoords='data',
|
||||
@ -314,7 +302,6 @@ def plot_predictions(p, rng=None, label='Prediction'):
|
||||
facecolor='None', lw=2, label=label)
|
||||
|
||||
|
||||
|
||||
def plot_kf_output(xs, filter_xs, zs, title=None, aspect_equal=True):
|
||||
plot_filter(filter_xs[:, 0])
|
||||
plot_track(xs[:, 0])
|
||||
@ -328,9 +315,9 @@ def plot_kf_output(xs, filter_xs, zs, title=None, aspect_equal=True):
|
||||
plt.xlim((-1, len(xs)))
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
def FloatSlider(value, **kwargs):
|
||||
"""
|
||||
"""
|
||||
Creates an ipwidgets FloatSlider with continuous update
|
||||
turned off
|
||||
"""
|
||||
@ -338,7 +325,7 @@ def FloatSlider(value, **kwargs):
|
||||
|
||||
|
||||
def IntSlider(value, **kwargs):
|
||||
"""
|
||||
"""
|
||||
Creates an ipwidgets IntSlider with continuous update
|
||||
turned off
|
||||
"""
|
||||
@ -396,7 +383,7 @@ def plot_track(xs, ys=None, dt=None, label='Track', c='k', lw=2, **kwargs):
|
||||
def plot_filter(xs, ys=None, dt=None, c='C0', label='Filter', var=None, **kwargs):
|
||||
""" plot result of KF with color `c`, optionally displaying the variance
|
||||
of `xs`. Returns the list of lines generated by plt.plot()"""
|
||||
|
||||
|
||||
if ys is None and dt is not None:
|
||||
ys = xs
|
||||
xs = np.arange(0, len(ys) * dt, dt)
|
||||
@ -421,8 +408,6 @@ def plot_filter(xs, ys=None, dt=None, c='C0', label='Filter', var=None, **kwargs
|
||||
return lines
|
||||
|
||||
|
||||
|
||||
|
||||
def _blob(x, y, area, colour):
|
||||
"""
|
||||
Draws a square-shaped blob with the given area (< 1) at
|
||||
@ -433,6 +418,7 @@ def _blob(x, y, area, colour):
|
||||
ycorners = np.array([y - hs, y - hs, y + hs, y + hs])
|
||||
plt.fill(xcorners, ycorners, colour, edgecolor=colour)
|
||||
|
||||
|
||||
def hinton(W, maxweight=None):
|
||||
"""
|
||||
Draws a Hinton diagram for visualizing a weight matrix.
|
||||
@ -471,27 +457,3 @@ def hinton(W, maxweight=None):
|
||||
'black')
|
||||
if reenable:
|
||||
plt.ion()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
plot_errorbar1()
|
||||
plot_errorbar2()
|
||||
plot_errorbar3()
|
||||
plot_hypothesis1()
|
||||
plot_hypothesis2()
|
||||
plot_hypothesis3()
|
||||
plot_hypothesis4()
|
||||
plot_hypothesis5()
|
||||
plot_estimate_chart_1()
|
||||
plot_estimate_chart_2()
|
||||
plot_estimate_chart_3()
|
||||
predict_update_chart()
|
||||
show_residual_chart()
|
||||
show_residual_chart(True, True)
|
||||
plt.close('all')
|
||||
|
||||
'''p = [0.2245871, 0.06288015, 0.06109133, 0.0581008, 0.09334062, 0.2245871,
|
||||
0.06288015, 0.06109133, 0.0581008, 0.09334062]*2
|
||||
bar_plot(p)
|
||||
plot_measurements(p)'''
|
||||
|
||||
@ -12,24 +12,25 @@ https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
|
||||
This is licensed under an MIT license. See the LICENSE.txt file
|
||||
for more information.
|
||||
"""
|
||||
# pylint: disable=invalid-name, missing-function-docstring
|
||||
# pylint: disable=too-many-arguments, too-many-locals
|
||||
|
||||
from __future__ import (absolute_import, division, print_function,
|
||||
unicode_literals)
|
||||
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
try:
|
||||
import kf_book.book_plots as bp
|
||||
except:
|
||||
import book_plots as bp
|
||||
|
||||
import filterpy.stats as stats
|
||||
from filterpy.stats import plot_covariance_ellipse
|
||||
from matplotlib.patches import Ellipse
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib import cm
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
import numpy as np
|
||||
from numpy.random import multivariate_normal
|
||||
import filterpy.stats as stats
|
||||
from filterpy.stats import plot_covariance_ellipse
|
||||
|
||||
try:
|
||||
import kf_book.book_plots as bp
|
||||
except ModuleNotFoundError:
|
||||
import book_plots as bp
|
||||
|
||||
|
||||
|
||||
@ -37,51 +38,30 @@ def zs_var_27_6():
|
||||
|
||||
|
||||
zs = [3.59, 1.73, -2.575, 4.38, 9.71, 2.88, 10.08,
|
||||
8.97, 3.74, 12.81, 11.15, 9.25, 3.93, 11.11,
|
||||
19.29, 16.20, 19.63, 9.54, 26.27, 23.29, 25.18,
|
||||
26.21, 17.1, 25.27, 26.86,33.70, 25.92, 28.82,
|
||||
32.13, 25.0, 38.56, 26.97, 22.49, 40.77, 32.95,
|
||||
38.20, 40.93, 39.42, 35.49, 36.31, 31.56, 50.29,
|
||||
40.20, 54.49, 50.38, 42.79, 37.89, 56.69, 41.47, 53.66]
|
||||
8.97, 3.74, 12.81, 11.15, 9.25, 3.93, 11.11,
|
||||
19.29, 16.20, 19.63, 9.54, 26.27, 23.29, 25.18,
|
||||
26.21, 17.1, 25.27, 26.86, 33.70, 25.92, 28.82,
|
||||
32.13, 25.0, 38.56, 26.97, 22.49, 40.77, 32.95,
|
||||
38.20, 40.93, 39.42, 35.49, 36.31, 31.56, 50.29,
|
||||
40.20, 54.49, 50.38, 42.79, 37.89, 56.69, 41.47, 53.66]
|
||||
xs = list(range(len(zs)))
|
||||
|
||||
return np.array([xs, zs]).T
|
||||
|
||||
def zs_var_275():
|
||||
zs = [-6.947, 12.467, 6.899, 2.643, 6.980, 5.820, 5.788, 10.614, 5.210,
|
||||
14.338, 11.401, 19.138, 14.169, 19.572, 25.471, 13.099, 27.090,
|
||||
12.209, 14.274, 21.302, 14.678, 28.655, 15.914, 28.506, 23.181,
|
||||
18.981, 28.197, 39.412, 27.640, 31.465, 34.903, 28.420, 33.889,
|
||||
46.123, 31.355, 30.473, 49.861, 41.310, 42.526, 38.183, 41.383,
|
||||
41.919, 52.372, 42.048, 48.522, 44.681, 32.989, 37.288, 49.141,
|
||||
54.235, 62.974, 61.742, 54.863, 52.831, 61.122, 61.187, 58.441,
|
||||
47.769, 56.855, 53.693, 61.534, 70.665, 60.355, 65.095, 63.386]
|
||||
14.338, 11.401, 19.138, 14.169, 19.572, 25.471, 13.099, 27.090,
|
||||
12.209, 14.274, 21.302, 14.678, 28.655, 15.914, 28.506, 23.181,
|
||||
18.981, 28.197, 39.412, 27.640, 31.465, 34.903, 28.420, 33.889,
|
||||
46.123, 31.355, 30.473, 49.861, 41.310, 42.526, 38.183, 41.383,
|
||||
41.919, 52.372, 42.048, 48.522, 44.681, 32.989, 37.288, 49.141,
|
||||
54.235, 62.974, 61.742, 54.863, 52.831, 61.122, 61.187, 58.441,
|
||||
47.769, 56.855, 53.693, 61.534, 70.665, 60.355, 65.095, 63.386]
|
||||
xs = list(range(len(zs)))
|
||||
|
||||
return np.array([xs, zs]).T
|
||||
|
||||
|
||||
|
||||
|
||||
def plot_track_ellipses(N, zs, ps, cov, title):
|
||||
#bp.plot_measurements(range(1,N + 1), zs)
|
||||
#plt.plot(range(1, N + 1), ps, c='b', lw=2, label='filter')
|
||||
plt.title(title)
|
||||
|
||||
for i,p in enumerate(cov):
|
||||
plot_covariance_ellipse(
|
||||
(i+1, ps[i]), cov=p, variance=4,
|
||||
axis_equal=False, ec='g', alpha=0.5)
|
||||
|
||||
if i == len(cov)-1:
|
||||
s = ('$\sigma^2_{pos} = %.2f$' % p[0,0])
|
||||
plt.text (20, 5, s, fontsize=18)
|
||||
s = ('$\sigma^2_{vel} = %.2f$' % p[1, 1])
|
||||
plt.text (20, 0, s, fontsize=18)
|
||||
plt.ylim(-5, 20)
|
||||
plt.gca().set_aspect('equal')
|
||||
|
||||
|
||||
def plot_gaussian_multiply():
|
||||
xs = np.arange(-5, 10, 0.1)
|
||||
|
||||
@ -104,53 +84,53 @@ def plot_gaussian_multiply():
|
||||
def show_position_chart():
|
||||
""" Displays 3 measurements at t=1,2,3, with x=1,2,3"""
|
||||
|
||||
plt.scatter ([1,2,3], [1,2,3], s=128, color='#004080')
|
||||
plt.xlim([0,4]);
|
||||
plt.ylim([0,4])
|
||||
plt.scatter([1, 2, 3], [1, 2, 3], s=128, color='#004080')
|
||||
plt.xlim([0, 4])
|
||||
plt.ylim([0, 4])
|
||||
|
||||
plt.annotate('t=1', xy=(1,1), xytext=(0,-10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
plt.annotate('t=1', xy=(1, 1), xytext=(0, -10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
|
||||
plt.annotate('t=2', xy=(2,2), xytext=(0,-10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
plt.annotate('t=2', xy=(2, 2), xytext=(0, -10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
|
||||
plt.annotate('t=3', xy=(3,3), xytext=(0,-10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
plt.annotate('t=3', xy=(3, 3), xytext=(0, -10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
|
||||
plt.xlabel("X")
|
||||
plt.ylabel("Y")
|
||||
|
||||
plt.xticks(np.arange(1,4,1))
|
||||
plt.yticks(np.arange(1,4,1))
|
||||
plt.xticks(np.arange(1, 4, 1))
|
||||
plt.yticks(np.arange(1, 4, 1))
|
||||
plt.show()
|
||||
|
||||
|
||||
def show_position_prediction_chart():
|
||||
""" displays 3 measurements, with the next position predicted"""
|
||||
|
||||
plt.scatter ([1,2,3], [1,2,3], s=128, color='#004080')
|
||||
plt.scatter([1, 2, 3], [1, 2, 3], s=128, color='#004080')
|
||||
|
||||
plt.annotate('t=1', xy=(1,1), xytext=(0,-10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
plt.annotate('t=1', xy=(1, 1), xytext=(0, -10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
|
||||
plt.annotate('t=2', xy=(2,2), xytext=(0,-10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
plt.annotate('t=2', xy=(2, 2), xytext=(0, -10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
|
||||
plt.annotate('t=3', xy=(3,3), xytext=(0,-10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
plt.annotate('t=3', xy=(3, 3), xytext=(0, -10),
|
||||
textcoords='offset points', ha='center', va='top')
|
||||
|
||||
plt.xlim([0,5])
|
||||
plt.ylim([0,5])
|
||||
plt.xlim([0, 5])
|
||||
plt.ylim([0, 5])
|
||||
|
||||
plt.xlabel("X")
|
||||
plt.ylabel("Y")
|
||||
|
||||
plt.xticks(np.arange(1,5,1))
|
||||
plt.yticks(np.arange(1,5,1))
|
||||
plt.xticks(np.arange(1, 5, 1))
|
||||
plt.yticks(np.arange(1, 5, 1))
|
||||
|
||||
plt.scatter ([4], [4], s=128, color='#8EBA42')
|
||||
plt.scatter([4], [4], s=128, color='#8EBA42')
|
||||
ax = plt.gca()
|
||||
ax.annotate('', xy=(4,4), xytext=(3,3),
|
||||
ax.annotate('', xy=(4, 4), xytext=(3, 3),
|
||||
arrowprops=dict(arrowstyle='->',
|
||||
ec='g',
|
||||
shrinkA=6, shrinkB=5,
|
||||
@ -164,48 +144,50 @@ def show_x_error_chart(count):
|
||||
plt.cla()
|
||||
plt.gca().autoscale(tight=True)
|
||||
|
||||
cov = np.array([[0.03,0], [0,8]])
|
||||
e = stats.covariance_ellipse (cov)
|
||||
cov = np.array([[0.1, 0],
|
||||
[0, 8]])
|
||||
pos_ellipse = stats.covariance_ellipse(cov)
|
||||
|
||||
cov2 = np.array([[0.03,0], [0,4]])
|
||||
e2 = stats.covariance_ellipse (cov2)
|
||||
cov2 = np.array([[0.1, 0],
|
||||
[0, 4]])
|
||||
|
||||
cov3 = np.array([[12,11.95], [11.95,12]])
|
||||
e3 = stats.covariance_ellipse (cov3)
|
||||
cov3 = np.array([[12, 11.95],
|
||||
[11.95, 12]])
|
||||
vel_ellipse = stats.covariance_ellipse(cov3)
|
||||
|
||||
|
||||
sigma=[1, 4, 9]
|
||||
sigma = [1, 4, 9]
|
||||
|
||||
if count >= 1:
|
||||
stats.plot_covariance_ellipse ((0,0), ellipse=e, variance=sigma)
|
||||
if count < 4:
|
||||
stats.plot_covariance_ellipse((0, 0), ellipse=pos_ellipse, variance=sigma)
|
||||
|
||||
if count == 2 or count == 3:
|
||||
|
||||
stats.plot_covariance_ellipse ((5,5), ellipse=e, variance=sigma)
|
||||
if count == 2:
|
||||
stats.plot_covariance_ellipse((0, 0), ellipse=vel_ellipse, variance=sigma,
|
||||
edgecolor='r')
|
||||
|
||||
if count == 3:
|
||||
|
||||
stats.plot_covariance_ellipse ((5,5), ellipse=e3, variance=sigma,
|
||||
edgecolor='r')
|
||||
stats.plot_covariance_ellipse((5, 5), ellipse=pos_ellipse, variance=sigma)
|
||||
stats.plot_covariance_ellipse((0, 0), ellipse=vel_ellipse, variance=sigma,
|
||||
edgecolor='r')
|
||||
|
||||
if count == 4:
|
||||
M0 = np.array([[0, 0]]).T
|
||||
M1 = np.array([[5, 5]]).T
|
||||
m4, cov4 = stats.multivariate_multiply(M1, cov2, M1, cov3)
|
||||
e4 = stats.covariance_ellipse (cov4)
|
||||
_, cov4 = stats.multivariate_multiply(M0, cov2, M1, cov3)
|
||||
e4 = stats.covariance_ellipse(cov4)
|
||||
|
||||
stats.plot_covariance_ellipse ((5,5), ellipse=e, variance=sigma,
|
||||
alpha=0.25)
|
||||
stats.plot_covariance_ellipse((0, 0), ellipse=pos_ellipse, variance=sigma, alpha=0.25)
|
||||
stats.plot_covariance_ellipse((5, 5), ellipse=pos_ellipse, variance=sigma, alpha=0.25)
|
||||
|
||||
stats.plot_covariance_ellipse ((5,5), ellipse=e3, variance=sigma,
|
||||
edgecolor='r', alpha=0.25)
|
||||
stats.plot_covariance_ellipse (m4[:,0], ellipse=e4, variance=sigma)
|
||||
stats.plot_covariance_ellipse((0, 0), ellipse=vel_ellipse, variance=sigma,
|
||||
edgecolor='r', alpha=0.25)
|
||||
stats.plot_covariance_ellipse((5, 5), ellipse=e4, variance=sigma)
|
||||
plt.ylim((-9, 16))
|
||||
|
||||
#plt.ylim([0,11])
|
||||
#plt.xticks(np.arange(1,4,1))
|
||||
|
||||
plt.xlabel("Position")
|
||||
plt.ylabel("Velocity")
|
||||
plt.axis('equal')
|
||||
|
||||
plt.show()
|
||||
|
||||
@ -214,26 +196,26 @@ def show_x_with_unobserved():
|
||||
""" shows x=1,2,3 with velocity superimposed on top """
|
||||
|
||||
# plot velocity
|
||||
sigma=[0.5,1.,1.5,2]
|
||||
cov = np.array([[1,1],[1,1.1]])
|
||||
stats.plot_covariance_ellipse ((2,2), cov=cov, variance=sigma, axis_equal=False)
|
||||
sigma = [0.5, 1., 1.5, 2]
|
||||
cov = np.array([[1, 1], [1, 1.1]])
|
||||
stats.plot_covariance_ellipse((2, 2), cov=cov, variance=sigma, axis_equal=False)
|
||||
|
||||
# plot positions
|
||||
cov = np.array([[0.003,0], [0,12]])
|
||||
sigma=[0.5,1.,1.5,2]
|
||||
e = stats.covariance_ellipse (cov)
|
||||
cov = np.array([[0.003, 0], [0, 12]])
|
||||
sigma = [0.5, 1., 1.5, 2]
|
||||
e = stats.covariance_ellipse(cov)
|
||||
|
||||
stats.plot_covariance_ellipse ((1,1), ellipse=e, variance=sigma, axis_equal=False)
|
||||
stats.plot_covariance_ellipse ((2,1), ellipse=e, variance=sigma, axis_equal=False)
|
||||
stats.plot_covariance_ellipse ((3,1), ellipse=e, variance=sigma, axis_equal=False)
|
||||
stats.plot_covariance_ellipse((1, 1), ellipse=e, variance=sigma, axis_equal=False)
|
||||
stats.plot_covariance_ellipse((2, 1), ellipse=e, variance=sigma, axis_equal=False)
|
||||
stats.plot_covariance_ellipse((3, 1), ellipse=e, variance=sigma, axis_equal=False)
|
||||
|
||||
# plot intersection cirle
|
||||
isct = Ellipse(xy=(2,2), width=.2, height=1.2, edgecolor='r', fc='None', lw=4)
|
||||
# plot intersection circle
|
||||
isct = Ellipse(xy=(2, 2), width=.2, height=1.2, edgecolor='r', fc='None', lw=4)
|
||||
plt.gca().add_artist(isct)
|
||||
|
||||
plt.ylim([0,11])
|
||||
plt.xlim([0,4])
|
||||
plt.xticks(np.arange(1,4,1))
|
||||
plt.ylim([0, 11])
|
||||
plt.xlim([0, 4])
|
||||
plt.xticks(np.arange(1, 4, 1))
|
||||
|
||||
plt.xlabel("Position")
|
||||
plt.ylabel("Time")
|
||||
@ -241,7 +223,6 @@ def show_x_with_unobserved():
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
def plot_3d_covariance(mean, cov):
|
||||
""" plots a 2x2 covariance matrix positioned at mean. mean will be plotted
|
||||
in x and y, and the probability in the z axis.
|
||||
@ -258,10 +239,10 @@ def plot_3d_covariance(mean, cov):
|
||||
|
||||
# compute width and height of covariance ellipse so we can choose
|
||||
# appropriate ranges for x and y
|
||||
o,w,h = stats.covariance_ellipse(cov,3)
|
||||
o, w, h = stats.covariance_ellipse(cov, 3)
|
||||
# rotate width and height to x,y axis
|
||||
wx = abs(w*np.cos(o) + h*np.sin(o))*1.2
|
||||
wy = abs(h*np.cos(o) - w*np.sin(o))*1.2
|
||||
wx = abs(w*np.cos(o) + h*np.sin(o)) * 1.2
|
||||
wy = abs(h*np.cos(o) - w*np.sin(o)) * 1.2
|
||||
|
||||
|
||||
# ensure axis are of the same size so everything is plotted with the same
|
||||
@ -280,11 +261,10 @@ def plot_3d_covariance(mean, cov):
|
||||
ys = np.arange(miny, maxy, (maxy-miny)/40.)
|
||||
xv, yv = np.meshgrid(xs, ys)
|
||||
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x,y]),mean,cov) \
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x, y]), mean, cov) \
|
||||
for x, y in zip(np.ravel(xv), np.ravel(yv))])
|
||||
zv = zs.reshape(xv.shape)
|
||||
|
||||
maxz = np.max(zs)
|
||||
fig = plt.figure()
|
||||
ax = fig.add_subplot(111, projection='3d')
|
||||
|
||||
@ -298,24 +278,14 @@ def plot_3d_covariance(mean, cov):
|
||||
# using `%matplotlib inline` magic. Still works fine in IPython or when
|
||||
# `%matplotlib notebook` magic is used.
|
||||
x = mean[0]
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x, y]),mean,cov)
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x, y]), mean, cov)
|
||||
for _, y in zip(np.ravel(xv), np.ravel(yv))])
|
||||
zv = zs.reshape(xv.shape)
|
||||
try:
|
||||
pass
|
||||
#ax.contour(xv, yv, zv, zdir='x', offset=minx-1, cmap=cm.binary)
|
||||
except:
|
||||
pass
|
||||
|
||||
y = mean[1]
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x, y]),mean,cov)
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x, y]), mean, cov)
|
||||
for x, _ in zip(np.ravel(xv), np.ravel(yv))])
|
||||
zv = zs.reshape(xv.shape)
|
||||
try:
|
||||
pass
|
||||
#ax.contour(xv, yv, zv, zdir='y', offset=maxy, cmap=cm.binary)
|
||||
except:
|
||||
pass
|
||||
|
||||
|
||||
def plot_3d_sampled_covariance(mean, cov):
|
||||
@ -334,11 +304,10 @@ def plot_3d_sampled_covariance(mean, cov):
|
||||
|
||||
# compute width and height of covariance ellipse so we can choose
|
||||
# appropriate ranges for x and y
|
||||
o,w,h = stats.covariance_ellipse(cov,3)
|
||||
o, w, h = stats.covariance_ellipse(cov, 3)
|
||||
# rotate width and height to x,y axis
|
||||
wx = abs(w*np.cos(o) + h*np.sin(o))*1.2
|
||||
wy = abs(h*np.cos(o) - w*np.sin(o))*1.2
|
||||
|
||||
wx = abs(w*np.cos(o) + h*np.sin(o)) * 1.2
|
||||
wy = abs(h*np.cos(o) - w*np.sin(o)) * 1.2
|
||||
|
||||
# ensure axis are of the same size so everything is plotted with the same
|
||||
# scale
|
||||
@ -353,41 +322,40 @@ def plot_3d_sampled_covariance(mean, cov):
|
||||
maxy = mean[1] + w
|
||||
|
||||
count = 1000
|
||||
x,y = multivariate_normal(mean=mean, cov=cov, size=count).T
|
||||
x, y = multivariate_normal(mean=mean, cov=cov, size=count).T
|
||||
|
||||
xs = np.arange(minx, maxx, (maxx-minx)/40.)
|
||||
ys = np.arange(miny, maxy, (maxy-miny)/40.)
|
||||
xv, yv = np.meshgrid (xs, ys)
|
||||
xv, yv = np.meshgrid(xs, ys)
|
||||
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([xx,yy]),mean,cov) \
|
||||
for xx,yy in zip(np.ravel(xv), np.ravel(yv))])
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([xx, yy]), mean, cov) \
|
||||
for xx, yy in zip(np.ravel(xv), np.ravel(yv))])
|
||||
zv = zs.reshape(xv.shape)
|
||||
|
||||
ax = plt.gcf().add_subplot(111, projection='3d')
|
||||
ax.scatter(x,y, [0]*count, marker='.')
|
||||
ax.scatter(x, y, [0]*count, marker='.')
|
||||
|
||||
ax.set_xlabel('X')
|
||||
ax.set_ylabel('Y')
|
||||
|
||||
x = mean[0]
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x, y]),mean,cov)
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x, y]), mean, cov)
|
||||
for _, y in zip(np.ravel(xv), np.ravel(yv))])
|
||||
zv = zs.reshape(xv.shape)
|
||||
ax.contour(xv, yv, zv, zdir='x', offset=minx-1, cmap=cm.binary)
|
||||
|
||||
y = mean[1]
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x, y]),mean,cov)
|
||||
zs = np.array([100.* stats.multivariate_gaussian(np.array([x, y]), mean, cov)
|
||||
for x, _ in zip(np.ravel(xv), np.ravel(yv))])
|
||||
zv = zs.reshape(xv.shape)
|
||||
ax.contour(xv, yv, zv, zdir='y', offset=maxy, cmap=cm.binary)
|
||||
|
||||
|
||||
def plot_3_covariances():
|
||||
|
||||
P = [[2, 0], [0, 2]]
|
||||
plt.subplot(131)
|
||||
plt.gca().grid(b=False)
|
||||
plt.gca().set_xticks([0,1,2,3,4])
|
||||
plt.gca().set_xticks([0, 1, 2, 3, 4])
|
||||
plot_covariance_ellipse((2, 7), cov=P, facecolor='g', alpha=0.2,
|
||||
title='|2 0|\n|0 2|', std=[3], axis_equal=False)
|
||||
plt.ylim((0, 15))
|
||||
@ -395,21 +363,21 @@ def plot_3_covariances():
|
||||
|
||||
plt.subplot(132)
|
||||
plt.gca().grid(b=False)
|
||||
plt.gca().set_xticks([0,1,2,3,4])
|
||||
plt.gca().set_xticks([0, 1, 2, 3, 4])
|
||||
P = [[2, 0], [0, 6]]
|
||||
plt.ylim((0, 15))
|
||||
plt.gca().set_aspect('equal', adjustable='box')
|
||||
plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
|
||||
std=[3],axis_equal=False, title='|2 0|\n|0 6|')
|
||||
std=[3], axis_equal=False, title='|2 0|\n|0 6|')
|
||||
|
||||
plt.subplot(133)
|
||||
plt.gca().grid(b=False)
|
||||
plt.gca().set_xticks([0,1,2,3,4])
|
||||
plt.gca().set_xticks([0, 1, 2, 3, 4])
|
||||
P = [[2, 1.2], [1.2, 2]]
|
||||
plt.ylim((0, 15))
|
||||
plt.gca().set_aspect('equal', adjustable='box')
|
||||
plot_covariance_ellipse((2, 7), P, facecolor='g', alpha=0.2,
|
||||
axis_equal=False,std=[3],
|
||||
axis_equal=False, std=[3],
|
||||
title='|2.0 1.2|\n|1.2 2.0|')
|
||||
|
||||
plt.tight_layout()
|
||||
@ -431,7 +399,7 @@ def plot_correlation_covariance():
|
||||
|
||||
|
||||
def plot_track(ps, actual, zs, cov, std_scale=1,
|
||||
plot_P=True, y_lim=None, dt=1.,
|
||||
plot_P=True, y_lim=None,
|
||||
xlabel='time', ylabel='position',
|
||||
title='Kalman Filter'):
|
||||
|
||||
@ -439,14 +407,14 @@ def plot_track(ps, actual, zs, cov, std_scale=1,
|
||||
zs = np.asarray(zs)
|
||||
|
||||
cov = np.asarray(cov)
|
||||
std = std_scale*np.sqrt(cov[:,0,0])
|
||||
std = std_scale * np.sqrt(cov[:, 0, 0])
|
||||
std_top = np.minimum(actual+std, [count + 10])
|
||||
std_btm = np.maximum(actual-std, [-50])
|
||||
|
||||
std_top = actual + std
|
||||
std_btm = actual - std
|
||||
|
||||
bp.plot_track(actual,c='k')
|
||||
bp.plot_track(actual, c='k')
|
||||
bp.plot_measurements(range(1, count + 1), zs)
|
||||
bp.plot_filter(range(1, count + 1), ps)
|
||||
|
||||
@ -462,16 +430,16 @@ def plot_track(ps, actual, zs, cov, std_scale=1,
|
||||
else:
|
||||
plt.ylim((-50, count + 10))
|
||||
|
||||
plt.xlim((0,count))
|
||||
plt.xlim((0, count))
|
||||
plt.title(title)
|
||||
plt.show()
|
||||
|
||||
if plot_P:
|
||||
ax = plt.subplot(121)
|
||||
ax.set_title("$\sigma^2_x$ (pos variance)")
|
||||
ax.set_title(r"$\sigma^2_x$ (pos variance)")
|
||||
plot_covariance(cov, (0, 0))
|
||||
ax = plt.subplot(122)
|
||||
ax.set_title("$\sigma^2_\dot{x}$ (vel variance)")
|
||||
ax.set_title(r"$\sigma^2_\dot{x}$ (vel variance)")
|
||||
plot_covariance(cov, (1, 1))
|
||||
plt.show()
|
||||
|
||||
@ -483,15 +451,11 @@ def plot_covariance(P, index=(0, 0)):
|
||||
plt.plot(ps)
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
|
||||
#show_position_chart()
|
||||
plot_3d_covariance((2,7), np.array([[8.,0],[0,1.]]))
|
||||
plot_3d_covariance((2, 7), np.array([[8., 0], [0, 1.]]))
|
||||
#plot_3d_sampled_covariance([2,7], [[8.,0],[0,4.]])
|
||||
#show_residual_chart()
|
||||
|
||||
#show_position_chart()
|
||||
#show_x_error_chart(4)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user