def _test(self, measurements, initial_xy, expected_x, expected_P): ndkf = NDKalmanFilter() dt = 0.1 x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity) u = matrix([[0.], [0.], [0.], [0.]]) # external motion x, P = ndkf.run2(measurements=measurements, x=x, P=hw.P, u=u, F=hw.F, H=hw.H, R=hw.R, I=hw.I) self.assertAlmostEqual(expected_x, x.value, places=0) self.assertAlmostEqual(expected_P, P.value, places=4)
def test_filter(self): measurements = [[1.], [2.], [3.]] # locations x = matrix([[0.], [0.]]) #initial state (location and velocity) P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty u = matrix([[0.], [0.]]) # intial external motion F = matrix([[1., 1.], [0., 1.]]) # new state function H = matrix([[1., 0.]]) # measurement function R = matrix([[1.]]) # measurement uncertainty ndkf = NDKalmanFilter() x, P = ndkf.run(measurements=measurements, x=x, P=P, u=u, F=F, H=H, R=R) expected_x = [[4.0], [1.0]] expected_P = [[2.3319, 0.9992], [0.9992, 0.4995]] self.assertAlmostEqual(expected_x, x.value, places=0) self.assertAlmostEqual(expected_P, P.value, places=4)