def error_and_plotting(m_res,
Res,
m_a,
l_a,
w_a,
al_a,
m_b,
l_b,
w_b,
al_b,
m_gt,
l_gt,
w_gt,
al_gt,
plotting,
name,
filename,
est_color='red'):
# turn result back and compare
l_res, w_res, al_res = get_ellipse_params(Res)
if plotting:
plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
w_gt, al_gt, m_res, l_res, w_res, al_res, name, filename,
est_color)
return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, m_res, l_res, w_res,
al_res)
def best_or(m_a, cov_a, l_a, w_a, al_a, m_b, cov_b, l_b, w_b, al_b, m_gt, l_gt,
w_gt, al_gt, plotting):
[res_orig_alt, cov, k] = best_fusion(m_a, cov_a, l_a, w_a, al_a, m_b,
cov_b, l_b, w_b, al_b)
if plotting:
plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
w_gt, al_gt, res_orig_alt[:2], res_orig_alt[3],
res_orig_alt[4], res_orig_alt[2],
'Linearized Approximated Fusion With Best Orientation',
'exampleRegAltFus.svg')
return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, res_orig_alt[:2],
res_orig_alt[3], res_orig_alt[4], res_orig_alt[2])
def shape_mean_k(m_a, A, l_a, w_a, al_a, m_b, B, l_b, w_b, al_b, m_gt, l_gt,
w_gt, al_gt, plotting):
m_res_alt2 = 0.5 * (m_a + m_b)
Res_alt2 = 0.5 * (A + B) + 0.25 * np.outer(m_a - m_b, m_a - m_b)
ellipse_axisalt2, valt2 = np.linalg.eig(Res_alt2)
ellipse_axisalt2 = np.sqrt(ellipse_axisalt2)
l_res_alt2 = ellipse_axisalt2[0]
w_res_alt2 = ellipse_axisalt2[1]
al_res_alt2 = np.arctan2(valt2[1, 0], valt2[0, 0])
if plotting:
plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
w_gt, al_gt, m_res_alt2, l_res_alt2, w_res_alt2,
al_res_alt2, 'Shape Mean', 'exampleMean.svg')
return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, m_res_alt2, l_res_alt2,
w_res_alt2, al_res_alt2)
def ord_fusion(m_a, cov_a, l_a, w_a, al_a, m_b, cov_b, l_b, w_b, al_b, m_gt,
l_gt, w_gt, al_gt, plotting):
S_orig = cov_a + cov_b
if np.linalg.det(S_orig) == 0:
print('Singular S_orig')
# print(S_orig)
K_orig = np.dot(cov_a, np.linalg.inv(S_orig))
res_orig = np.array([m_a[0], m_a[1], al_a, l_a, w_a]) + np.dot(
K_orig,
np.array([m_b[0], m_b[1], al_b, l_b, w_b]) -
np.array([m_a[0], m_a[1], al_a, l_a, w_a]))
if plotting:
plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
w_gt, al_gt, res_orig[:2], res_orig[3], res_orig[4],
res_orig[2], 'Fusion of Original State',
'exampleRegFus.svg')
return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, res_orig[:2],
res_orig[3], res_orig[4], res_orig[2])
def lin_approx(m_a, cov_a, A, l_a, w_a, al_a, m_b, cov_b, B, l_b, w_b, al_b,
m_gt, l_gt, w_gt, al_gt, plotting):
jac_a = np.array([
[1, 0, 0, 0, 0],
[0, 1, 0, 0, 0],
[
0, 0, (w_a**2 - l_a**2) * np.sin(2 * al_a),
2 * l_a * np.cos(al_a)**2, 2 * w_a * np.sin(al_a)**2
],
[
0, 0, (l_a**2 - w_a**2) * np.cos(2 * al_a),
2 * l_a * np.cos(al_a) * np.sin(al_a),
-2 * w_a * np.sin(al_a) * np.cos(al_a)
],
[
0, 0, (l_a**2 - w_a**2) * np.sin(2 * al_a),
2 * l_a * np.sin(al_a)**2, 2 * w_a * np.cos(al_a)**2
],
])
jac_b = np.array([
[1, 0, 0, 0, 0],
[0, 1, 0, 0, 0],
[
0, 0, (w_b**2 - l_b**2) * np.sin(2 * al_b),
2 * l_b * np.cos(al_b)**2, 2 * w_b * np.sin(al_b)**2
],
[
0, 0, (l_b**2 - w_b**2) * np.cos(2 * al_b),
2 * l_b * np.cos(al_b) * np.sin(al_b),
-2 * w_b * np.sin(al_b) * np.cos(al_b)
],
[
0, 0, (l_b**2 - w_b**2) * np.sin(2 * al_b),
2 * l_b * np.sin(al_b)**2, 2 * w_b * np.cos(al_b)**2
],
])
s_a = 2 * np.sqrt(A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1])
t_a = np.sqrt(A[0, 0] + A[1, 1] + s_a)
s_a_al = (jac_a[2, 2] * A[1, 1] + jac_a[4, 2] * A[0, 0] - 2 * jac_a[3, 2] *
A[1, 0]) / np.sqrt(A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1])
t_a_al = (jac_a[2, 2] + jac_a[4, 2] + s_a_al) / (2 * t_a)
s_a_l = (jac_a[2, 3] * A[1, 1] + jac_a[4, 3] * A[0, 0] - 2 * jac_a[3, 3] *
A[1, 0]) / np.sqrt(A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1])
t_a_l = (jac_a[2, 3] + jac_a[4, 3] + s_a_al) / (2 * t_a)
s_a_w = (jac_a[2, 4] * A[1, 1] + jac_a[4, 4] * A[0, 0] - 2 * jac_a[3, 4] *
A[1, 0]) / np.sqrt(A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1])
t_a_w = (jac_a[2, 4] + jac_a[4, 4] + s_a_al) / (2 * t_a)
jac_a2 = np.array([
[1, 0, 0, 0, 0],
[0, 1, 0, 0, 0],
[
0, 0,
-t_a_al * (A[0, 0] + s_a) / t_a**2 + (jac_a[2, 2] + s_a_al) / t_a,
-t_a_l * (A[0, 0] + s_a) / t_a**2 + (jac_a[2, 3] + s_a_l) / t_a,
-t_a_w * (A[0, 0] + s_a) / t_a**2 + (jac_a[2, 4] + s_a_w) / t_a
],
[
0, 0, -t_a_al * A[1, 0] / t_a**2 + jac_a[3, 2] / t_a,
-t_a_l * A[1, 0] / t_a**2 + jac_a[3, 3] / t_a,
-t_a_w * A[1, 0] / t_a**2 + jac_a[3, 4] / t_a
],
[
0, 0,
-t_a_al * (A[1, 1] + s_a) / t_a**2 + (jac_a[4, 2] + s_a_al) / t_a,
-t_a_l * (A[1, 1] + s_a) / t_a**2 + (jac_a[4, 3] + s_a_l) / t_a,
-t_a_w * (A[1, 1] + s_a) / t_a**2 + (jac_a[4, 4] + s_a_w) / t_a
],
])
s_b = 2 * np.sqrt(B[0, 0] * B[1, 1] - B[1, 0] * B[0, 1])
t_b = np.sqrt(B[0, 0] + B[1, 1] + s_b)
s_b_al = (jac_b[2, 2] * B[1, 1] + jac_b[4, 2] * B[0, 0] - 2 * jac_b[3, 2] *
B[1, 0]) / np.sqrt(B[0, 0] * B[1, 1] - B[1, 0] * B[0, 1])
t_b_al = (jac_b[2, 2] + jac_b[4, 2] + s_b_al) / (2 * t_b)
s_b_l = (jac_b[2, 3] * B[1, 1] + jac_b[4, 3] * B[0, 0] - 2 * jac_b[3, 3] *
B[1, 0]) / np.sqrt(B[0, 0] * B[1, 1] - B[1, 0] * B[0, 1])
t_b_l = (jac_b[2, 3] + jac_b[4, 3] + s_b_al) / (2 * t_b)
s_b_w = (jac_b[2, 4] * B[1, 1] + jac_b[4, 4] * B[0, 0] - 2 * jac_b[3, 4] *
B[1, 0]) / np.sqrt(B[0, 0] * B[1, 1] - B[1, 0] * B[0, 1])
t_b_w = (jac_b[2, 4] + jac_b[4, 4] + s_b_al) / (2 * t_b)
jac_b2 = np.array([
[1, 0, 0, 0, 0],
[0, 1, 0, 0, 0],
[
0, 0,
-t_b_al * (B[0, 0] + s_b) / t_b**2 + (jac_b[2, 2] + s_b_al) / t_b,
-t_b_l * (B[0, 0] + s_b) / t_b**2 + (jac_b[2, 3] + s_b_l) / t_b,
-t_b_w * (B[0, 0] + s_b) / t_b**2 + (jac_b[2, 4] + s_b_w) / t_b
],
[
0, 0, -t_b_al * B[1, 0] / t_b**2 + jac_b[3, 2] / t_b,
-t_b_l * B[1, 0] / t_b**2 + jac_b[3, 3] / t_b,
-t_b_w * B[1, 0] / t_b**2 + jac_b[3, 4] / t_b
],
[
0, 0,
-t_b_al * (B[1, 1] + s_b) / t_b**2 + (jac_b[4, 2] + s_b_al) / t_b,
-t_b_l * (B[1, 1] + s_b) / t_b**2 + (jac_b[4, 3] + s_b_l) / t_b,
-t_b_w * (B[1, 1] + s_b) / t_b**2 + (jac_b[4, 4] + s_b_w) / t_b
],
])
sqrt_A = sqrtm(A)
sqrt_B = sqrtm(B)
vec_A_direct = np.array(
[m_a[0], m_a[1], sqrt_A[0, 0], sqrt_A[0, 1], sqrt_A[1, 1]])
vec_B_direct = np.array(
[m_b[0], m_b[1], sqrt_B[0, 0], sqrt_B[0, 1], sqrt_B[1, 1]])
S_lin2 = np.dot(np.dot(jac_a2, cov_a), jac_a2.T) + np.dot(
np.dot(jac_b2, cov_b), jac_b2.T)
K2 = np.dot(np.dot(np.dot(jac_a2, cov_a), jac_a2.T), np.linalg.inv(S_lin2))
res_lin_vec2 = vec_A_direct + np.dot(K2, vec_B_direct - vec_A_direct)
res_lin_S2 = np.array([
[res_lin_vec2[2], res_lin_vec2[3]],
[res_lin_vec2[3], res_lin_vec2[4]],
])
res_lin_S2 = np.dot(res_lin_S2, res_lin_S2)
ellipse_axislin2, vlin2 = np.linalg.eig(res_lin_S2)
ellipse_axislin2 = np.sqrt(ellipse_axislin2)
l_res_lin2 = ellipse_axislin2[0]
w_res_lin2 = ellipse_axislin2[1]
al_res_lin2 = np.arctan2(vlin2[1, 0], vlin2[0, 0])
if plotting:
plot_ellipses(m_a, l_a, w_a, al_a, m_b, l_b, w_b, al_b, m_gt, l_gt,
w_gt, al_gt, res_lin_vec2[:2], l_res_lin2, w_res_lin2,
al_res_lin2, 'Linearized Approximated Fusion',
'exampleGWFus.svg')
return gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, res_lin_vec2[:2],
l_res_lin2, w_res_lin2, al_res_lin2)
def test_convergence_pos(steps, runs, m_prior, l_prior, w_prior, al_prior,
cov_a, cov_b, n_particles):
save_path = '/home/kthormann/Desktop/research-fusion/research-fusion/Python/EllipseFusion/convergence/'
error = np.zeros(steps)
error_reg = np.zeros(steps)
error_shape = np.zeros(steps)
error_params = np.zeros(steps)
error_lin = np.zeros(steps)
for r in range(runs):
print('Run %i of %i' % (r, runs))
# create gt with sensor A as prior
m_gt = mvn(m_prior, cov_a[:2, :2])
l_gt = np.maximum(normal(l_prior, cov_a[3, 3]), 0.1)
w_gt = np.maximum(normal(w_prior, cov_a[4, 4]),
0.1) # np.maximum(normal(w_gt, cov_a[4, 4]), 0.1)
al_gt = normal(al_prior, cov_a[2, 2]) # - 0.1 * np.pi
al_gt %= (2 * np.pi)
# get prior in square root space
m_prior, el_sr_prior, cov_sr_prior = single_particle_approx_gaussian(
m_prior, l_prior, w_prior, al_prior, cov_a, n_particles, True)
# get prior for regular state
prior_reg = np.array(
[m_prior[0], m_prior[1], al_prior, l_prior, w_prior])
cov_prior_reg = cov_a.copy()
# get prior for best params
prior_params = np.copy(prior_reg)
cov_prior_params = np.copy(cov_prior_reg)
# get prior for shape mean
prior_shape_m = m_prior
prior_shape_l = l_prior
prior_shape_w = w_prior
prior_shape_al = al_prior
rot_prior = np.array([
[np.cos(al_prior), -np.sin(al_prior)],
[np.sin(al_prior), np.cos(al_prior)],
])
prior_shape = np.dot(np.dot(rot_prior,
np.diag([l_prior, w_prior])**2),
rot_prior.T)
# get prior for linearization
prior_shape_sqrt = sqrtm(prior_shape)
prior_lin = np.array([
m_prior[0], m_prior[1], prior_shape_sqrt[0, 0],
prior_shape_sqrt[1, 0], prior_shape_sqrt[1, 1]
])
prior_lin_l = l_prior
prior_lin_w = w_prior
prior_lin_al = al_prior
jac = get_jacobian(l_prior, w_prior, al_prior)
prior_lin_cov = np.dot(np.dot(jac, cov_prior_reg), jac.T)
# test different methods =======================================================================================
for i in range(steps):
if i % 10 == 0:
print(i)
# create measurement from gt (using alternating sensors)
if (i % 2) == 0:
m_m = mvn(m_gt, cov_b[:2, :2])
l_m = np.maximum(normal(w_gt, cov_b[3, 3]), 0.1)
w_m = np.maximum(normal(l_gt, cov_b[4, 4]), 0.1)
al_m = normal(al_gt, cov_b[2, 2]) + 0.5 * np.pi
al_m %= (2 * np.pi)
cov_m = cov_b.copy()
else:
m_m = mvn(m_gt, cov_a[:2, :2])
l_m = np.maximum(normal(l_gt, cov_a[3, 3]), 0.1)
w_m = np.maximum(normal(w_gt, cov_a[4, 4]), 0.1)
al_m = normal(al_gt, cov_a[2, 2])
al_m %= (2 * np.pi)
cov_m = cov_a.copy()
# MC approximation =========================================================================================
# get measurement in square root space
m_m, el_sr_m, cov_sr_m = single_particle_approx_gaussian(
m_m, l_m, w_m, al_m, cov_m, n_particles, True)
# get posterior in square root space
# fuse A and B via mean and variance
S = cov_sr_prior + cov_sr_m
K = np.dot(cov_sr_prior, np.linalg.inv(S))
el_sr_post = el_sr_prior + np.dot(K, el_sr_m - el_sr_prior)
cov_sr_post = cov_sr_prior - np.dot(np.dot(K, S), K.T)
# var_Res = var_A - np.dot(np.dot(K, S), K.T)
el_sr_res = np.array([
[el_sr_post[2], el_sr_post[3]],
[el_sr_post[3], el_sr_post[4]],
])
el_res = np.dot(el_sr_res, el_sr_res)
error[i] += error_and_plotting(el_sr_post[:2],
el_res,
el_sr_prior[:2],
l_prior,
w_prior,
al_prior,
m_m,
l_m,
w_m,
al_m,
m_gt,
l_gt,
w_gt,
al_gt, (r + 1) == runs,
'MC Approximated Fusion',
save_path +
'exampleMCApprox%i.svg' % i,
est_color='green')
el_sr_prior = el_sr_post
cov_sr_prior = cov_sr_post
l_prior, w_prior, al_prior = get_ellipse_params(el_res)
# regular fusion ===========================================================================================
# get posterior in square root space
# fuse A and B via mean and variance
S = cov_prior_reg + cov_m
K = np.dot(cov_prior_reg, np.linalg.inv(S))
post_reg = prior_reg + np.dot(
K,
np.array([m_m[0], m_m[1], al_m, l_m, w_m]) - prior_reg)
cov_post_reg = cov_prior_reg - np.dot(np.dot(K, S), K.T)
if (r + 1) == runs:
plot_ellipses(prior_reg[:2], prior_reg[3], prior_reg[4],
prior_reg[2], m_m, l_m, w_m, al_m, m_gt, l_gt,
w_gt, al_gt, post_reg[:2], post_reg[3],
post_reg[4], post_reg[2],
'Fusion of Original State',
save_path + 'exampleRegFus%i.svg' % i)
error_reg[i] += gauss_wasserstein(m_gt, l_gt, w_gt, al_gt,
post_reg[:2], post_reg[3],
post_reg[4], post_reg[2])
prior_reg = post_reg
cov_prior_reg = cov_post_reg
# best params fusion =======================================================================================
[post_params, cov_post_params,
k] = best_fusion(prior_params[:2], cov_prior_params,
prior_params[3], prior_params[4],
prior_params[2], m_m, cov_m, l_m, w_m, al_m)
if (r + 1) == runs:
plot_ellipses(prior_params[:2], prior_params[3],
prior_params[4], prior_params[2], m_m, l_m, w_m,
al_m, m_gt, l_gt, w_gt, al_gt, post_params[:2],
post_params[3], post_params[4], post_params[2],
'Best Params',
save_path + 'exampleBestParams%i.svg' % i)
error_params[i] += gauss_wasserstein(m_gt, l_gt, w_gt, al_gt,
post_params[:2],
post_params[3],
post_params[4],
post_params[2])
prior_params = post_params
cov_prior_params = cov_post_params
# shape mean ===============================================================================================
post_shape_m = 0.5 * (prior_shape_m + m_m)
rot_m = np.array([
[np.cos(al_m), -np.sin(al_m)],
[np.sin(al_m), np.cos(al_m)],
])
m_shape = np.dot(np.dot(rot_m, np.diag([l_m, w_m])**2), rot_m.T)
post_shape = 0.5 * (prior_shape + m_shape)
post_shape_l, post_shape_w, post_shape_al = get_ellipse_params(
post_shape)
if (r + 1) == runs:
plot_ellipses(prior_shape_m, prior_shape_l, prior_shape_w,
prior_shape_al, m_m, l_m, w_m, al_m, m_gt, l_gt,
w_gt, al_gt, post_shape_m, post_shape_l,
post_shape_w, post_shape_al, 'Shape Mean',
'exampleMean.svg')
error_shape[i] += gauss_wasserstein(m_gt, l_gt, w_gt, al_gt,
post_shape_m, post_shape_l,
post_shape_w, post_shape_al)
prior_shape_m = post_shape_m
prior_shape = post_shape
prior_shape_l = post_shape_l
prior_shape_w = post_shape_w
prior_shape_al = post_shape_al
# linearization ============================================================================================
m_shape_sqrt = sqrtm(m_shape)
lin_m = np.array([
m_m[0], m_m[1], m_shape_sqrt[0, 0], m_shape_sqrt[1, 0],
m_shape_sqrt[1, 1]
])
jac_m = get_jacobian(l_m, w_m, al_m)
cov_m_lin = np.dot(np.dot(jac_m, cov_m), jac_m.T)
S_lin = prior_lin_cov + cov_m_lin
K_lin = np.dot(prior_lin_cov, np.linalg.inv(S_lin))
post_lin = prior_lin + np.dot(K_lin, lin_m - prior_lin)
post_lin_cov = prior_lin_cov - np.dot(np.dot(K_lin, S_lin),
K_lin.T)
post_lin_shape = np.array([
[post_lin[2], post_lin[3]],
[post_lin[3], post_lin[4]],
])
post_lin_shape = np.dot(post_lin_shape, post_lin_shape)
ellipse_axis_post_lin, v_post_lin = np.linalg.eig(post_lin_shape)
ellipse_axis_post_lin = np.sqrt(ellipse_axis_post_lin)
post_lin_l, post_lin_w, post_lin_al = get_ellipse_params(
post_lin_shape)
if (r + 1) == runs:
plot_ellipses(prior_lin[:2], prior_lin_l, prior_lin_w,
prior_lin_al, m_m, l_m, w_m, al_m, m_gt, l_gt,
w_gt, al_gt, post_lin[:2], post_lin_l,
post_lin_w, post_lin_al, 'Shape Mean',
'exampleMean.svg')
error_lin[i] += gauss_wasserstein(m_gt, l_gt, w_gt, al_gt,
post_lin[:2], post_lin_l,
post_lin_w, post_lin_al)
prior_lin = post_lin
prior_lin_cov = post_lin_cov
prior_lin_l = post_lin_l
prior_line_w = post_lin_w
prior_lin_al = post_lin_al
error = np.sqrt(error / runs)
error_reg = np.sqrt(error_reg / runs)
error_shape = np.sqrt(error_shape / runs)
error_params = np.sqrt(error_params / runs)
error_lin = np.sqrt(error_lin / runs)
print(error)
print(error_params)
# error first and last
bars = np.array([1, 3, 5, 7, 9])
ticks = np.array([
'Regular', 'Mean of\n shape matrix', 'MMGW-Lin', 'Heuristic', 'MMGW-MC'
])
plt.bar(0.825, error_reg[0], width=0.25, color='red', align='center')
plt.bar(1.175, error_reg[-1], width=0.25, color='red', align='center')
plt.bar(2.825, error_shape[0], width=0.25, color='m', align='center')
plt.bar(3.175, error_shape[-1], width=0.25, color='m', align='center')
plt.bar(4.825,
error_lin[0],
width=0.25,
color='deepskyblue',
align='center')
plt.bar(5.175,
error_lin[-1],
width=0.25,
color='deepskyblue',
align='center')
plt.bar(6.825,
error_params[0],
width=0.25,
color='darkcyan',
align='center')
plt.bar(7.175,
error_params[-1],
width=0.25,
color='darkcyan',
align='center')
plt.bar(8.825, error[0], width=0.25, color='green', align='center')
plt.bar(9.175, error[-1], width=0.25, color='green', align='center')
plt.xticks(bars, ticks)
plt.title('GW RMSE')
plt.ylim(0.0, 1.5)
plt.savefig('gwRmse.svg')
plt.show()
plt.plot(np.arange(1, steps + 1), error)
plt.savefig(save_path + 'MCApproxError.svg')
plt.show()
plt.plot(np.arange(1, steps + 1), error_reg)
plt.savefig(save_path + 'regFusError.svg')
plt.show()
plt.plot(np.arange(1, steps + 1),
error_reg,
color='red',
label='Regular State')
plt.plot(np.arange(1, steps + 1),
error_shape,
color='m',
label='Mean of shape matrix')
plt.plot(np.arange(1, steps + 1),
error_lin,
color='deepskyblue',
label='MMGW-Lin')
plt.plot(np.arange(1, steps + 1),
error_params,
color='darkcyan',
label='Heuristic')
plt.plot(np.arange(1, steps + 1), error, color='green', label='MMGW-MC')
plt.legend()
plt.xticks([5, 10, 15, 20])
plt.savefig(save_path + 'errorComp.svg')
plt.show()
def test_convergence(steps, m_prior, l_prior, w_prior, al_prior, cov_a, cov_b,
n_particles):
save_path = '/home/convergence/'
error = np.zeros(steps)
error_reg = np.zeros(steps)
# create gt with sensor A as prior
m_gt = m_prior # mvn(m_a, cov_a[:2, :2])
l_gt = np.maximum(normal(l_prior, cov_a[3, 3]), 0.1)
w_gt = np.maximum(normal(w_prior, cov_a[4, 4]),
0.1) # np.maximum(normal(w_gt, cov_a[4, 4]), 0.1)
al_gt = normal(al_prior, cov_a[2, 2]) # - 0.1 * np.pi
al_gt %= (2 * np.pi)
# get prior in square root space
m_prior, el_sr_prior, cov_sr_prior = single_particle_approx_gaussian(
m_prior, l_prior, w_prior, al_prior, cov_a, n_particles, False)
# get prior for regular state
m_prior_reg = m_prior.copy()
prior_reg = np.array([al_prior, l_prior, w_prior])
cov_prior_reg = cov_a.copy()[2:, 2:]
# test different methods ===========================================================================================
for i in range(steps):
if i % 10 == 0:
print(i)
# create measurement from gt (using alternating sensors)
if (i % 2) == 0:
m_m = m_gt # mvn(m_gt, cov_b[:2, :2])
l_m = np.maximum(normal(w_gt, cov_b[3, 3]), 0.1)
w_m = np.maximum(normal(l_gt, cov_b[4, 4]), 0.1)
al_m = normal(al_gt, cov_b[2, 2]) + 0.5 * np.pi
al_m %= (2 * np.pi)
cov_m = cov_b.copy()
else:
m_m = m_gt # mvn(m_gt, cov_a[:2, :2])
l_m = np.maximum(normal(l_gt, cov_a[3, 3]), 0.1)
w_m = np.maximum(normal(w_gt, cov_a[4, 4]), 0.1)
al_m = normal(al_gt, cov_a[2, 2])
al_m %= (2 * np.pi)
cov_m = cov_a.copy()
# MC approximation =============================================================================================
# get measurement in square root space
m_m, el_sr_m, cov_sr_m = single_particle_approx_gaussian(
m_m, l_m, w_m, al_m, cov_m, n_particles, False)
# get posterior in square root space
# fuse A and B via mean and variance
S = cov_sr_prior + cov_sr_m
K = np.dot(cov_sr_prior, np.linalg.inv(S))
el_sr_post = el_sr_prior + np.dot(K, el_sr_m - el_sr_prior)
cov_sr_post = cov_sr_prior - np.dot(np.dot(K, S), K.T)
# var_Res = var_A - np.dot(np.dot(K, S), K.T)
m_post = 0.5 * (m_prior + m_m) # mean_Res[:2]
el_sr_res = np.array([
[el_sr_post[0], el_sr_post[1]],
[el_sr_post[1], el_sr_post[2]],
])
el_res = np.dot(el_sr_res, el_sr_res)
error[i] = error_and_plotting(m_post, el_res, m_prior, l_prior,
w_prior, al_prior, m_m, l_m, w_m, al_m,
m_gt, l_gt, w_gt, al_gt, True,
'MC Approximated Fusion',
save_path + 'exampleMCApprox%i.svg' % i)
m_prior = m_post
el_sr_prior = el_sr_post
cov_sr_prior = cov_sr_post
l_prior, w_prior, al_prior = get_ellipse_params(el_res)
# regular fusion ===============================================================================================
# get posterior in square root space
# fuse A and B via mean and variance
S = cov_prior_reg + cov_m[2:, 2:]
K = np.dot(cov_prior_reg, np.linalg.inv(S))
post_reg = prior_reg + np.dot(K,
np.array([al_m, l_m, w_m]) - prior_reg)
cov_post_reg = cov_prior_reg - np.dot(np.dot(K, S), K.T)
m_post_reg = 0.5 * (m_prior_reg + m_m)
plot_ellipses(m_prior, prior_reg[1], prior_reg[2], prior_reg[0], m_m,
l_m, w_m, al_m, m_gt, l_gt, w_gt, al_gt, m_post_reg,
post_reg[1], post_reg[2], post_reg[0],
'Fusion of Original State',
save_path + 'exampleRegFus%i.svg' % i)
error_reg[i] = gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, m_post_reg,
post_reg[1], post_reg[2], post_reg[0])
m_prior_reg = m_post_reg
prior_reg = post_reg
cov_prior_reg = cov_post_reg
plt.plot(np.arange(1, steps + 1), error)
plt.savefig(save_path + 'MCApproxError.svg')
plt.show()
plt.plot(np.arange(1, steps + 1), error_reg)
plt.savefig(save_path + 'regFusError.svg')
plt.show()
plt.plot(np.arange(1, steps + 1), error, color='green', label='MC Fusion')
plt.plot(np.arange(1, steps + 1),
error_reg,
color='red',
label='Regular Fusion')
plt.legend()
plt.savefig(save_path + 'errorComp.svg')
plt.show()