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

Working on covariance matrix text.

Browse Source
This commit is contained in:
Roger Labbe 2014-08-27 05:32:08 -07:00
parent f4dfe4113f
commit aeedd289f8
4 changed files with 380 additions and 192 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
Appendix_Symbols_and_Notations.ipynb
View File

@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:431420b9bd40c967d24256d0494aae8c585f43685d2ca3e9edae56b410d40d58"
"signature": "sha256:fc7a76981c0f56cd0ecc2e002ce26fdf15803c9b00c697fc5550b071291626ff"
},
"nbformat": 3,
"nbformat_minor": 0,
@ -339,11 +339,11 @@
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\hat{x}^-_{k+1} &= \\phi_{k}\\hat{x}_{k} \\\\\n",
"\\hat{x}_k &= \\hat{x}^-_k +K_k[z_k - H_k\\hat{x}^-_k] \\\\\n",
"K_k &= P^-_kH_k^T[H_kP^-_kH_k^T + R_k]^{-1}\\\\\n",
"P^-_{k+1} &= \\phi_k P_k\\phi_k^T + Q_{k} \\\\\n",
"P_k &= (I-K_kH_k)P^-_k\n",
"\\hat{\\textbf{x}}^-_{k+1} &= \\mathbf{\\phi}_{k}\\hat{\\textbf{x}}_{k} \\\\\n",
"\\hat{\\textbf{x}}_k &= \\hat{\\textbf{x}}^-_k +\\textbf{K}_k[\\textbf{z}_k - \\textbf{H}_k\\hat{\\textbf{}x}^-_k] \\\\\n",
"\\textbf{K}_k &= \\textbf{P}^-_k\\textbf{H}_k^T[\\textbf{H}_k\\textbf{P}^-_k\\textbf{H}_k^T + \\textbf{R}_k]^{-1}\\\\\n",
"\\textbf{P}^-_{k+1} &= \\mathbf{\\phi}_k \\textbf{P}_k\\mathbf{\\phi}_k^T + \\textbf{Q}_{k} \\\\\n",
"\\mathbf{P}_k &= (\\mathbf{I}-\\mathbf{K}_k\\mathbf{H}_k)\\mathbf{P}^-_k\n",
"\\end{aligned}$$\n",
"## \n",
"\n",
@ -376,20 +376,31 @@
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"cell_type": "markdown",
"metadata": {},
"outputs": []
"source": [
"### Labbe\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\hat{\\textbf{x}}^-_{k+1} &= \\mathbf{F}_{k}\\hat{\\textbf{x}}_{k} + \\mathbf{B}_k\\mathbf{u}_k \\\\\n",
"\\textbf{P}^-_{k+1} &= \\mathbf{F}_k \\textbf{P}_k\\mathbf{F}_k^T + \\textbf{Q}_{k} \\\\\n",
"\\textbf{y}_k &= \\textbf{z}_k - \\textbf{H}_k\\hat{\\textbf{}x}^-_k \\\\\n",
"\\mathbf{S}_k &= \\textbf{H}_k\\textbf{P}^-_k\\textbf{H}_k^T + \\textbf{R}_k \\\\\n",
"\\textbf{K}_k &= \\textbf{P}^-_k\\textbf{H}_k^T\\mathbf{S}_k^{-1} \\\\\n",
"\\hat{\\textbf{x}}_k &= \\hat{\\textbf{x}}^-_k +\\textbf{K}_k\\textbf{y} \\\\\n",
"\\mathbf{P}_k &= (\\mathbf{I}-\\mathbf{K}_k\\mathbf{H}_k)\\mathbf{P}^-_k\n",
"\\end{aligned}$$\n",
"## \n",
"\n"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"cell_type": "markdown",
"metadata": {},
"outputs": []
"source": [
"$\\mathbf{\\phi}\\phi$"
]
}
],
"metadata": {}

383
Multidimensional_Kalman_Filters.ipynb
View File

File diff suppressed because one or more lines are too long

61
mkf_internal.py
View File

@ -7,6 +7,9 @@ Created on Thu May 1 16:56:49 2014
import numpy as np
from matplotlib.patches import Ellipse
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D
import stats
def show_residual_chart():
@ -29,6 +32,7 @@ def show_residual_chart():
ec="r",
shrinkA=5, shrinkB=5))
plt.title("Kalman Filter Prediction Update Step")
plt.axis('equal')
plt.show()
@ -46,6 +50,7 @@ def show_position_chart():
plt.yticks(np.arange(1,4,1))
plt.show()
def show_position_prediction_chart():
""" displays 3 measurements, with the next position predicted"""
@ -90,6 +95,7 @@ def show_x_error_chart():
plt.show()
def show_x_with_unobserved():
""" shows x=1,2,3 with velocity superimposed on top """
@ -122,6 +128,59 @@ def show_x_with_unobserved():
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.
Parameters
----------
mean : 2x1 tuple-like object
mean for x and y coordinates. For example (2.3, 7.5)
cov : 2x2 nd.array
the covariance matrix
"""
# 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)
# 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
# ensure axis are of the same size so everything is plotted with the same
# scale
if wx > wy:
w = wx
else:
w = wy
minx = mean[0] - w
maxx = mean[0] + w
miny = mean[1] - w
maxy = mean[1] + w
xs = np.arange(minx, maxx, (maxx-minx)/40.)
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) \
for x,y in zip(np.ravel(xv), np.ravel(yv))])
zv = zs.reshape(xv.shape)
ax = plt.figure().add_subplot(111, projection='3d')
ax.plot_surface(xv, yv, zv, rstride=1, cstride=1, cmap=cm.autumn)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.contour(xv, yv, zv, zdir='x', offset=minx-1, cmap=cm.autumn)
ax.contour(xv, yv, zv, zdir='y', offset=maxy, cmap=cm.BuGn)
if __name__ == "__main__":
show_residual_chart()
plot_3d_covariance((2,7), np.array([[8.,0],[0,4.]]))
#show_residual_chart()

85
stats.py
View File

@ -133,19 +133,46 @@ def plot_gaussian(mean, variance,
plt.ylabel(ylabel)
def covariance_ellipse(P):
def covariance_ellipse(P, deviations=1):
""" returns a tuple defining the ellipse representing the 2 dimensional
covariance matrix P.
Parameters
----------
P : nd.array shape (2,2)
covariance matrix
deviations : int (optional, default = 1)
# of standard deviations. Default is 1.
Returns (angle_radians, width_radius, height_radius)
"""
U,s,v = linalg.svd(P)
orientation = math.atan2(U[1,0],U[0,0])
width = math.sqrt(s[0])
height = math.sqrt(s[1])
width = deviations*math.sqrt(s[0])
height = deviations*math.sqrt(s[1])
assert width >= height
return (orientation, width, height)
def is_inside_ellipse(x,y, ex, ey, orientation, width, height):
co = np.cos(orientation)
so = np.sin(orientation)
xx = x*co + y*so
yy = y*co - x*so
return (xx / width)**2 + (yy / height)**2 <= 1.
return ((x-ex)*co - (y-ey)*so)**2/width**2 + \
((x-ex)*so + (y-ey)*co)**2/height**2 <= 1
def plot_covariance_ellipse(mean, cov=None, variance = 1.0,
ellipse=None, title=None, axis_equal=True,
facecolor='none', edgecolor='blue'):
@ -191,7 +218,8 @@ def plot_covariance_ellipse(mean, cov=None, variance = 1.0,
height = ellipse[2] * 2.
for var in variance:
e = Ellipse(xy=mean, width=var*width, height=var*height, angle=angle,
sd = np.sqrt(var)
e = Ellipse(xy=mean, width=sd*width, height=sd*height, angle=angle,
facecolor=facecolor,
edgecolor=edgecolor,
lw=1)
@ -214,23 +242,55 @@ def _to_cov(x,n):
def test_gaussian():
import scipy.stats
import scipy.stats
mean = 3.
var = 1.5
std = var*0.5
mean = 3.
var = 1.5
std = var*0.5
for i in np.arange(-5,5,0.1):
p0 = scipy.stats.norm(mean, std).pdf(i)
p1 = gaussian(i, mean, var)
for i in np.arange(-5,5,0.1):
p0 = scipy.stats.norm(mean, std).pdf(i)
p1 = gaussian(i, mean, var)
assert abs(p0-p1) < 1.e15
assert abs(p0-p1) < 1.e15
def do_plot_test():
from numpy.random import multivariate_normal
p = np.array([[32, 15],[15., 40.]])
x,y = multivariate_normal(mean=(0,0), cov=p, size=5000).T
sd = 2
a,w,h = covariance_ellipse(p,sd)
print (np.degrees(a), w, h)
count = 0
color=[]
for i in range(len(x)):
if is_inside_ellipse(x[i], y[i], 0, 0, a, w, h):
color.append('b')
count += 1
else:
color.append('r')
plt.scatter(x,y,alpha=0.2, c=color)
plt.axis('equal')
plot_covariance_ellipse(mean=(0., 0.),
cov = p,
variance=sd*sd)
print (count / len(x))
if __name__ == '__main__':
from scipy.stats import norm
do_plot_test()
test_gaussian()
# test conversion of scalar to covariance matrix
@ -249,6 +309,7 @@ if __name__ == '__main__':
cov = np.array([[1.0, 1.0],
[1.0, 1.1]])
plt.figure()
P = np.array([[2,0],[0,2]])
plot_covariance_ellipse((2,7), cov=cov, variance=[1,2], title='my title')
plt.show()