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)
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)