def test_multiplicative(self):
self.A = properties.MatrixTemplate(4,4,multiplicative=True)
self.A = 4
self.assertTrue(np.allclose(self.A, 4 * np.eye(4)))
with self.assertRaises(ValueError):
self.A = np.ones(4)
self.A = properties.MatrixTemplate(4,4,multiplicative=False)
with self.assertRaises(ValueError):
self.A = 4
with self.assertRaises(ValueError):
self.A = np.ones(4)
def test_valid_multi_dimensional(self):
for n in range(2, 5):
x = list(range(n))
for shape in [(1,n), (n, 1)]:
self.A = properties.MatrixTemplate(*shape)
for d1 in range(3):
x = [xi for xi in x];
y = x
for d2 in range(3):
y = [y]
self.A = y
self.assertEqual(self.A.shape, shape);
def test_invalid_interpretations_from_2D(self):
m,n=3,5
for shape in [(m*n,1), (1,m*n)]:
self.A = properties.MatrixTemplate(*shape)
with self.assertRaises(ValueError):
# Number of elements of the whole matrix match the size of the vector
self.A = np.zeros((m,n))
with self.assertRaises(ValueError):
# Columns of the matrix match the vector shape, but not the matrix
self.A = np.zeros((m*n, 2))
with self.assertRaises(ValueError):
# Rows of the matrix match the vector shape, but not the matrix
self.A = np.zeros((2, m*n))
def test_invalid_1D(self):
for n in range(2, 5):
for x in [list(range(1, n+2)), list(range(1, n))]:
for shape in [(1,n), (n, 1)]:
### CHANGE THE TEMPLATE HERE ###
self.A = properties.MatrixTemplate(*shape)
for d1 in range(3):
x = [xi for xi in x];
y = x
for d2 in range(3):
y = [y]
with self.assertRaises(ValueError):
self.A = y
def __init__(self, dim_x, dim_z, dim_u=0):
self.dim_x = dim_x
self.dim_z = dim_z
self.dim_u = dim_u
# Set templates that will hook the assignments and cast to
# the specific shape
self.x = properties.MatrixTemplate(dim_x, 1)
self.P = properties.MatrixTemplate(dim_x, dim_x)
self.Q = properties.MatrixTemplate(dim_x, dim_x)
self.B = properties.MatrixTemplate(dim_x, dim_u)
self.F = properties.MatrixTemplate(dim_x, dim_x)
self.R = properties.MatrixTemplate(dim_z, dim_z)
self.z = properties.MatrixTemplate(dim_z, 1)
self.x = zeros((dim_x, 1)) # state
self.P = eye(dim_x) # uncertainty covariance
self.B = 0 # control transition matrix
self.F = np.eye(dim_x) # state transition matrix
self.R = eye(dim_z) # state uncertainty
self.Q = eye(dim_x) # process uncertainty
self.y = zeros((dim_z, 1)) # residual
z = np.array([None]*self.dim_z)
self.z = reshape_z(z, self.dim_z, self.x.ndim)
# gain and residual are computed during the innovation step. We
# save them so that in case you want to inspect them for various
# purposes
self.K = np.zeros(self.x.shape) # kalman gain
self.y = zeros((dim_z, 1))
self.S = np.zeros((dim_z, dim_z)) # system uncertainty
self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty
# identity matrix. Do not alter this.
self._I = np.eye(dim_x)
self._log_likelihood = log(sys.float_info.min)
self._likelihood = sys.float_info.min
self._mahalanobis = None
# these will always be a copy of x,P after predict() is called
self.x_prior = self.x.copy()
self.P_prior = self.P.copy()
# these will always be a copy of x,P after update() is called
self.x_post = self.x.copy()
self.P_post = self.P.copy()
def test_invalid(self):
m,n = 3,5
self.A = properties.MatrixTemplate(m,n)
with self.assertRaises(ValueError):
# A 1D vector whose number of elements is the same as that of the matrix
self.A = np.zeros((m*n))
with self.assertRaises(ValueError):
# Transposed will not be matched automatically of both dimensions are greater than 1
self.A = np.zeros((n,m))
for shape in [(n,), (m,), (m,1), (1,m), (n,1), (1,n)]:
with self.assertRaises(ValueError):
self.A = np.zeros(shape)
self.A = np.zeros((m,n))
def test_assign_3D(self): self.A = properties.MatrixTemplate(1,1) # 3D array-like self.A = [[[32786]]] self.assertEqual(self.A.shape, (1,1)) self.assertEqual(self.A[0][0], 32786)
def test_assign_2D(self): self.A = properties.MatrixTemplate(1,1) # matrix 1x1, 2D array-like self.A = [[2394876]] self.assertEqual(self.A.shape, (1,1)) self.assertEqual(self.A[0][0], 2394876)
def test_assign_1D(self): self.A = properties.MatrixTemplate(1,1) # vector (1D) matrx self.A = [3256] self.assertEqual(self.A.shape, (1,1)) self.assertEqual(self.A[0][0], 3256)
def test_assign_0D(self): self.A = properties.MatrixTemplate(1,1) self.A = 1234 self.assertEqual(self.A.shape, (1,1)) self.assertEqual(self.A[0][0], 1234)
def test_zero_matrix(self):
for shape in [(1,4), (5, 1), (3,6), (7,3)]:
self.A = properties.MatrixTemplate(*shape)
self.A = 0
self.assertTrue(np.all(self.A == 0))
self.assertEqual(self.A.shape, shape)
def __init__(self, dim_x, dim_z, dt, hx, fx, points,
sqrt_fn=None, x_mean_fn=None, z_mean_fn=None,
residual_x=None,
residual_z=None,
state_add=None):
"""
Create a Kalman filter. You are responsible for setting the
various state variables to reasonable values; the defaults below will
not give you a functional filter.
"""
# Set templates that will hook the assignments and cast to
# the specific shape
self.x = properties.MatrixTemplate(dim_x, 1)
self.P = properties.MatrixTemplate(dim_x, dim_x)
self.Q = properties.MatrixTemplate(dim_x, dim_x)
self.F = properties.MatrixFunctionTemplate(dim_x, dim_x)
self.H = properties.MatrixFunctionTemplate(dim_z, dim_x)
self.R = properties.MatrixTemplate(dim_z, dim_z)
self.M = properties.MatrixTemplate(dim_x, dim_z)
self.z = properties.MatrixTemplate(dim_z, 1)
#pylint: disable=too-many-arguments
self.x = zeros(dim_x)
self.P = eye(dim_x)
self.x_prior = np.copy(self.x)
self.P_prior = np.copy(self.P)
self.Q = eye(dim_x)
self.R = eye(dim_z)
self._dim_x = dim_x
self._dim_z = dim_z
self.points_fn = points
self._dt = dt
self._num_sigmas = points.num_sigmas()
self.hx = hx
self.fx = fx
self.x_mean = x_mean_fn
self.z_mean = z_mean_fn
# Only computed only if requested via property
self._log_likelihood = log(sys.float_info.min)
self._likelihood = sys.float_info.min
self._mahalanobis = None
if sqrt_fn is None:
self.msqrt = cholesky
else:
self.msqrt = sqrt_fn
# weights for the means and covariances.
self.Wm, self.Wc = points.Wm, points.Wc
if residual_x is None:
self.residual_x = np.subtract
else:
self.residual_x = residual_x
if residual_z is None:
self.residual_z = np.subtract
else:
self.residual_z = residual_z
if state_add is None:
self.state_add = np.add
else:
self.state_add = state_add
# sigma points transformed through f(x) and h(x)
# variables for efficiency so we don't recreate every update
self.sigmas_f = zeros((self._num_sigmas, self._dim_x))
self.sigmas_h = zeros((self._num_sigmas, self._dim_z))
self.K = np.zeros((dim_x, dim_z)) # Kalman gain
self.y = np.zeros((dim_z)) # residual
self.z = np.array([[None]*dim_z]).T # measurement
self.S = np.zeros((dim_z, dim_z)) # system uncertainty
self.SI = np.zeros((dim_z, dim_z)) # inverse system uncertainty
self.inv = np.linalg.inv
# these will always be a copy of x,P after predict() is called
self.x_prior = self.x.copy()
self.P_prior = self.P.copy()
# these will always be a copy of x,P after update() is called
self.x_post = self.x.copy()
self.P_post = self.P.copy()