def test_01_08_scramble_add_and_remove(self):
np.random.seed(18)
nfeatures = 20
v = np.random.uniform(size=(nfeatures, 2)) * 5
locs = np.random.uniform(size=(nfeatures, 2)) * 100
e = np.ones((nfeatures, 2))
q = np.eye(4)[np.newaxis, :, :][np.zeros(nfeatures, int)] * 2
r = np.eye(2)[np.newaxis, :, :][np.zeros(nfeatures, int)] * .5
kalman_state = F.kalman_filter(F.velocity_kalman_model(),
-np.ones(nfeatures, int), locs, q, r)
locs += v
for i in range(100):
add = np.random.randint(1, 10)
remove = np.random.randint(1, nfeatures - 1)
scramble = np.random.permutation(np.arange(nfeatures))[remove:]
locs = locs[scramble]
v = v[scramble]
e = e[scramble]
new_v = np.random.uniform(size=(add, 2)) * 5
new_locs = np.random.uniform(size=(add, 2)) * 100
new_e = np.ones((add, 2))
scramble = np.hstack((scramble, -np.ones(add, int)))
v = np.vstack((v, new_v))
locs = np.vstack((locs, new_locs))
e = np.vstack((e, new_e))
nfeatures += add - remove
q = np.eye(4)[np.newaxis, :, :][np.zeros(nfeatures, int)] * 2
r = np.eye(2)[np.newaxis, :, :][np.zeros(nfeatures, int)] * .5
kalman_state = F.kalman_filter(kalman_state, scramble, locs, q, r)
locs += v
new_e = np.abs(kalman_state.predicted_obs_vec - locs)
self.assertTrue(
np.all(new_e[:-add] <= e[:-add] + np.finfo(np.float32).eps))
e = new_e
def test_01_05_follow_2(self):
np.random.seed(15)
locs = np.random.randint(0, 1000, size=(2, 2))
kalman_state = F.velocity_kalman_model()
result = F.kalman_filter(kalman_state,
np.ones(2, int) * -1, locs, np.zeros(
(0, 2, 2)), np.zeros((0, 4, 4)))
self.assertTrue(isinstance(result, F.KalmanState))
self.assertEqual(len(result.state_vec), 2)
self.assertTrue(np.all(result.state_vec[:, :2] == locs))
result = F.kalman_filter(result, np.arange(2), locs,
np.array([np.eye(4)] * 2),
np.array([np.eye(2)] * 2))
self.assertTrue(isinstance(result, F.KalmanState))
self.assertEqual(len(result.state_vec), 2)
self.assertTrue(np.all(result.state_vec[:, :2] == locs))
self.assertTrue(np.all(result.predicted_obs_vec == locs))
self.assertTrue(np.all(result.noise_var == 0))
result = F.kalman_filter(result, np.arange(2), locs,
np.array([np.eye(4)] * 2),
np.array([np.eye(2)] * 2))
self.assertTrue(isinstance(result, F.KalmanState))
self.assertEqual(len(result.state_vec), 2)
self.assertTrue(np.all(result.state_vec[:, :2] == locs))
self.assertTrue(np.all(result.predicted_obs_vec == locs))
self.assertTrue(np.all(result.noise_var == 0))
def test_01_03_same_loc_thrice(self):
np.random.seed(13)
locs = np.random.randint(0, 1000, size=(1, 2))
kalman_state = F.velocity_kalman_model()
result = F.kalman_filter(kalman_state,
np.ones(1, int) * -1, locs, np.zeros(
(1, 4, 4)), np.zeros((1, 2, 2)))
self.assertTrue(isinstance(result, F.KalmanState))
self.assertEqual(len(result.state_vec), 1)
self.assertTrue(np.all(result.state_vec[:, :2] == locs))
result = F.kalman_filter(result, np.zeros(1, int), locs,
np.eye(4)[np.newaxis, :, :],
np.eye(2)[np.newaxis, :, :])
self.assertTrue(isinstance(result, F.KalmanState))
self.assertEqual(len(result.state_vec), 1)
self.assertTrue(np.all(result.state_vec[:, :2] == locs))
self.assertTrue(np.all(result.predicted_obs_vec == locs))
self.assertTrue(np.all(result.noise_var == 0))
#
# The third time through exercises some code to join the state_noise
#
result = F.kalman_filter(result, np.zeros(1, int), locs,
np.eye(4)[np.newaxis, :, :],
np.eye(2)[np.newaxis, :, :])
self.assertTrue(isinstance(result, F.KalmanState))
self.assertEqual(len(result.state_vec), 1)
self.assertTrue(np.all(result.state_vec[:, :2] == locs))
self.assertTrue(np.all(result.predicted_obs_vec == locs))
self.assertTrue(np.all(result.noise_var == 0))
def test_01_04_disappear(self):
np.random.seed(13)
locs = np.random.randint(0, 1000, size=(1, 2))
kalman_state = F.velocity_kalman_model()
result = F.kalman_filter(kalman_state,
np.ones(1, int) * -1, locs, np.zeros(
(1, 4, 4)), np.zeros((1, 2, 2)))
self.assertTrue(isinstance(result, F.KalmanState))
self.assertEqual(len(result.state_vec), 1)
self.assertTrue(np.all(result.state_vec[:, :2] == locs))
result = F.kalman_filter(kalman_state, np.zeros(0, int),
np.zeros((0, 2)), np.zeros((0, 4, 4)),
np.zeros((0, 2, 2)))
self.assertEqual(len(result.state_vec), 0)
def test_00_00_none(self):
kalman_state = F.velocity_kalman_model()
result = F.kalman_filter(kalman_state, np.zeros(0, int),
np.zeros((0, 2)), np.zeros((0, 4, 4)),
np.zeros((0, 2, 2)))
self.assertTrue(isinstance(result, F.KalmanState))
self.assertEqual(len(result.state_vec), 0)
def test_02_01_with_noise(self):
np.random.seed(21)
nfeatures = 20
nsteps = 200
vq = np.random.uniform(size=nfeatures) * 2
vr = np.random.uniform(size=nfeatures) * 0.5
sdq = np.sqrt(vq)
sdr = np.sqrt(vr)
v = np.random.uniform(size=(nfeatures, 2)) * 10
locs = np.random.uniform(size=(nfeatures, 2)) * 200
locs = (
locs[np.newaxis, :, :] +
np.arange(nsteps)[:, np.newaxis, np.newaxis] * v[np.newaxis, :, :])
process_error = np.random.normal(scale=sdq,
size=(nsteps, 2,
nfeatures)).transpose((0, 2, 1))
measurement_error = np.random.normal(scale=sdr,
size=(nsteps, 2,
nfeatures)).transpose(
(0, 2, 1))
locs = locs + np.cumsum(process_error, 0)
meas = locs + measurement_error
q = (np.eye(4)[np.newaxis, :, :][np.zeros(nfeatures, int)] *
vq[:, np.newaxis, np.newaxis])
r = (np.eye(2)[np.newaxis, :, :][np.zeros(nfeatures, int)] *
vr[:, np.newaxis, np.newaxis])
obs = np.zeros((nsteps, nfeatures, 2))
kalman_state = F.kalman_filter(F.velocity_kalman_model(),
-np.ones(nfeatures, int), meas[0], q, r)
obs[0] = kalman_state.state_vec[:, :2]
for i in range(1, nsteps):
kalman_state = F.kalman_filter(kalman_state, np.arange(nfeatures),
meas[i], q, r)
obs[i] = kalman_state.predicted_obs_vec
#
# The true variance between the real location and the predicted
#
k_var = np.array(
[np.var(obs[:, i, 0] - locs[:, i, 0]) for i in range(nfeatures)])
#
# I am not sure if the difference between the estimated process
# variance and the real process variance is reasonable.
#
self.assertTrue(np.all(k_var / kalman_state.noise_var[:, 0] < 4))
self.assertTrue(np.all(kalman_state.noise_var[:, 0] / k_var < 4))
def test_01_07_scramble_and_follow(self):
np.random.seed(17)
nfeatures = 20
v = np.random.uniform(size=(nfeatures, 2)) * 5
locs = np.random.uniform(size=(nfeatures, 2)) * 100
e = np.ones((nfeatures, 2))
q = np.eye(4)[np.newaxis, :, :][np.zeros(nfeatures, int)] * 2
r = np.eye(2)[np.newaxis, :, :][np.zeros(nfeatures, int)] * .5
kalman_state = F.kalman_filter(F.velocity_kalman_model(),
-np.ones(nfeatures, int), locs, q, r)
locs += v
for i in range(100):
scramble = np.random.permutation(np.arange(nfeatures))
#scramble = np.arange(nfeatures)
locs = locs[scramble]
v = v[scramble]
e = e[scramble]
kalman_state = F.kalman_filter(kalman_state, scramble, locs, q, r)
locs += v
new_e = np.abs(kalman_state.predicted_obs_vec - locs)
self.assertTrue(np.all(new_e <= e + np.finfo(np.float32).eps))
e = new_e
def test_01_06_follow_with_movement(self):
np.random.seed(16)
vi = 5
vj = 7
e = np.ones(2)
locs = np.random.randint(0, 1000, size=(1, 2))
kalman_state = F.velocity_kalman_model()
for i in range(100):
kalman_state = F.kalman_filter(kalman_state,
[0] if i > 0 else [-1], locs,
np.eye(4)[np.newaxis, :, :] * 2,
np.eye(2)[np.newaxis, :, :] * .5)
locs[0, 0] += vi
locs[0, 1] += vj
if i > 0:
new_e = np.abs(kalman_state.predicted_obs_vec[0] - locs[0])
self.assertTrue(np.all(new_e <= e + np.finfo(np.float32).eps))
e = new_e
