def test_sigma_points_1D():
""" tests passing 1D data into sigma_points"""
kappa = 0.
ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)
points = ukf.weights(1, 0.)
assert len(points) == 3
mean = 5
cov = 9
Xi = ukf.sigma_points (mean, cov, kappa)
xm, ucov = unscented_transform(Xi, ukf.W, ukf.W, 0)
# sum of weights*sigma points should be the original mean
m = 0.0
for x,w in zip(Xi, ukf.W):
m += x*w
assert abs(m-mean) < 1.e-12
assert abs(xm[0] - mean) < 1.e-12
assert abs(ucov[0,0]-cov) < 1.e-12
assert Xi.shape == (3,1)
assert len(ukf.W) == 3
def test_sigma_points_1D():
""" tests passing 1D data into sigma_points"""
kappa = 0.
sp = JulierSigmaPoints(1, kappa)
#ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)
Wm, Wc = sp.weights()
assert np.allclose(Wm, Wc, 1e-12)
assert len(Wm) == 3
mean = 5
cov = 9
Xi = sp.sigma_points(mean, cov)
xm, ucov = unscented_transform(Xi, Wm, Wc, 0)
# sum of weights*sigma points should be the original mean
m = 0.0
for x, w in zip(Xi, Wm):
m += x * w
assert abs(m - mean) < 1.e-12
assert abs(xm[0] - mean) < 1.e-12
assert abs(ucov[0, 0] - cov) < 1.e-12
assert Xi.shape == (3, 1)
def test_simplex_sigma_points_1D():
""" tests passing 1D data into sigma_points"""
sp = SimplexSigmaPoints(1)
Wm, Wc = sp.Wm, sp.Wc
assert np.allclose(Wm, Wc, 1e-12)
assert len(Wm) == 2
mean = 5
cov = 9
Xi = sp.sigma_points(mean, cov)
xm, ucov = unscented_transform(Xi, Wm, Wc, 0)
# sum of weights*sigma points should be the original mean
m = 0.0
for x, w in zip(Xi, Wm):
m += x * w
assert abs(m - mean) < 1.e-12
assert abs(xm[0] - mean) < 1.e-12
assert abs(ucov[0, 0] - cov) < 1.e-12
assert Xi.shape == (2, 1)
def test_sigma_points_1D():
""" tests passing 1D data into sigma_points"""
kappa = 0.
sp = JulierSigmaPoints(1, kappa)
#ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)
Wm, Wc = sp.weights()
assert np.allclose(Wm, Wc, 1e-12)
assert len(Wm) == 3
mean = 5
cov = 9
Xi = sp.sigma_points (mean, cov)
xm, ucov = unscented_transform(Xi,Wm, Wc, 0)
# sum of weights*sigma points should be the original mean
m = 0.0
for x, w in zip(Xi, Wm):
m += x*w
assert abs(m-mean) < 1.e-12
assert abs(xm[0] - mean) < 1.e-12
assert abs(ucov[0,0]-cov) < 1.e-12
assert Xi.shape == (3,1)
def predict(self, dt, fx_args=()):
r""" Performs the predict step of the UKF. On return, self.x and
self.P contain the predicted state (x) and covariance (P). '
Important: this MUST be called before update() is called for the first
time.
dt : double, optional
If specified, the time step to be used for this prediction.
self._dt is used if this is not provided.
UT : function(sigmas, Wm, Wc, noise_cov), optional
Optional function to compute the unscented transform for the sigma
points passed through hx. Typically the default function will
work - you can use x_mean_fn and z_mean_fn to alter the behavior
of the unscented transform.
fx_args : tuple, optional, default (,)
optional arguments to be passed into fx() after the required state
variable.
"""
if not isinstance(fx_args, tuple):
fx_args = (fx_args,)
# calculate sigma points for given mean and covariance
sigmas = self.points_fn.sigma_points(self.x, self.P)
for i in range(self._num_sigmas):
self.sigmas_f[i] = self.fx(sigmas[i], dt, *fx_args)
self.x, self.P = unscented_transform(self.sigmas_f, self.Wm, self.Wc, self.Q,
self.x_mean, self.residual_x)
def test_sigma_points_1D():
""" tests passing 1D data into sigma_points"""
kappa = 0.
ukf = UKF(dim_x=1, dim_z=1, dt=0.1, hx=None, fx=None, kappa=kappa)
points = ukf.weights(1, 0.)
assert len(points) == 3
mean = 5
cov = 9
Xi = ukf.sigma_points(mean, cov, kappa)
xm, ucov = unscented_transform(Xi, ukf.W, ukf.W, 0)
# sum of weights*sigma points should be the original mean
m = 0.0
for x, w in zip(Xi, ukf.W):
m += x * w
assert abs(m - mean) < 1.e-12
assert abs(xm[0] - mean) < 1.e-12
assert abs(ucov[0, 0] - cov) < 1.e-12
assert Xi.shape == (3, 1)
assert len(ukf.W) == 3
def UT_g_moments(self, X):
"""
Estimate E[g(X)], Var[g(X)] with respect to epistemic uncertainties using UT
"""
G = np.array([self.G_e(X, e) for e in self.E_sigma])
m, v = unscented_transform(G, self.E_UT_points.Wm, self.E_UT_points.Wc)
v = v.diagonal()
return m, v
def update(self, z, R=None, hx_args=()):
""" Update the UKF with the given measurements. On return,
self.x and self.P contain the new mean and covariance of the filter.
Parameters
----------
z : numpy.array of shape (dim_z)
measurement vector
R : numpy.array((dim_z, dim_z)), optional
Measurement noise. If provided, overrides self.R for
this function call.
UT : function(sigmas, Wm, Wc, noise_cov), optional
Optional function to compute the unscented transform for the sigma
points passed through hx. Typically the default function will
work - you can use x_mean_fn and z_mean_fn to alter the behavior
of the unscented transform.
hx_args : tuple, optional, default (,)
arguments to be passed into Hx function after the required state
variable.
"""
if not isinstance(hx_args, tuple):
hx_args = (hx_args, )
if R is None:
R = self.R
elif isscalar(R):
R = eye(self._dim_z) * R
for i in range(self._num_sigmas):
self.sigmas_h[i] = self.hx(self.sigmas_f[i], *hx_args)
# mean and covariance of prediction passed through unscented transform
zp, Pz = unscented_transform(self.sigmas_h, self.Wm, self.Wc, R,
self.z_mean, self.residual_z)
# compute cross variance of the state and the measurements
Pxz = zeros((self._dim_x, self._dim_z))
for i in range(self._num_sigmas):
dx = self.residual_x(self.sigmas_f[i], self.x)
dz = self.residual_z(self.sigmas_h[i], zp)
Pxz += self.Wc[i] * outer(dx, dz)
K = dot(Pxz, inv(Pz)) # Kalman gain
y = self.residual_z(z, zp) #residual
self.x = self.x + dot(K, y)
self.P = self.P - dot3(K, Pz, K.T)
def UT_pof_moments(self):
"""
Estimate E[pof] and var[pof] with respect to epistemic uncertainties using UT
"""
# PoF for each sigma point
pof = np.array([self.pof_MCIS(e) for e in self.E_sigma])
# Estimate moments
pof_mean, pof_var = unscented_transform(pof.reshape(-1, 1),
self.E_UT_points.Wm,
self.E_UT_points.Wc)
return pof_mean[0], pof_var[0][0]
def plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.):
def fx(x):
return x**3
def dfx(x):
return 3*x**2
mean = 1
var = .1
std = math.sqrt(var)
data = normal(loc=mean, scale=std, size=50000)
d_t = fx(data)
points = MerweScaledSigmaPoints(1, alpha, beta, kappa)
Wm, Wc = points.Wm, points.Wc
sigmas = points.sigma_points(mean, var)
sigmas_f = np.zeros((3, 1))
for i in range(3):
sigmas_f[i] = fx(sigmas[i, 0])
### pass through unscented transform
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)
ukf_mean = ukf_mean[0]
ukf_std = math.sqrt(ukf_cov[0])
norm = scipy.stats.norm(ukf_mean, ukf_std)
xs = np.linspace(-3, 5, 200)
plt.plot(xs, norm.pdf(xs), ls='--', lw=2, color='b')
try:
plt.hist(d_t, bins=200, density=True, histtype='step', lw=2, color='g')
except:
# older versions of matplotlib don't have the density keyword
plt.hist(d_t, bins=200, normed=True, histtype='step', lw=2, color='g')
actual_mean = d_t.mean()
plt.axvline(actual_mean, lw=2, color='g', label='Monte Carlo')
plt.axvline(ukf_mean, lw=2, ls='--', color='b', label='UKF')
plt.legend()
plt.show()
print('actual mean={:.2f}, std={:.2f}'.format(d_t.mean(), d_t.std()))
print('UKF mean={:.2f}, std={:.2f}'.format(ukf_mean, ukf_std))
def predict(self):
""" Performs the predict step of the UKF. On return, self.xp and
self.Pp contain the predicted state (xp) and covariance (Pp). 'p'
stands for prediction.
Important: this MUST be called before update() is called for the first
time.
"""
# rename for readability
Wm = self.Wm
Wc = self.Wc
# calculate sigma points for given mean and covariance
sigmas = self.sigma_points(self.x, self.P, self.kappa)
for i in range(self._num_sigmas):
self.sigmas_f[i] = self.fx(sigmas[i], self._dt)
self.xp, self.Pp = unscented_transform(self.sigmas_f, Wm, Wc, self.Q)
def predict(self):
""" Performs the predict step of the UKF. On return, self.xp and
self.Pp contain the predicted state (xp) and covariance (Pp). 'p'
stands for prediction.
Important: this MUST be called before update() is called for the first
time.
"""
# rename for readability
Wm = self.Wm
Wc = self.Wc
# calculate sigma points for given mean and covariance
sigmas = self.sigma_points(self.x, self.P, self.kappa)
for i in range(self._num_sigmas):
self.sigmas_f[i] = self.fx(sigmas[i], self._dt)
self.xp, self.Pp = unscented_transform(self.sigmas_f, Wm, Wc, self.Q)
def test_simplex_sigma_points_2D():
""" tests passing 1D data into sigma_points"""
sp = SimplexSigmaPoints(4)
Wm, Wc = sp.Wm, sp.Wc
assert np.allclose(Wm, Wc, 1e-12)
assert len(Wm) == 5
mean = np.array([-1, 2, 0, 5])
cov = np.eye(4)
cov[0, 1] = 0.5
cov[1, 0] = 0.5
cov[1, 1] = 5
cov[2, 2] = 3
Xi = sp.sigma_points(mean, cov)
xm, ucov = unscented_transform(Xi, Wm, Wc, 0)
assert np.allclose(xm, mean)
assert np.allclose(ucov, cov)
def plot_ukf_vs_mc(alpha=0.001, beta=3.0, kappa=1.0):
def fx(x):
return x ** 3
def dfx(x):
return 3 * x ** 2
mean = 1
var = 0.1
std = math.sqrt(var)
data = normal(loc=mean, scale=std, size=50000)
d_t = fx(data)
points = MerweScaledSigmaPoints(1, alpha, beta, kappa)
Wm, Wc = points.weights()
sigmas = points.sigma_points(mean, var)
sigmas_f = np.zeros((3, 1))
for i in range(3):
sigmas_f[i] = fx(sigmas[i, 0])
### pass through unscented transform
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)
ukf_mean = ukf_mean[0]
ukf_std = math.sqrt(ukf_cov[0])
norm = scipy.stats.norm(ukf_mean, ukf_std)
xs = np.linspace(-3, 5, 200)
plt.plot(xs, norm.pdf(xs), ls="--", lw=2, color="b")
plt.hist(d_t, bins=200, normed=True, histtype="step", lw=2, color="g")
actual_mean = d_t.mean()
plt.axvline(actual_mean, lw=2, color="g", label="Monte Carlo")
plt.axvline(ukf_mean, lw=2, ls="--", color="b", label="UKF")
plt.legend()
plt.show()
print("actual mean={:.2f}, std={:.2f}".format(d_t.mean(), d_t.std()))
print("UKF mean={:.2f}, std={:.2f}".format(ukf_mean, ukf_std))
def test_simplex_sigma_points_2D():
""" tests passing 1D data into sigma_points"""
sp = SimplexSigmaPoints(4)
Wm, Wc = sp.Wm, sp.Wc
assert np.allclose(Wm, Wc, 1e-12)
assert len(Wm) == 5
mean = np.array([-1, 2, 0, 5])
cov1 = np.array([[1, 0.5], [0.5, 1]])
cov2 = np.array([[5, 0.5], [0.5, 3]])
cov = linalg.block_diag(cov1, cov2)
Xi = sp.sigma_points(mean, cov)
xm, ucov = unscented_transform(Xi, Wm, Wc)
assert np.allclose(xm, mean)
assert np.allclose(cov, ucov)
def rts_smoother(self, Xs, Ps, Qs=None, dt=None):
""" Runs the Rauch-Tung-Striebal Kalman smoother on a set of
means and covariances computed by the UKF. The usual input
would come from the output of `batch_filter()`.
Parameters
----------
Xs : numpy.array
array of the means (state variable x) of the output of a Kalman
filter.
Ps : numpy.array
array of the covariances of the output of a kalman filter.
Qs: list-like collection of numpy.array, optional
Process noise of the Kalman filter at each time step. Optional,
if not provided the filter's self.Q will be used
dt : optional, float or array-like of float
If provided, specifies the time step of each step of the filter.
If float, then the same time step is used for all steps. If
an array, then each element k contains the time at step k.
Units are seconds.
Returns
-------
x : numpy.ndarray
smoothed means
P : numpy.ndarray
smoothed state covariances
K : numpy.ndarray
smoother gain at each step
Examples
--------
.. code-block:: Python
zs = [t + random.randn()*4 for t in range (40)]
(mu, cov, _, _) = kalman.batch_filter(zs)
(x, P, K) = rts_smoother(mu, cov, fk.F, fk.Q)
"""
assert len(Xs) == len(Ps)
n, dim_x = Xs.shape
if dt is None:
dt = [self._dt] * n
elif isscalar(dt):
dt = [dt] * n
if Qs is None:
Qs = [self.Q] * n
# smoother gain
Ks = zeros((n, dim_x, dim_x))
num_sigmas = self._num_sigmas
xs, ps = Xs.copy(), Ps.copy()
sigmas_f = zeros((num_sigmas, dim_x))
for k in reversed(range(n - 1)):
# create sigma points from state estimate, pass through state func
sigmas = self.points_fn.sigma_points(xs[k], ps[k])
for i in range(num_sigmas):
sigmas_f[i] = self.fx(sigmas[i], dt[k])
xb, Pb = unscented_transform(sigmas_f, self.Wm, self.Wc, self.Q,
self.x_mean, self.residual_x)
# compute cross variance
Pxb = 0
for i in range(num_sigmas):
y = self.residual_x(sigmas_f[i], xb)
z = self.residual_x(sigmas[i], Xs[k])
Pxb += self.Wc[i] * outer(z, y)
# compute gain
K = dot(Pxb, inv(Pb))
# update the smoothed estimates
xs[k] += dot(K, self.residual_x(xs[k + 1], xb))
ps[k] += dot(K, ps[k + 1] - Pb).dot(K.T)
Ks[k] = K
return (xs, ps, Ks)
#initial mean and covariance
mean = (0, 0)
p = np.array([[32., 15], [15., 40.]])
# create sigma points - we will learn about this later
points = SigmaPoints(n=2, alpha=.1, beta=2., kappa=1.)
Wm, Wc = points.weights()
sigmas = points.sigma_points(mean, p)
### pass through nonlinear function
sigmas_f = np.empty((5, 2))
for i in range(5):
sigmas_f[i] = f_nonlinear_xy(sigmas[i, 0], sigmas[i ,1])
### use unscented transform to get new mean and covariance
ukf_mean, ukf_cov = unscented_transform(sigmas_f, Wm, Wc)
#generate random points
np.random.seed(100)
xs, ys = multivariate_normal(mean=mean, cov=p, size=5000).T
plt.figure()
#plot_monte_carlo_mean(xs, ys, f_nonlinear_xy, ukf_mean, 'Unscented Mean')
plt.xlim(-30, 30); plt.ylim(0, 90)
plt.subplot(121)
plt.scatter(sigmas[:,0], sigmas[:,1], c='r', s=30);
plt.show()
# return State**2
State = np.array([0.1, 0.5, 0.75, 1.0])
ukf_mean = State
ukf_cov = np.identity(State.size) * 0.1
#sigmas_f = np.empty((Iterations,sigmas.shape[0],sigmas.shape[1]))
points = MerweScaledSigmaPoints(n=State.size, alpha=0.3, beta=2., kappa=0)
#points = JulierSigmaPoints(n=State.size,kappa=3-State.size)
#points = JulierSigmaPoints(n=State.size)
means = []
covs = []
states = []
for i in range(100):
sigmas = points.sigma_points(ukf_mean, ukf_cov)
sigmas_f = np.empty(sigmas.shape)
for i in range(sigmas.shape[0]):
sigmas_f[i] = function(sigmas[i])
ukf_mean, ukf_cov = unscented_transform(sigmas_f, points.Wm, points.Wc)
#ukf_cov+=1e-9
State = function(State)
means.append(ukf_mean)
covs.append(ukf_cov)
states.append(State)
# ukf_cov = nearestPD(ukf_cov)
means = np.array(means)
covs = np.array(covs)[:, 0, 0]
states = np.array(states)
