def GNN_LG_models_2_obj_1D():
arr = lambda scalar: np.array([[scalar]])
# Prior
x_prior = arr(2.5)
P_prior = arr(0.36)
p_prior = GaussianMixture(
[Gaussian(-x_prior, P_prior),
Gaussian(x_prior, P_prior)])
n = 2
# g_mix = GaussianMixture([Gaussian(-3, 0.2), Gaussian(3, 0.2)])
# p_prior = MultiGaussianMixture(g_mix)
# Models
# - Motion
Q = arr(0.25)
F = arr(1)
# - Measurement
R = arr(0.2)
H = arr(1)
PD = 0.85
# - Clutter
lamb = 0.3
# Measurements
Z = [-3.2, -2.4, 1.9, 2.2, 2.7, 2.9]
Z = [arr(val) for val in Z]
p, p_pred, theta_star = GNN_LG(p_prior, Z, n, PD, lamb, F, Q, H, R)
# Plot
ax = plt.subplot()
res = 100
marker_size = 100
x_lim = [-4, 4]
colors = ['orange', 'purple']
ax.axhline(0, color='gray', linewidth=0.5)
# - Measurements
plot_1D_measurements(ax, Z, color='b', marker='*', s=marker_size, zorder=3)
# - Object densities
plts = []
for i in range(n):
# - Prior density
plt1 = plot_gaussian_pdf(ax,
p_prior.components[i],
x_lim,
res=res,
color='g',
zorder=1)
# - Predicted density
plt2 = plot_gaussian_pdf(ax,
p_pred.components[i],
x_lim,
res=res,
color='r',
linestyle='--',
zorder=0)
# - Posterior density
plt3 = plot_gaussian_pdf(ax,
p.components[i],
x_lim,
res=res,
color=colors[i],
linestyle='--',
zorder=2)
# - Association
if theta_star[i] >= len(Z):
ax.scatter(p_pred.components[i].x,
0,
color=colors[i],
marker='o',
s=marker_size,
zorder=3)
else:
ax.scatter(Z[theta_star[i]],
0,
color=colors[i],
marker='*',
s=marker_size,
zorder=3)
[plts.append(plt) for plt in [plt1, plt2, plt3]]
# - Final details
# ax.set_xlim(x_lim)
ax.set_ylim([-0.05, 1.5])
ax.legend(
plts,
('o1_prior', 'o1_pred', 'o1_GNN', 'o2_prior', 'o2_pred', 'o2_GNN'))
plt.pause(0.0001)
def GSF_linear_gaussian_models_simple_scenario():
arr = lambda scalar: np.array([[scalar]])
# Prior
x_prior = arr(0.5)
P_prior = arr(0.2)
p_prior_exact = GaussianMixture(Gaussian(x_prior, P_prior, weight=1))
p_prior_GSF = GaussianMixture(Gaussian(x_prior, P_prior, weight=1))
# Models
# - Motion
Q = arr(0.35)
F = arr(1)
# - Measurement
R = arr(0.2)
H = arr(1)
PD = 0.9
# - Clutter
lamb = 0.4
# GSF tuning params
Nmax = 5
tol_prune = 0.01 # or None
# Sensor
space = Space1D(-4, 4)
# Create measurement vector
Z1 = [-1.3, 1.7]
Z2 = [1.3]
Z3 = [-0.3, 2.3]
Z4 = [-2, 3]
Z5 = [2.6]
Z6 = [-3.5, 2.8]
Zs = [Z1, Z2, Z3, Z4, Z5, Z6]
# Plot settings
ax = plt.subplot()
res = 100
marker_size = 100
x_lim = [space.min, space.max]
# Compute exact posterior
for k, Z in enumerate(Zs, start=1):
print(f'Measurements: {Z}')
# Compute posterior
p_exact, p_pred_exact = exact_posterior_LG(p_prior_exact, Z, PD, lamb,
F, Q, H, R)
p_GSF, p_pred_GSF = GSF_LG(p_prior_GSF, Z, PD, lamb, F, Q, H, R, Nmax,
tol_prune)
# Number of hypotesis
print(f'Number of hypotesis, exact posterior: {p_exact.n_components}')
print(f'Number of hypotesis, GSF posterior: {p_GSF.n_components}')
# Plot
ax.clear()
ax.set(title=f'k = {k}', xlabel='x')
ax.axhline(0, color='gray', linewidth=0.5)
# - Predicted density according to GSF
plt1 = plot_gaussian_mixture_pdf(ax,
p_pred_GSF,
x_lim,
res=res,
color='r',
linestyle='--',
zorder=0)
# - Exact posterior density
plt2 = plot_gaussian_mixture_pdf(ax,
p_exact,
x_lim,
res=res,
color='k',
zorder=1)
# - Posterior density according to GSF
plt3 = plot_gaussian_mixture_pdf(ax,
p_GSF,
x_lim,
res=res,
color='orange',
linestyle='--',
marker='x',
zorder=2)
# - Measurements
plot_1D_measurements(ax,
Z,
color='b',
marker='*',
s=marker_size,
zorder=3)
# - Final details
ax.set_xlim([space.min, space.max])
ax.set_ylim([-0.05, 1.05])
ax.legend((plt1, plt2, plt3), ('pred', 'exact', 'GSF'))
plt.pause(0.0001)
p_prior_exact = p_exact
p_prior_GSF = p_GSF
input('Press to continue')
def GNN_LG_models_2_obj_sequence_1D():
arr = lambda scalar: np.array([[scalar]])
# Space
space = Space1D(-4, 4)
# Prior
x_prior = arr(2.5)
P_prior = arr(0.36)
p_prior = GaussianMixture(
[Gaussian(-x_prior, P_prior),
Gaussian(x_prior, P_prior)])
# Models
# - Motion
Q = arr(0.25)
F = arr(1)
# - Measurement
R = arr(0.2)
H = arr(1)
PD = 0.85
_PD = lambda x: PD
# - Clutter
lamb = 0.4
clutter_model = UniformClutterModel1D(space, lamb)
# Create true sequence
n = 2
n_ks = 50
X = np.vstack((-x_prior * np.ones(n_ks), x_prior * np.ones(n_ks)))
# Measurements
sensor = Detector1D(R, _PD, clutter_model, space)
Z = sensor.get_measurements(X)
# Estimate trajectories
x_hat = np.zeros((n, n_ks))
P_hat = np.zeros((n, n_ks))
thetas = np.zeros((n, n_ks))
for k in range(n_ks):
print(f'---k={k+1}---')
print(Z[k])
p, p_pred, theta_star = GNN_LG(p_prior, Z[k], n, PD, lamb, F, Q, H, R)
for i in range(n):
x_hat[i, k] = p.components[i].x
P_hat[i, k] = p.components[i].P
thetas[i, k] = theta_star[i]
p_prior = p
# Plot
ax = plt.subplot()
marker_size = 100
colors = ['orange', 'purple']
ass_colors = ['g', 'g']
y_axis = np.linspace(1, n_ks, num=n_ks)
# - Heatmaps
for k in range(n_ks):
for i in range(n):
plot_1D_heatmap(ax, x_hat[i, k], P_hat[i, k], k + 1)
# - True trajectories
for i in range(n):
ax.plot(X[i, :], y_axis, color='k', marker='o')
# - Measurements
for k in range(n_ks):
ax.axhline(k + 1.5, color='gray', linewidth=0.5)
plot_1D_measurements(ax,
Z[k],
k=k + 1,
color='b',
marker='*',
s=marker_size)
# - Associations
for k in range(n_ks):
for i in range(n):
if thetas[i, k] < len(Z[k]):
ax.scatter(Z[k][int(thetas[i, k])],
k + 1,
color=ass_colors[i],
marker='*',
s=marker_size)
# - Estimates
for i in range(n):
ax.plot(x_hat[i, :], y_axis, color=colors[i], marker='s')
# - Final details
ax.set(xlabel='x', ylabel='k')
ax.set_xlim([space.min, space.max])
plt.pause(0.0001)