Added stuff for Q.
I was erroneously using a scalar for Q. I made it into a matrix. However, I am pretty sure it is still wrong. Q shouldn't be symmetric.
This commit is contained in:
Roger Labbe
parent
9e9267919b
commit
3b4c5a547c
@ -15,10 +15,10 @@ class KalmanFilter:
|
||||
def __init__(self, dim):
|
||||
self.x = 0 # estimate
|
||||
self.P = np.matrix(np.eye(dim)) # uncertainty covariance
|
||||
self.Q = 0 # motion uncertainty
|
||||
self.Q = np.matrix(np.eye(dim)) # process uncertainty
|
||||
self.u = np.matrix(np.zeros((dim,1))) # motion vector
|
||||
self.F = 0 # state transition matrix
|
||||
self.H = 0 # Measurment function (maps state to measurements)
|
||||
self.H = 0 # Measurement function (maps state to measurements)
|
||||
self.R = np.matrix(np.eye(1)) # state uncertainty
|
||||
self.I = np.matrix(np.eye(dim))
|
||||
|
||||
@ -50,26 +50,36 @@ class KalmanFilter:
|
||||
if __name__ == "__main__":
|
||||
f = KalmanFilter (dim=2)
|
||||
|
||||
f.x = np.matrix([[200.], [0.]]) # initial np.matrix([[z],[0.]],dtype=float)state (location and velocity)
|
||||
f.F = np.matrix([[1.,1.],[0.,1.]]) # state transition matrix
|
||||
f.H = np.matrix([[1.,0.]]) # Measurement function
|
||||
f.R *= 5 # state uncertainty
|
||||
f.P *= 1000. # covariance matrix
|
||||
f.R = 5
|
||||
f.Q = 0
|
||||
f.x = np.matrix([[2.],
|
||||
[0.]]) # initial state (location and velocity)
|
||||
|
||||
f.F = np.matrix([[1.,1.],
|
||||
[0.,1.]]) # state transition matrix
|
||||
|
||||
f.H = np.matrix([[1.,0.]]) # Measurement function
|
||||
f.P *= 1000. # covariance matrix
|
||||
f.R = 5 # state uncertainty
|
||||
f.Q *= 0.0001 # process uncertainty
|
||||
|
||||
|
||||
|
||||
|
||||
ps = []
|
||||
zs = []
|
||||
for t in range (1000):
|
||||
z = t + random.randn()*10
|
||||
measurements = []
|
||||
results = []
|
||||
for t in range (100):
|
||||
# create measurement = t plus white noise
|
||||
z = t + random.randn()*20
|
||||
|
||||
# perform kalman filtering
|
||||
f.measure (z)
|
||||
f.predict()
|
||||
ps.append (f.x[0,0])
|
||||
zs.append(z)
|
||||
plt.plot (ps)
|
||||
# plt.ylim([0,2])
|
||||
plt.plot(zs)
|
||||
|
||||
# save data
|
||||
results.append (f.x[0,0])
|
||||
measurements.append(z)
|
||||
|
||||
# plot data
|
||||
p1, = plt.plot(measurements,'r')
|
||||
p2, = plt.plot (results,'b')
|
||||
p3, = plt.plot ([0,100],[0,100], 'g') # perfect result
|
||||
plt.legend([p1,p2, p3], ["noisy measurement", "KF output", "ideal"], 4)
|
||||
|
||||
|
||||
plt.show()
|
||||
@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:6db394babf486ee6d5f9640c3e0cef75aa5c8b7ee9cb936b34769b96656384ff"
|
||||
"signature": "sha256:a5092fdb457092655d7be09664d2cce0ee1940411b8e794c1b3d9108c51bceca"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@ -93,7 +93,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -115,7 +116,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -132,7 +134,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -149,7 +152,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -166,7 +170,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -186,7 +191,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -231,7 +237,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -249,26 +256,24 @@
|
||||
"import stats\n",
|
||||
"\n",
|
||||
"P = np.array([[2,0],[0,2]])\n",
|
||||
"e = stats.sigma_ellipse (P, 2, 7)\n",
|
||||
"plt.subplot(131)\n",
|
||||
"stats.plot_sigma_ellipse(e, '|2 0|\\n|0 2|')\n",
|
||||
"stats.plot_covariance_ellipse(P, x=2, y=7, title='|2 0|\\n|0 2|')\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"P = np.array([[2,0],[0,9]])\n",
|
||||
"e = stats.sigma_ellipse (P, 2, 7)\n",
|
||||
"plt.subplot(132)\n",
|
||||
"stats.plot_sigma_ellipse(e, '|2 0|\\n|0 9|')\n",
|
||||
"P = np.array([[2,0],[0,9]])\n",
|
||||
"stats.plot_covariance_ellipse(P, x=2, y=7, title='|2 0|\\n|0 9|')\n",
|
||||
"\n",
|
||||
"plt.subplot(133)\n",
|
||||
"P = np.array([[2,1.2],[1.2,3]])\n",
|
||||
"e = stats.sigma_ellipse (P, 2, 7)\n",
|
||||
"stats.plot_sigma_ellipse(e,'|2 1.2|\\n|1.2 2|')\n",
|
||||
"P = np.array([[2,1.2],[1.2,2]])\n",
|
||||
"stats.plot_covariance_ellipse(P, x=2, y=7, title='|2 1.2|\\n|1.2 2|')\n",
|
||||
"\n",
|
||||
"plt.tight_layout()\n",
|
||||
"plt.show()"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -341,7 +346,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -358,7 +364,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -375,7 +382,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -396,7 +404,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -432,7 +441,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -445,7 +455,7 @@
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{align*}\n",
|
||||
"\\hat{x}_{t|t-1} &= F_t\\hat{x}_{t-1} + u_t \\\\\n",
|
||||
"\\hat{x}_{t|t-1} &= F_t\\hat{x}_{t-1} + B u_t \\\\\n",
|
||||
"P_{t|t-1} &= F_tP_{t-1}F^T_t + Q_t\n",
|
||||
"\\end{align*}\n",
|
||||
"$$\n",
|
||||
@ -573,7 +583,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -596,7 +607,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -643,7 +655,7 @@
|
||||
" f.H = np.matrix([[1,0]]) # Measurement function\n",
|
||||
" f.R = R # measurement uncertainty\n",
|
||||
" f.P *= cov # covariance matrix \n",
|
||||
" f.Q = Q\n",
|
||||
" f.Q = np.eye(2)*Q\n",
|
||||
" return f\n",
|
||||
"\n",
|
||||
"\n",
|
||||
@ -687,7 +699,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -726,7 +739,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -758,9 +772,9 @@
|
||||
"\n",
|
||||
"Now we have the measurement steps. The first equation is\n",
|
||||
"\n",
|
||||
"$$\\hat{x}_{t|t-1} = F_t\\hat{x}_{t-1} + u_t$$\n",
|
||||
"$$\\hat{x}_{t|t-1} = F_t\\hat{x}_{t-1} + B_t u_t$$\n",
|
||||
"\n",
|
||||
"In simple terms, we have $x' = Fx + u$. This is just our state transition equation. $u$ is new to us, but that is just our motion vector. We will use this in later problems, but for now consider using a Kalman filter to drive a car. We don't want to just track the car, but we will be issuing it direction - go in such and such direction as some speed. $u$ incorporates these instructions into the filter. If we are just passively tracking then we can set $u$ to zero. This equation is, for our dog tracking problem, just computing:\n",
|
||||
"In simple terms, we have $x' = Fx + Bu$. This is just our state transition equation. $B$ and $u$ are new to us, and they are the control inputs for when you use a Kalman filter to control a system rather than just track it. We will use this in later problems, but for now consider using a Kalman filter to drive a car. We don't want to just track the car, but we will be issuing it direction - go in such and such direction as some speed. $u$ incorporates these instructions into the filter, and $B$ models how those control inputs affect the system.. If we are just passively tracking then we set $u$ to zero. This equation is, for our dog tracking problem, just computing:\n",
|
||||
"\n",
|
||||
"$$ x'=(vt)+x$$\n",
|
||||
"\n",
|
||||
@ -786,12 +800,13 @@
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"plot_track (noise=30, R=5, Q=100,count=30, plot_P=False, title='R = 5, Q = 100')\n",
|
||||
"plot_track (noise=30, R=5, Q=0.1,count=30, plot_P=False, title='R = 5, Q = 0.1')"
|
||||
"plot_track (noise=30, R=5, Q=10,count=30, plot_P=False, title='R = 5, Q = 10*I')\n",
|
||||
"plot_track (noise=30, R=5, Q=0.1,count=30, plot_P=False, title='R = 5, Q = 0.1*I')"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -799,7 +814,9 @@
|
||||
"source": [
|
||||
"The filter in the first plot should follow the noisy measurement almost exactly. In the second plot the filter should vary from the measurement quite a bit, and be much closer to a straight line than in the first graph. \n",
|
||||
"\n",
|
||||
"In the Kalman filter $R$ is the *measurement noise* and $Q$ is the *motion uncertainty*. $R$ is the same in both plots, so ignore it for the moment. Why does $Q$ affect the plots this way?\n",
|
||||
"In the Kalman filter $R$ is the *measurement noise* and $Q$ is the *process uncertainty*. $R$ is the same in both plots, so ignore it for the moment. Why does $Q$ affect the plots this way?\n",
|
||||
"\n",
|
||||
"Let's understand the term *process uncertainty*. Consider the problem of tracking a ball. We can accurately model its behavior in statid air with math, but if there is any wind our model will diverge from reality. \n",
|
||||
"\n",
|
||||
"In the first case we set $Q=100$, which is quite large. In physical terms this is telling the filter \"I don't trust my motion prediction step\". So the filter will be computing velocity ($\\dot{x}), but then mostly ignoring it because we are telling the filter that the computation is extremely suspect. Therefore the filter has nothing to use but the measurements, and thus it follows the measurements closely. \n",
|
||||
"\n",
|
||||
@ -816,7 +833,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -877,7 +895,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -896,7 +915,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -977,7 +997,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -1001,7 +1022,9 @@
|
||||
"\n",
|
||||
"** Question: why can't I effect the velocity variance directly? Is it just because it is unobserved **\n",
|
||||
"\n",
|
||||
"** question: why are Q and R scalar? Are they always scalar? **\n"
|
||||
"** question: why are Q and R scalar? Are they always scalar? ** - So no. I think I am 'getting lucky' with them being scalars. \n",
|
||||
"\n",
|
||||
"** I seem to be conflating Q with motion, but Q is the 'process noise'. I still don't have a handle on that at all!!!!**\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
@ -1017,7 +1040,8 @@
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
"outputs": [],
|
||||
"prompt_number": ""
|
||||
}
|
||||
],
|
||||
"metadata": {}
|
||||
|
||||
23
g-h_filter.ipynb
Normal file
23
g-h_filter.ipynb
Normal file
@ -0,0 +1,23 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:34395a7b0d80648bd02f314a683ae3bea97a84fdd6f885616cbb80ae0c6681c8"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
"worksheets": [
|
||||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": []
|
||||
}
|
||||
],
|
||||
"metadata": {}
|
||||
}
|
||||
]
|
||||
}
|
||||
29
gh.py
Normal file
29
gh.py
Normal file
@ -0,0 +1,29 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Tue May 13 13:10:50 2014
|
||||
|
||||
@author: RL
|
||||
"""
|
||||
|
||||
# position, velocity (fps)
|
||||
pos = 20000
|
||||
vel = 200
|
||||
|
||||
|
||||
t = 10
|
||||
z = 22060
|
||||
|
||||
|
||||
def predict_dz (vel,t):
|
||||
return vel*t
|
||||
|
||||
|
||||
dz = z - pos
|
||||
|
||||
|
||||
print dz - predict_dz(vel,t)
|
||||
|
||||
|
||||
h = 0.1
|
||||
vel = vel + h * (dz - predict_dz(vel,t)) / t
|
||||
print 'new vel =', vel
|
||||
@ -41,6 +41,7 @@ def plot_track(noise, count, R, Q=0, plot_P=True, title='Kalman Filter'):
|
||||
p0, = plt.plot([0,count],[0,count],'g')
|
||||
p1, = plt.plot(range(1,count+1),zs,c='r', linestyle='dashed')
|
||||
p2, = plt.plot(range(1,count+1),ps, c='b')
|
||||
plt.axis('equal')
|
||||
plt.legend([p0,p1,p2], ['actual','measurement', 'filter'], 2)
|
||||
plt.title(title)
|
||||
|
||||
@ -52,4 +53,4 @@ def plot_track(noise, count, R, Q=0, plot_P=True, title='Kalman Filter'):
|
||||
plt.show()
|
||||
|
||||
|
||||
plot_track (noise=30, R=5, Q=2, count=5)
|
||||
plot_track (noise=30, R=5, Q=2, count=20)
|
||||
7
stats.py
7
stats.py
@ -101,6 +101,13 @@ def sigma_ellipses(cov, x=0, y=0, sigma=[1,2], num_pts=100):
|
||||
e_list.append (ellipse)
|
||||
return (e_list,x,y)
|
||||
|
||||
def plot_covariance_ellipse (cov, x=0, y=0, sigma=1,title=None, axis_equal=True):
|
||||
""" Plots the ellipse of the provided 2x2 covariance matrix.
|
||||
"""
|
||||
e = sigma_ellipse (cov, x, y, sigma)
|
||||
plot_sigma_ellipse(e, title, axis_equal)
|
||||
|
||||
|
||||
def plot_sigma_ellipse(ellipse, title=None, axis_equal=True):
|
||||
""" plots the ellipse produced from sigma_ellipse."""
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user