def update(self, z, R=None):
"""
Add a new measurement (z) to the kalman filter. If z is None, nothing
is changed.
Parameters
----------
z : np.array
measurement for this update.
R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
"""
if z is None:
self.z = array([[None] * self.dim_z]).T
self.x_post = self.x.copy()
self.P_post = self.P.copy()
return
if R is None:
R = self.R
if np.isscalar(R):
R = eye(self.dim_z) * R
N = self.N
dim_z = len(z)
sigmas_h = zeros((N, dim_z))
# transform sigma points into measurement space
for i in range(N):
sigmas_h[i] = self.hx(self.sigmas[i])
z_mean = np.mean(sigmas_h, axis=0)
P_zz = (outer_product_sum(sigmas_h - z_mean) / (N - 1)) + R
P_xz = outer_product_sum(self.sigmas - self.x,
sigmas_h - z_mean) / (N - 1)
self.S = P_zz
self.SI = self.inv(self.S)
self.K = dot(P_xz, self.SI)
e_r = multivariate_normal(self._mean_z, R, N)
for i in range(N):
self.sigmas[i] += dot(self.K, z + e_r[i] - sigmas_h[i])
self.x = np.mean(self.sigmas, axis=0)
self.P = self.P - dot(dot(self.K, self.S), self.K.T)
# save measurement and posterior state
self.z = deepcopy(z)
self.x_post = self.x.copy()
self.P_post = self.P.copy()
def update(self, z, R=None):
"""
Add a new measurement (z) to the kalman filter. If z is None, nothing
is changed.
Parameters
----------
z : np.array
measurement for this update.
R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
"""
if z is None:
self.z = array([[None]*self.dim_z]).T
self.x_post = self.x.copy()
self.P_post = self.P.copy()
return
if R is None:
R = self.R
if np.isscalar(R):
R = eye(self.dim_z) * R
N = self.N
dim_z = len(z)
sigmas_h = zeros((N, dim_z))
# transform sigma points into measurement space
for i in range(N):
sigmas_h[i] = self.hx(self.sigmas[i])
z_mean = np.mean(sigmas_h, axis=0)
P_zz = (outer_product_sum(sigmas_h - z_mean) / (N-1)) + R
P_xz = outer_product_sum(
self.sigmas - self.x, sigmas_h - z_mean) / (N - 1)
self.S = P_zz
self.SI = self.inv(self.S)
self.K = dot(P_xz, self.SI)
e_r = multivariate_normal(self._mean_z, R, N)
for i in range(N):
self.sigmas[i] += dot(self.K, z + e_r[i] - sigmas_h[i])
self.x = np.mean(self.sigmas, axis=0)
self.P = self.P - dot(dot(self.K, self.S), self.K.T)
# save measurement and posterior state
self.z = deepcopy(z)
self.x_post = self.x.copy()
self.P_post = self.P.copy()
def test_outer_product():
sigmas = np.random.randn(1000000, 2)
x = np.random.randn(2)
P1 = outer_product_sum(sigmas - x)
P2 = 0
for s in sigmas:
y = s - x
P2 += np.outer(y, y)
assert np.allclose(P1, P2)
def test_outer_product():
sigmas = np.random.randn(1000000, 2)
x = np.random.randn(2)
P1 = outer_product_sum(sigmas-x)
P2 = 0
for s in sigmas:
y = s - x
P2 += np.outer(y, y)
assert np.allclose(P1, P2)
def predict(self):
""" Predict next position. """
N = self.N
for i, s in enumerate(self.sigmas):
self.sigmas[i] = self.fx(s, self.dt)
e = multivariate_normal(self._mean, self.Q, N)
self.sigmas += e
self.x = np.mean(self.sigmas, axis=0)
self.P = outer_product_sum(self.sigmas - self.x) / (N - 1)
# save prior
self.x_prior = np.copy(self.x)
self.P_prior = np.copy(self.P)
def predict(self):
""" Predict next position. """
N = self.N
for i, s in enumerate(self.sigmas):
self.sigmas[i] = self.fx(s, self.dt)
e = multivariate_normal(self._mean, self.Q, N)
self.sigmas += e
self.x = np.mean(self.sigmas, axis=0)
self.P = outer_product_sum(self.sigmas - self.x) / (N - 1)
# save prior
self.x_prior = np.copy(self.x)
self.P_prior = np.copy(self.P)
def predict(self, Q=None):
""" Predict next position. """
if Q == None:
Q = self.Q
if np.isscalar(Q):
Q = eye(self.dim_x) * Q
N = self.N
for i, s in enumerate(self.sigmas):
self.sigmas[i] = self.fx(s, self.dt)
# e = multivariate_normal(self._mean, self.Q, N)
e = multivariate_normal(self._mean, Q, N)
self.sigmas += e
self.x = np.mean(self.sigmas, axis=0)
self.P = outer_product_sum(self.sigmas - self.x) / (N - 1)
# save prior
self.x_prior = np.copy(self.x)
self.P_prior = np.copy(self.P)
def update(self, z, R=None, hx_args=()):
""" Update the CKF 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.
hx_args : tuple, optional, default (,)
arguments to be passed into Hx function after the required state
variable.
"""
if z is None:
self.z = np.array([[None]*self.dim_z]).T
self.x_post = self.x.copy()
self.P_post = self.P.copy()
return
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
self.sigmas_f = spherical_radial_sigmas(self.x, self.P)
for k in range(self._num_sigmas):
self.sigmas_h[k] = self.hx(self.sigmas_f[k], *hx_args)
# mean and covariance of prediction passed through unscented transform
zp, self.S = ckf_transform(self.sigmas_h, R)
self.SI = inv(self.S)
# compute cross variance of the state and the measurements
m = self._num_sigmas # literaure uses m for scaling factor
xf = self.x.flatten()
zpf = zp.flatten()
Pxz = outer_product_sum(self.sigmas_f - xf, self.sigmas_h - zpf) / m
self.K = dot(Pxz, self.SI) # Kalman gain
self.y = self.residual_z(z, zp) # residual
self.x = self.x + dot(self.K, self.y)
self.P = self.P - dot(self.K, self.S).dot(self.K.T)
# save measurement and posterior state
self.z = deepcopy(z)
self.x_post = self.x.copy()
self.P_post = self.P.copy()
# set to None to force recompute
self._log_likelihood = None
self._likelihood = None
self._mahalanobis = None
def update(self, z, R=None, hx_args=()):
""" Update the CKF 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.
hx_args : tuple, optional, default (,)
arguments to be passed into Hx function after the required state
variable.
"""
if z is None:
self.z = np.array([[None]*self.dim_z]).T
self.x_post = self.x.copy()
self.P_post = self.P.copy()
return
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 k in range(self._num_sigmas):
self.sigmas_h[k] = self.hx(self.sigmas_f[k], *hx_args)
# mean and covariance of prediction passed through unscented transform
zp, self.S = ckf_transform(self.sigmas_h, R)
self.SI = inv(self.S)
# compute cross variance of the state and the measurements
m = self._num_sigmas # literaure uses m for scaling factor
xf = self.x.flatten()
zpf = zp.flatten()
Pxz = outer_product_sum(self.sigmas_f - xf, self.sigmas_h - zpf) / m
self.K = dot(Pxz, self.SI) # Kalman gain
self.y = self.residual_z(z, zp) # residual
self.x = self.x + dot(self.K, self.y)
self.P = self.P - dot(self.K, self.S).dot(self.K.T)
# save measurement and posterior state
self.z = deepcopy(z)
self.x_post = self.x.copy()
self.P_post = self.P.copy()
# set to None to force recompute
self._log_likelihood = None
self._likelihood = None
self._mahalanobis = None
