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 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)