def ekf_test_multiple_observer(dt, numSteps, num_observers, yrecs):
"""
Run a matchernet of the following structure
b0 --- m01 --- b1
--- m02 --- b2
--- m03 --- b3
where b1, b2, and b3 are Observers
"""
f = fn.LinearFn(A0 * 0.5)
Sigma0 = 2 * ey2
Q = ey2
mu1 = np.array([0, 2.0], dtype=np.float32)
b0 = BundleEKFContinuousTime("b0", dt, f, Q, mu1, Sigma0)
bp = []
for i in range(num_observers):
bpname = "bp{}".format(i)
bptmp = Observer(bpname, yrecs[i])
bptmp.obs_noise_covariance = 5.0 * ey2
bp.append(bptmp)
# bp[0].obs_noise_covariance = 4*ey2
# bp[1].obs_noise_covariance = 4*ey2
mp = []
g0 = fn.LinearFn(np.eye(2, dtype=np.float32))
g1 = fn.LinearFn(np.eye(2, dtype=np.float32))
for i in range(num_observers):
mpname = "mp0{}".format(i)
mptmp = MatcherEKF(mpname, b0, bp[i], g0, g1)
mp.append(mptmp)
s = VirtualTimeScheduler()
bt = Timing(0, 1, 1)
bm = Timing(1, 1, 1)
s.add_component(b0.component, bt)
for i in range(num_observers):
s.add_component(bp[i].component, bt)
s.add_component(mp[i].component, bm)
for i in range(numSteps):
print_flush("Step {}/{} with brica".format(i, numSteps))
s.step()
def test_BundleEKFContinuousTime01(dt, n_steps):
f = fn.LinearFn(A0)
b = BundleEKFContinuousTime("b0", dt, f, Q, mu0, Sigma0)
b.logger_state()
dummy_input = {} # list of matchers (#matcher=0)
for i in range(n_steps):
b(dummy_input)
visualize_bundle_rec(b)
def setUp(self):
self.n = 2
mu0 = np.zeros(self.n, dtype=np.float32)
Sigma0 = np.eye(self.n, dtype=np.float32)
A0 = np.array([[-0.1, 2], [-2, -0.1]], dtype=np.float32)
Q = np.eye(self.n, dtype=np.float32)
f = fn.LinearFn(A0)
dt = 0.01
self.b0 = BundleEKFContinuousTime("b0", dt, f, Q, mu0, Sigma0)
self.b1 = BundleEKFContinuousTime("b1", dt, f, Q, mu0, Sigma0)
def test_MatcherEKF01(dt, n_steps, y_rec):
"""
Run a matchernet of the following structure
b0 --- m01 --- b1
"""
b1 = observer.Observer("b1", y_rec)
b1.obs_noise_covariance = 2 * ey2
f = fn.LinearFn(A0)
mu1 = np.array([0, 2.0], dtype=np.float32)
Sigma1 = 2.0 * ey2
b0 = BundleEKFContinuousTime("b0", dt, f, Q, mu1, Sigma1)
g0 = fn.LinearFn(ey2)
g1 = fn.LinearFn(ey2)
m01 = MatcherEKF("m01", b0, b1, g0, g1)
if _with_brica is False:
for i in range(n_steps):
print_flush("Step {}/{} with brica".format(i, n_steps))
inputs_to_m01 = {"b0": b0.state.data, "b1": b1.state.data}
results = m01(inputs_to_m01)
inputs_to_b0 = {"m01": results["b0"]}
s0 = b0(inputs_to_b0)
inputs_to_b1 = {"m01": results["b1"]}
s1 = b1(inputs_to_b1)
else:
s = VirtualTimeScheduler()
bt = Timing(0, 1, 1)
bm = Timing(1, 1, 1)
s.add_component(b0.component, bt)
s.add_component(b1.component, bt)
s.add_component(m01.component, bm)
for i in range(n_steps):
print_flush("Step {}/{} with brica".format(i, n_steps + 1))
s.step()
def test_bundle_and_observer(dt, n_steps, y_rec):
b0 = observer.Observer("b0", y_rec)
f = fn.LinearFn(A0)
b1 = BundleEKFContinuousTime("b1", dt, f, Q, mu0, Sigma0)
dummy_input = {}
for i in range(n_steps):
b0(dummy_input)
b1(dummy_input)
y = b0.get_state()
if i == 0:
y_rec2 = y
else:
y_rec2 = np.vstack((y_rec2, y))
visualize_bundle_rec(b1, y_rec2)
def prepare_data(dt, num_steps, num_observers):
y_recs = []
for i in range(num_observers):
print_flush("Generating simulation sequences {}/{}".format(
i, num_observers))
sm = StateSpaceModel2Dim(n_dim=2,
A=A0,
g=fn.LinearFn(utils.zeros(2)),
sigma_w=0.1,
sigma_z=0.1,
x=mu0[0],
y=utils.zeros((1, 2)))
sm.dt = dt
(x_rec, y_rec) = sm.simulation(num_steps, dt)
y_recs.append(y_rec)
return y_recs
# print("===Starting UnitTest01===")
# print("-- A simple test of Bundle with no Matcher")
# plt.figure(1)
# test_BundleEKFContinuousTime01(dt, n_steps)
# plt.pause(0.2)
dt = 0.02
n_steps = 500
# preparing a list of three simulation sequences
yrecs = []
for i in range(1):
sm = StateSpaceModel2Dim(n_dim=2,
A=np.array([[-0.1, 2], [-2, -0.1]],
dtype=np.float32),
g=fn.LinearFn(utils.zeros(2)),
sigma_w=0.1,
sigma_z=0.1,
x=np.array([0, 0], dtype=np.float32),
y=utils.zeros((1, 2)))
sm.A = A0
sm.x = mu0
sm.dt = dt
(x_rec, y_rec) = sm.simulation(n_steps, dt)
yrecs.append(y_rec)
plt.figure(0)
for i in range(1):
plt.subplot(2, 2, i + 1)
plt.scatter(yrecs[i][:, 0], yrecs[i][:, 1], s=2)
plt.title("sequence {}".format(i))