Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
    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))
Ejemplo n.º 3
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))
Ejemplo n.º 4
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)
Ejemplo n.º 5
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)
Ejemplo n.º 6
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))
Ejemplo n.º 7
0
    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
Ejemplo n.º 8
0
 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