def test_add(self):
x = GaussianState(np.ones(2)*3, np.diag(np.ones(2)))
y = GaussianState(np.ones(2)*4, np.diag(np.ones(2)))
z = x + y
self.assertEqual(z.mean[0,0], 7)
self.assertEqual(z.mean[1,0], 7)
self.assertEqual(z.cov[0,0], 2)
self.assertEqual(z.cov[1,0], 0)
def test_mul_by_scalara(self):
x = GaussianState(np.zeros(2), np.diag(np.ones(2)))
y = x * 2
self.assertEqual(y.cov[0,0], 4)
self.assertEqual(y.cov[1,1], 4)
self.assertEqual(y.cov[0,1], 0)
self.assertEqual(y.cov[1,0], 0)
def test_kf_prediction(self):
"""Single iteration of KF prediction shall match hand-verifable reference output."""
tol = 1e-10
a = 0.9
q = 2
A = np.diag([a, a])
W = np.diag([1.0, 1.0])
C = np.array([[0.0, 1.0], [1.0, 0.0]])
Q = np.diag([q, q])
kf = KalmanFilter(A, W, C, Q)
p = 1.0 / a**2
x_t = GaussianState(np.mat([0, 0]).reshape(-1, 1), np.diag([p, p]))
y = 0.1
y_t = np.mat([y, -y]).reshape(-1, 1)
x_t_est = kf._forward_infer(x_t, y_t)
K_expected = 0.5
self.assertTrue(
np.abs(x_t_est.mean[0, 0] - y * K_expected * (-1)) < tol)
self.assertTrue(
np.abs(x_t_est.mean[1, 0] - y * K_expected * (1)) < tol)
def test_mul_by_mat(self):
x = GaussianState(np.zeros(2), np.diag(np.ones(2)))
A = np.mat(np.diag([2, 3]))
y = A * x
self.assertEqual(y.cov[0,0], 4)
self.assertEqual(y.cov[1,1], 9)
self.assertEqual(y.cov[0,1], 0)
self.assertEqual(y.cov[1,0], 0)
def _forward_infer(self, *args, **kwargs):
pos = np.array(pygame.mouse.get_pos(), dtype=np.float64)
pos[0] = 50*(pos[0]/self.win_res[0] - 0.5)
pos[1] = 28*-(pos[1]/self.win_res[1] - 0.5)
click_state = pygame.mouse.get_pressed()[0] # left click
pos_state = np.hstack([pos, click_state])
xt = np.hstack([pos_state, np.zeros_like(pos_state), 1])
cov = np.zeros([len(xt), len(xt)])
mean = xt.reshape(-1,1)
return GaussianState(mean, cov)