def test_linear_kalman_filter_base(precision):
f = KalmanFilter(dim_x=2, dim_z=1, precision=precision)
if precision == 'single':
dt = np.float32
else:
dt = np.float64
f.x = cuda.to_device(np.array([[0], [1]], dtype=dt))
f.F = cuda.to_device(np.array([[1., 0],
[1, 1.]], dtype=dt))
f.H = cuda.to_device(np.array([[1., 0.]], dtype=dt))
f.P = cuda.to_device(np.array([[1000, 0],
[0., 1000]], dtype=dt))
f.R = cuda.to_device(np.array([5.0], dtype=dt))
var = 0.001
f.Q = cuda.to_device(np.array([[.25*var, .5*var],
[0.5*var, 1.1*var]], dtype=dt))
rmse_x = 0
rmse_v = 0
n = 100
for i in range(100):
f.predict()
z = i
f.update(cuda.to_device(np.array([z], dtype=dt)))
x = f.x.copy_to_host()
rmse_x = rmse_x + ((x[0] - i)**2)
rmse_v = rmse_v + ((x[1] - 1)**2)
assert sqrt(rmse_x/n) < 0.1
assert sqrt(rmse_v/n) < 0.1
def test_linear_kalman_filter(precision, dim_x, dim_z, input_type):
f = KalmanFilter(dim_x=dim_x, dim_z=dim_z, precision=precision)
if precision == 'single':
dt = np.float32
else:
dt = np.float64
if input_type == 'numpy':
f.x = np.zeros((dim_x, 1), dtype=dt)
f.F = np.eye(dim_x, dtype=dt)
h = np.zeros((dim_x, dim_z), dtype=dt)
h[0] = 1
f.H = h
f.P = np.eye(dim_x, dtype=dt)*1000
f.R = np.eye(dim_z, dtype=dt)*5.0
else:
f.x = np_to_dataframe(np.zeros((dim_x, 1), dtype=dt))
tmp = np.eye(dim_x, dtype=dt, order='F')
f.F = np_to_dataframe(tmp)
h = np.zeros((dim_x, dim_z), dtype=dt, order='F')
h[0] = 1
f.H = np_to_dataframe(h)
f.P = np_to_dataframe(np.eye(dim_x, dtype=dt, order='F')*1000)
f.R = np_to_dataframe(np.eye(dim_z, dtype=dt, order='F')*5.0)
rmse_x = 0
rmse_v = 0
n = 100
for i in range(100):
f.predict()
z = i*np.ones(dim_z, dtype=dt)
f.update(cuda.to_device(np.array(z, dtype=dt)))
x = f.x.copy_to_host()
rmse_x = rmse_x + ((x[0] - i)**2)
rmse_v = rmse_v + ((x[1] - 1)**2)
assert sqrt(rmse_x/n) < 0.1
assert sqrt(rmse_v/n) == 1.0
def test_linear_kalman_filter(precision, dim_x, dim_z):
f = KalmanFilter(dim_x=dim_x,
dim_z=dim_z,
precision=precision,
solver='short_implicit')
if precision == 'single':
dt = np.float32
else:
dt = np.float64
f.x = np_to_dataframe(np.zeros((dim_x, 1), dtype=dt))
tmp = np.eye(dim_x, dtype=dt, order='F')
f.F = np_to_dataframe(tmp)
h = np.zeros((dim_x, dim_z), dtype=dt, order='F')
h[0] = 2.0
f.H = np_to_dataframe(h)
f.P = np_to_dataframe(np.eye(dim_x, dtype=dt, order='F') * 1000)
f.R = np_to_dataframe(np.eye(dim_z, dtype=dt, order='F') * 5.0)
rmse_x = 0
rmse_v = 0
n = 25
for i in range(n):
f.predict()
z = cuda.to_device(np.full(dim_z, i * 2, dtype=dt))
f.update(z)
x = f.x.copy_to_host()
rmse_x = rmse_x + ((x[0] - i)**2)
rmse_v = rmse_v + ((x[1] - 1)**2)
assert sqrt(rmse_x / n) < 0.1
assert sqrt(rmse_v / n) == 1.0