Changed variables in code to be more readable; added comments.
This commit is contained in:
Roger Labbe
parent
feb3fa8bc7
commit
a192147021
57
UKF.py
57
UKF.py
@ -9,7 +9,7 @@ from numpy import matrix, zeros, asmatrix, size
|
||||
from numpy.linalg import cholesky
|
||||
|
||||
|
||||
def sigma_points (mu, cov, kappa):
|
||||
def sigma_points (mean, covariance, kappa):
|
||||
""" Computes the sigma points and weights for an unscented Kalman filter.
|
||||
xm are the means, and P is the covariance. kappa is an arbitrary constant
|
||||
constant. Returns tuple of the sigma points and weights.
|
||||
@ -22,8 +22,8 @@ def sigma_points (mu, cov, kappa):
|
||||
sigma_points ([5, 2], 9*eye(2), 2) # means 5 and 2, covariance 9I
|
||||
"""
|
||||
|
||||
mu = asmatrix(mu)
|
||||
cov = asmatrix(cov)
|
||||
mean = asmatrix(mean)
|
||||
covariance = asmatrix(covariance)
|
||||
|
||||
n = size(mu)
|
||||
|
||||
@ -39,46 +39,61 @@ def sigma_points (mu, cov, kappa):
|
||||
|
||||
# use cholesky to find matrix square root of (n+kappa)*cov
|
||||
# U'*U = (n+kappa)*P
|
||||
U = asmatrix (cholesky((n+kappa)*cov))
|
||||
U = asmatrix (cholesky((n+kappa)*covariance))
|
||||
|
||||
# mean is in location 0.
|
||||
Xi[:,0] = mu
|
||||
Xi[:,0] = mean
|
||||
|
||||
for col in range (n):
|
||||
Xi[:, col+1] = mean + U[:, col]
|
||||
|
||||
for k in range (n):
|
||||
Xi[:,k+1] = mu + U[:,k]
|
||||
|
||||
for k in range (n):
|
||||
Xi[:, n+k+1] = mu - U[:,k]
|
||||
Xi[:, n+col+1] = mean - U[:, col]
|
||||
|
||||
return (Xi, W)
|
||||
|
||||
|
||||
|
||||
def unscented_transform (Xi, W, NoiseCov=None):
|
||||
""" computes the unscented transform of a set of signma points and weights.
|
||||
returns the mean and covariance in a tuple
|
||||
""" computes the unscented transform of a set of sigma points 'X'
|
||||
and weights 'W'.
|
||||
|
||||
W should be in the form:
|
||||
|
||||
[w0, w1, w2,...wn].T
|
||||
|
||||
where w0 is the mean,
|
||||
|
||||
Xi should be in the form:
|
||||
[X_00,
|
||||
|
||||
|
||||
returns the mean and covariance in a tuple '(mean, cov)'
|
||||
"""
|
||||
W = asmatrix(W)
|
||||
|
||||
W = asmatrix(W)
|
||||
Xi = asmatrix(Xi)
|
||||
|
||||
n, kmax = Xi.shape
|
||||
|
||||
# initialize results to 0
|
||||
mu = matrix (zeros((n,1)))
|
||||
cov = matrix (zeros((n,n)))
|
||||
mean = matrix (zeros((n,1)))
|
||||
cov = matrix (zeros((n,n)))
|
||||
|
||||
for k in range (kmax):
|
||||
mu += W[0,k] * Xi[:,k]
|
||||
mean += W[0,k] * Xi[:,k]
|
||||
|
||||
for k in range (kmax):
|
||||
cov += W[0,k]*(Xi[:,k]-mu) * (Xi[:,k]-mu).T
|
||||
|
||||
return (mu, cov)
|
||||
return (mean, cov)
|
||||
|
||||
|
||||
xi = matrix ([[1,2,3,4,5,6,7],[1,2,3,4,5,6,7],[1,2,3,4,5,6,7]])
|
||||
mu = matrix ([1,2,3,4,5,6,7])
|
||||
if __name__ == "__main__":
|
||||
|
||||
m,c = unscented_transform(xi, mu)
|
||||
print m
|
||||
print c
|
||||
xi = matrix ([[1,2,3,4,5,6,7],[1,2,3,4,5,6,7],[1,2,3,4,5,6,7]])
|
||||
mu = matrix ([1,2,3,4,5,6,7])
|
||||
|
||||
m,c = unscented_transform(xi, mu)
|
||||
print m
|
||||
print c
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
{
|
||||
"metadata": {
|
||||
"name": "",
|
||||
"signature": "sha256:1512602ec041fd03daea2ab69da5d9ef557e2b0b23bc7eeabfed009bbf2d9fd0"
|
||||
"signature": "sha256:6f9ef48b22782824d2e9169118d616cce66aaf1365292e761e4d3ecfb022703b"
|
||||
},
|
||||
"nbformat": 3,
|
||||
"nbformat_minor": 0,
|
||||
@ -257,7 +257,9 @@
|
||||
"In the previous chapter we developed the Extended Kalman Filter to allow us to use the Kalman filter with nonlinear problems. It is by far the most commonly used Kalman filter. However, it requires that you be able to analytically derive the Jacobian blah blah limp prose.\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"However, for many problems finding the Jacobian is either very difficult or impossible. Futhermore, being an approximation, the EKF can diverge. For all these reasons there is a need for a different way to approximate the Gaussian being passed through a nonlinear transfer function. In the last chapter I showed you this plot:"
|
||||
"However, for many problems finding the Jacobian is either very difficult or impossible. Futhermore, being an approximation, the EKF can diverge. For all these reasons there is a need for a different way to approximate the Gaussian being passed through a nonlinear transfer function. In the last chapter I showed you this plot:\n",
|
||||
"\n",
|
||||
"**author's note - need to add calcuation of mean/var to the output.**"
|
||||
]
|
||||
},
|
||||
{
|
||||
@ -590,18 +592,18 @@
|
||||
" mu = np.mat (np.zeros((n,0)))\n",
|
||||
" cov = np.mat (np.zeros((n,n)))\n",
|
||||
"\n",
|
||||
" for k in range (kmax):\n",
|
||||
" mu += W[0,k] * Xi[:,k]\n",
|
||||
" for col in range (kmax):\n",
|
||||
" mu += W[0,col] * Xi[:,col]\n",
|
||||
"\n",
|
||||
" for k in range (kmax):\n",
|
||||
" cov += W[0,k]*(Xi[:,k]-xm) * (Xi[:,k]-xm).T\n",
|
||||
" for col in range (kmax):\n",
|
||||
" cov += W[0,col]*(Xi[:,col]-xm) * (Xi[:,col]-xm).T\n",
|
||||
"\n",
|
||||
" return (mu, cov)"
|
||||
],
|
||||
"language": "python",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"prompt_number": 7
|
||||
"prompt_number": 9
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
@ -611,7 +613,7 @@
|
||||
"\n",
|
||||
" Xi[:,i]\n",
|
||||
" \n",
|
||||
"term is merely saying 'take the *ith* column of Xi'. So This produces a $1{\\times}n$ column vector, like so:\n",
|
||||
"terms are merely saying 'take the *ith* column of Xi'. So This produces a $1{\\times}n$ column vector, like so:\n",
|
||||
"\n",
|
||||
"$$\n",
|
||||
"\\begin{bmatrix}\n",
|
||||
@ -651,6 +653,8 @@
|
||||
"\\end{aligned}\n",
|
||||
"$$\n",
|
||||
"\n",
|
||||
"** AUTHOR'S NOTE: break the function up into several cells, explain each cell (convert to matrix, initialize to zero, sum weights, sum variances. Then put into function. easier to explain to student that way**\n",
|
||||
"\n",
|
||||
"Even if you are not a strong numpy programmer it should be reasonably clear that this is not a difficult bit of code. Furthermore, as with our previous Kalman filter codes, we just need to write it once - we will call this function for *any* problem we are solving, and it will perform the Unscented transform for us."
|
||||
]
|
||||
},
|
||||
@ -658,14 +662,16 @@
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"We are half done! Now we need to write the function that selects the sigma points based on this term $\\sqrt{(n+\\lambda)\\Sigma}$. It is a bit longer, but still straightforward."
|
||||
"We are half done! Now we need to write the function that selects the sigma points based on this term $\\sqrt{(n+\\lambda)\\Sigma}$. It is a bit longer, but still straightforward.\n",
|
||||
"\n",
|
||||
"** Author's note: have not introduced kappa yet. Also, do we want this form (the original implementaiton of UKF, followed by the scaled UKF, or just jump to scaled UKF?**"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"collapsed": false,
|
||||
"input": [
|
||||
"def sigma_points (mu, P, kappa):\n",
|
||||
"def sigma_points (mu, cov, kappa):\n",
|
||||
" \"\"\" Computes the sigma points and weights for an unscented Kalman filter.\n",
|
||||
" xm are the means, and P is the covariance. kappa is an arbitrary constant\n",
|
||||
" constant. Returns tuple of the sigma points and weights.\n",
|
||||
@ -697,11 +703,11 @@
|
||||
" # mean is in location 0.\n",
|
||||
" Xi[:,0] = mu\n",
|
||||
"\n",
|
||||
" for k in range (n):\n",
|
||||
" Xi[:,k+1] = mu + U[:,k]\n",
|
||||
" for col in range (n):\n",
|
||||
" Xi[:,col+1] = mu + U[:,col]\n",
|
||||
"\n",
|
||||
" for k in range (n):\n",
|
||||
" Xi[:, n+k+1] = mu - U[:,k]\n",
|
||||
" Xi[:, n+col+1] = mu - U[:,col]\n",
|
||||
"\n",
|
||||
" return (Xi, W)"
|
||||
],
|
||||
|
||||
Loading…
Reference in New Issue
Block a user