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