def mmsr_mc_update(mmsr_mc, meas, cov_meas, n_particles, gt, i, steps,
plot_cond, save_path, use_pos):
"""
Fusion using MMSR-MC; creates particle density in square root space of measurements and approximates it as a
Gaussian distribution to fuse it with the current estimate in Kalman fashion
:param mmsr_mc: Current estimate (also stores error); will be modified as a result
:param meas: Measurement in original state space
:param cov_meas: Covariance of measurement in original state space
:param n_particles: Number of particles used for approximating the transformed density
:param gt: Ground truth
:param i: Current measurement step
:param steps: Total measurement steps
:param plot_cond: Boolean determining whether to plot the current estimate
:param save_path: Path to save the plots
:param use_pos: If false, use particles only for shape parameters and fuse position in Kalman fashion
"""
# convert measurement
meas_sr, cov_meas_sr, particles_meas = single_particle_approx_gaussian(
meas, cov_meas, n_particles, use_pos)
# store prior for plotting
m_prior = mmsr_mc['x'][M]
l_prior, w_prior, al_prior = get_ellipse_params_from_sr(mmsr_mc['x'][SR])
# Kalman fusion
S = mmsr_mc['cov'] + cov_meas_sr
K = np.dot(mmsr_mc['cov'], np.linalg.inv(S))
mmsr_mc['x'] = mmsr_mc['x'] + np.dot(K, meas_sr - mmsr_mc['x'])
mmsr_mc['cov'] = mmsr_mc['cov'] - np.dot(np.dot(K, S), K.T)
# save error and plot estimate
l_post_sr, w_post_sr, al_post_sr = get_ellipse_params_from_sr(
mmsr_mc['x'][SR])
mmsr_mc['error'][i::steps] += error_and_plotting(
mmsr_mc['x'][M],
l_post_sr,
w_post_sr,
al_post_sr,
m_prior,
l_prior,
w_prior,
al_prior,
meas[M],
meas[L],
meas[W],
meas[AL],
gt[M],
gt[L],
gt[W],
gt[AL],
plot_cond,
'MC Approximated Fusion',
save_path + 'exampleMCApprox%i.svg' % i,
est_color='green')
def mmsr_pf_update(mmsr_pf, meas, cov_meas, particles_pf, n_particles_pf, gt,
i, steps, plot_cond, save_path, use_pos):
"""
Fuse using MMSR-PF; keep estimate in square root space as particle density and update the weights over time; for the
likelihood, the particles are transformed back and the sum of the likelihoods for all 4 possible representations is
used
:param mmsr_pf: Current estimate (also stores error); will be modified as a result
:param meas: Measurement in original state space
:param cov_meas: Covariance of measurement in original state space
:param particles_pf: The particles of the particle filter in square root space
:param n_particles_pf: The number of particles
:param gt: Ground truth
:param i: Current measurement step
:param steps: Total measurement steps
:param plot_cond: Boolean determining whether to plot the current estimate
:param save_path: Path to save the plots
:param use_pos: If false, use particles only for shape parameters and fuse position in Kalman fashion
"""
# store prior for plotting
m_prior = mmsr_pf['x'][M]
l_prior, w_prior, al_prior = get_ellipse_params_from_sr(mmsr_pf['x'][SR])
# use particle filter
mmsr_pf['x'], mmsr_pf['weights'], mmsr_pf['cov'][:2, :2] = particle_filter(
particles_pf, mmsr_pf['weights'], meas, cov_meas, n_particles_pf,
'sum', mmsr_pf['x'][M], mmsr_pf['cov'][:2, :2], use_pos)
# save error and plot estimate
l_post, w_post, al_post = get_ellipse_params_from_sr(mmsr_pf['x'][SR])
mmsr_pf['error'][i::steps] += error_and_plotting(mmsr_pf['x'][M],
l_post,
w_post,
al_post,
m_prior,
l_prior,
w_prior,
al_prior,
meas[M],
meas[L],
meas[W],
meas[AL],
gt[M],
gt[L],
gt[W],
gt[AL],
plot_cond,
'MMGW-PF',
save_path +
'exampleMMGWPF%i.svg' % i,
est_color='green')
def mmgw_mc_update(mmgw_mc, meas, cov_meas, n_particles, gt, step_id, steps, plot_cond, save_path):
"""
Fusion using MMGW-MC; creates particle density in square root space of measurements and approximates it as a
Gaussian distribution to fuse it with the current estimate in Kalman fashion.
:param mmgw_mc: Current estimate (also stores error); will be modified as a result
:param meas: Measurement in ellipse parameter space
:param cov_meas: Covariance of measurement in ellipse parameter space
:param n_particles: Number of particles used for approximating the transformed density
:param gt: Ground truth
:param step_id: Current measurement step
:param steps: Total measurement steps
:param plot_cond: Boolean determining whether to plot the current estimate
:param save_path: Path to save the plots
"""
# predict
mmgw_mc['x'] = np.dot(F, mmgw_mc['x'])
error_mat = np.array([
[0.5 * T ** 2, 0.0],
[0.0, 0.5 * T ** 2],
[0.0, 0.0],
[0.0, 0.0],
[0.0, 0.0],
[T, 0.0],
[0.0, T],
])
error_cov = np.dot(np.dot(error_mat, np.diag([SIGMA_V1, SIGMA_V2]) ** 2), error_mat.T)
error_cov[SR, SR] = np.asarray(SIGMA_SHAPE) ** 2
mmgw_mc['cov'] = np.dot(np.dot(F, mmgw_mc['cov']), F.T) + error_cov
# convert measurement
meas_sr, cov_meas_sr, particles_meas = single_particle_approx_gaussian(meas, cov_meas, n_particles)
# store prior for plotting
m_prior = mmgw_mc['x'][M]
al_prior, l_prior, w_prior = get_ellipse_params_from_sr(mmgw_mc['x'][SR])
# Kalman fusion
innov_cov = np.dot(np.dot(H, mmgw_mc['cov']), H.T) + cov_meas_sr
gain = np.dot(np.dot(mmgw_mc['cov'], H.T), np.linalg.inv(innov_cov))
mmgw_mc['x'] = mmgw_mc['x'] + np.dot(gain, meas_sr - np.dot(H, mmgw_mc['x']))
mmgw_mc['cov'] = mmgw_mc['cov'] - np.dot(np.dot(gain, innov_cov), gain.T)
# save error and plot estimate
al_post_sr, l_post_sr, w_post_sr = get_ellipse_params_from_sr(mmgw_mc['x'][SR])
mmgw_mc['error'][step_id::steps] += error_and_plotting(mmgw_mc['x'][M], l_post_sr, w_post_sr, al_post_sr, m_prior,
l_prior, w_prior, al_prior, meas[M], meas[L], meas[W],
meas[AL], gt[M], gt[L], gt[W], gt[AL], plot_cond,
'MC Approximated Fusion',
save_path + 'exampleMCApprox%i.svg' % step_id,
est_color='green')
def test_convergence(steps, runs, prior, cov_prior, cov_meas, random_param,
save_path):
"""
Test convergence of error for different fusion methods. Creates plot of root mean square error convergence and
errors at first and last measurement step.
:param steps: Number of measurements
:param runs: Number of MC runs
:param prior: Prior prediction mean (ground truth will be drawn from it each run)
:param cov_prior: Prior prediction covariance (ground truth will be drawn from it each run)
:param cov_meas: Noise of sensor
:param random_param: use random parameter switch to simulate ambiguous parameterization
:param save_path: Path for saving figures
"""
error = np.zeros(steps * 2)
# setup state for various ellipse fusion methods
mmgw_mc = np.zeros(1, dtype=state_dtype)
mmgw_mc[0]['error'] = error.copy()
mmgw_mc[0]['name'] = 'MC-MMGW'
mmgw_mc[0]['color'] = 'cyan'
regular = np.zeros(1, dtype=state_dtype)
regular[0]['error'] = error.copy()
regular[0]['name'] = 'Regular'
regular[0]['color'] = 'red'
regular_mmgw = np.zeros(1, dtype=state_dtype)
regular_mmgw[0]['error'] = error.copy()
regular_mmgw[0]['name'] = 'Regular-MMGW'
regular_mmgw[0]['color'] = 'orange'
red_mmgw = np.zeros(1, dtype=state_dtype)
red_mmgw[0]['error'] = error.copy()
red_mmgw[0]['name'] = 'RED-MMGW'
red_mmgw[0]['color'] = 'green'
# red_mmgw_r = np.zeros(1, dtype=state_dtype)
# red_mmgw_r[0]['error'] = error.copy()
# red_mmgw_r[0]['name'] = 'RED-MMGW-r'
# red_mmgw_r[0]['color'] = 'lightgreen'
# red_mmgw_s = np.zeros(1, dtype=state_dtype)
# red_mmgw_s[0]['error'] = error.copy()
# red_mmgw_s[0]['name'] = 'RED-MMGW-s'
# red_mmgw_s[0]['color'] = 'turquoise'
shape_mean = np.zeros(1, dtype=state_dtype)
shape_mean[0]['error'] = error.copy()
shape_mean[0]['name'] = 'Shape-Mean'
shape_mean[0]['color'] = 'magenta'
# rt_red = 0.0
# rt_red_r = 0.0
# rt_red_s = 0.0
for r in range(runs):
print('Run %i of %i' % (r + 1, runs))
# initialize ===================================================================================================
# create gt from prior
gt = sample_m(prior, cov_prior, False, 1)
# ellipse orientation should be velocity orientation
vel = np.linalg.norm(gt[V])
gt[V] = np.array(np.cos(gt[AL]), np.sin(gt[AL])) * vel
# get prior in square root space
mmgw_mc[0]['x'], mmgw_mc[0][
'cov'], particles_mc = single_particle_approx_gaussian(
prior, cov_prior, N_PARTICLES_MMGW)
mmgw_mc[0]['est'] = mmgw_mc[0]['x'].copy()
mmgw_mc[0]['est'][SR] = get_ellipse_params_from_sr(mmgw_mc[0]['x'][SR])
mmgw_mc[0]['figure'], mmgw_mc[0]['axes'] = plt.subplots(1, 1)
# get prior for regular state
regular[0]['x'] = prior.copy()
regular[0]['cov'] = cov_prior.copy()
regular[0]['est'] = prior.copy()
regular[0]['figure'], regular[0]['axes'] = plt.subplots(1, 1)
regular_mmgw[0]['x'] = prior.copy()
regular_mmgw[0]['cov'] = cov_prior.copy()
particles = sample_m(regular_mmgw[0]['x'], regular_mmgw[0]['cov'],
False, N_PARTICLES_MMGW)
regular_mmgw[0]['est'] = mmgw_estimate_from_particles(particles)
regular_mmgw[0]['figure'], regular_mmgw[0]['axes'] = plt.subplots(1, 1)
# get prior for red
red_mmgw[0]['x'], red_mmgw[0]['cov'], red_mmgw[0][
'comp_weights'] = turn_mult(prior.copy(), cov_prior.copy())
particles = sample_mult(red_mmgw[0]['x'], red_mmgw[0]['cov'],
red_mmgw[0]['comp_weights'], N_PARTICLES_MMGW)
red_mmgw[0]['est'] = mmgw_estimate_from_particles(particles)
red_mmgw[0]['figure'], red_mmgw[0]['axes'] = plt.subplots(1, 1)
# red_mmgw_r[0]['x'], red_mmgw_r[0]['cov'], red_mmgw_r[0]['comp_weights'] = turn_mult(prior.copy(), cov_prior.copy())
# particles = sample_mult(red_mmgw_r[0]['x'], red_mmgw_r[0]['cov'], red_mmgw_r[0]['comp_weights'], N_PARTICLES_MMGW)
# red_mmgw_r[0]['est'] = mmgw_estimate_from_particles(particles)
# red_mmgw_r[0]['figure'], red_mmgw_r[0]['axes'] = plt.subplots(1, 1)
#
# red_mmgw_s[0]['x'], red_mmgw_s[0]['cov'], red_mmgw_s[0]['comp_weights'] = turn_mult(prior.copy(),
# cov_prior.copy())
# particles = sample_mult(red_mmgw_s[0]['x'], red_mmgw_s[0]['cov'], red_mmgw_s[0]['comp_weights'],
# N_PARTICLES_MMGW)
# red_mmgw_s[0]['est'] = mmgw_estimate_from_particles(particles)
# red_mmgw_s[0]['figure'], red_mmgw_s[0]['axes'] = plt.subplots(1, 1)
# get prior for RM mean
shape_mean[0]['x'] = prior[KIN]
shape_mean[0]['shape'] = to_matrix(prior[AL], prior[L], prior[W],
False)
shape_mean[0]['cov'] = cov_prior[KIN][:, KIN]
shape_mean[0]['gamma'] = 6.0
shape_mean[0]['figure'], shape_mean[0]['axes'] = plt.subplots(1, 1)
# test different methods
for i in range(steps):
if i % 10 == 0:
print('Step %i of %i' % (i + 1, steps))
plot_cond = (r + 1 == runs) & ((i % 2) == 1) # & (i + 1 == steps)
# move ground truth
gt = np.dot(F, gt)
error_mat = np.array([
[0.5 * T**2, 0.0],
[0.0, 0.5 * T**2],
[T, 0.0],
[0.0, T],
])
kin_cov = np.dot(
np.dot(error_mat,
np.diag([SIGMA_V1, SIGMA_V2])**2), error_mat.T)
gt[KIN] += mvn(np.zeros(len(KIN)), kin_cov)
gt[AL] = np.arctan2(gt[V2], gt[V1])
# create measurement from gt (using alternating sensors) ===================================================
k = np.random.randint(0, 4) if random_param else 0
gt_mean = gt.copy()
if k % 2 == 1:
l_save = gt_mean[L]
gt_mean[L] = gt_mean[W]
gt_mean[W] = l_save
gt_mean[AL] = (gt_mean[AL] + 0.5 * np.pi * k +
np.pi) % (2 * np.pi) - np.pi
meas = sample_m(np.dot(H, gt_mean), cov_meas, False, 1)
# fusion methods ===========================================================================================
mmgw_mc_update(mmgw_mc[0], meas.copy(), cov_meas.copy(),
N_PARTICLES_MMGW, gt, i, steps, plot_cond,
save_path)
regular_update(regular[0], meas.copy(), cov_meas.copy(), gt, i,
steps, plot_cond, save_path, False)
regular_update(regular_mmgw[0], meas.copy(), cov_meas.copy(), gt,
i, steps, plot_cond, save_path, True)
# tic = time.time()
red_update(red_mmgw[0],
meas.copy(),
cov_meas.copy(),
gt,
i,
steps,
plot_cond,
save_path,
True,
mixture_reduction='salmond')
# toc = time.time()
# rt_red += toc - tic
# tic = time.time()
# red_update(red_mmgw_r[0], meas.copy(), cov_meas.copy(), gt, i, steps, plot_cond, save_path, True,
# mixture_reduction='salmond', pruning=False)
# toc = time.time()
# rt_red_r += toc - tic
# tic = time.time()
# red_update(red_mmgw_s[0], meas.copy(), cov_meas.copy(), gt, i, steps, plot_cond, save_path, True,
# mixture_reduction='salmond')
# toc = time.time()
# rt_red_s += toc - tic
shape_mean_update(
shape_mean[0], meas.copy(), cov_meas.copy(), gt, i, steps,
plot_cond, save_path, 0.2 if cov_meas[AL, AL] < 0.1 * np.pi
else 5.0 if cov_meas[AL, AL] < 0.4 * np.pi else 10.0)
plt.close(mmgw_mc[0]['figure'])
plt.close(regular[0]['figure'])
plt.close(regular_mmgw[0]['figure'])
plt.close(red_mmgw[0]['figure'])
# plt.close(red_mmgw_r[0]['figure'])
# plt.close(red_mmgw_s[0]['figure'])
plt.close(shape_mean[0]['figure'])
mmgw_mc[0]['error'] = np.sqrt(mmgw_mc[0]['error'] / runs)
regular[0]['error'] = np.sqrt(regular[0]['error'] / runs)
regular_mmgw[0]['error'] = np.sqrt(regular_mmgw[0]['error'] / runs)
red_mmgw[0]['error'] = np.sqrt(red_mmgw[0]['error'] / runs)
# red_mmgw_r[0]['error'] = np.sqrt(red_mmgw_r[0]['error'] / runs)
# red_mmgw_s[0]['error'] = np.sqrt(red_mmgw_s[0]['error'] / runs)
shape_mean[0]['error'] = np.sqrt(shape_mean[0]['error'] / runs)
# print('Runtime RED:')
# print(rt_red / (runs*steps))
# print('Runtime RED no pruning:')
# print(rt_red_r / (runs * steps))
# print('Runtime RED-S:')
# print(rt_red_s / (runs * steps))
# error plotting ===================================================================================================
plot_error_bars(
np.block([regular, regular_mmgw, shape_mean, mmgw_mc, red_mmgw]),
steps)
plot_convergence(
np.block([regular, regular_mmgw, shape_mean, mmgw_mc, red_mmgw]),
steps, save_path)
def test_mean(orig, cov, n_particles, save_path):
"""
Compare the mean in original state space with mean in square root space (via MC approximation) in regards of their
GW and ESR error.
:param orig: Mean in original state space
:param cov: Covariance for original state space
:param n_particles: Number of particles for MC approximation of SR space mean
:param save_path: Path for saving figures
"""
# approximate mean in SR space
vec_mmsr, _, vec_particle = single_particle_approx_gaussian(
orig, cov, n_particles, True)
mat_mmsr = np.array([[vec_mmsr[2], vec_mmsr[3]],
[vec_mmsr[3], vec_mmsr[4]]])
mat_mmsr = np.dot(mat_mmsr, mat_mmsr)
al_mmsr, l_mmsr, w_mmsr = get_ellipse_params(mat_mmsr)
# approximate mean in matrix space
mat_mat = np.zeros((2, 2))
vec_mat = np.zeros(2)
for i in range(len(vec_particle)):
vec_mat += vec_particle[i, :2]
mat = np.array([[vec_particle[i, 2], vec_particle[i, 3]],
[vec_particle[i, 3], vec_particle[i, 4]]])
mat_mat += np.dot(mat, mat)
vec_mat /= len(vec_particle)
mat_mat /= len(vec_particle)
al_mat, l_mat, w_mat = get_ellipse_params(mat_mat)
# caclulate Barycenter using optimization
covs_sr = np.zeros((n_particles, 2, 2))
covs_sr[:, 0, 0] = vec_particle[:, 2]
covs_sr[:, 0, 1] = vec_particle[:, 3]
covs_sr[:, 1, 0] = vec_particle[:, 3]
covs_sr[:, 1, 1] = vec_particle[:, 4]
covs = np.einsum('xab, xbc -> xac', covs_sr, covs_sr)
bary_particles = np.zeros((n_particles, 5))
bary_particles[:, M] = vec_particle[:, M]
bary_particles[:, 2] = covs[:, 0, 0]
bary_particles[:, 3] = covs[:, 0, 1]
bary_particles[:, 4] = covs[:, 1, 1]
bary = barycenter(bary_particles,
np.ones(n_particles) / n_particles,
n_particles,
particles_sr=vec_particle)
mat_mmgw = np.array([
[bary[2], bary[3]],
[bary[3], bary[4]],
])
al_mmgw, l_mmgw, w_mmgw = get_ellipse_params(mat_mmgw)
# approximate error
error_orig_gw = 0
error_orig_sr = 0
error_mmsr_gw = 0
error_mmsr_sr = 0
error_mat_gw = 0
error_mat_sr = 0
error_mmgw_gw = 0
error_mmgw_sr = 0
for i in range(n_particles):
mat_particle = np.array([[vec_particle[i, 2], vec_particle[i, 3]],
[vec_particle[i, 3], vec_particle[i, 4]]])
mat_particle = np.dot(mat_particle, mat_particle)
al_particle, l_particle, w_particle = get_ellipse_params(mat_particle)
error_orig_gw += gauss_wasserstein(vec_particle[i, :2], l_particle,
w_particle, al_particle, orig[M],
orig[L], orig[W], orig[AL])
error_orig_sr += square_root_distance(vec_particle[i, :2], l_particle,
w_particle, al_particle, orig[M],
orig[L], orig[W], orig[AL])
error_mmsr_gw += gauss_wasserstein(vec_particle[i, :2], l_particle,
w_particle, al_particle,
vec_mmsr[M], l_mmsr, w_mmsr,
al_mmsr)
error_mmsr_sr += square_root_distance(vec_particle[i, :2], l_particle,
w_particle, al_particle,
vec_mmsr[M], l_mmsr, w_mmsr,
al_mmsr)
error_mat_gw += gauss_wasserstein(vec_particle[i, :2], l_particle,
w_particle, al_particle, vec_mat[M],
l_mat, w_mat, al_mat)
error_mat_sr += square_root_distance(vec_particle[i, :2], l_particle,
w_particle, al_particle,
vec_mat[M], l_mat, w_mat, al_mat)
error_mmgw_gw += gauss_wasserstein(vec_particle[i, :2], l_particle,
w_particle, al_particle, bary[M],
l_mmgw, w_mmgw, al_mmgw)
error_mmgw_sr += square_root_distance(vec_particle[i, :2], l_particle,
w_particle, al_particle, bary[M],
l_mmgw, w_mmgw, al_mmgw)
error_orig_gw = np.sqrt(error_orig_gw / n_particles)
error_orig_sr = np.sqrt(error_orig_sr / n_particles)
error_mmsr_gw = np.sqrt(error_mmsr_gw / n_particles)
error_mmsr_sr = np.sqrt(error_mmsr_sr / n_particles)
error_mat_gw = np.sqrt(error_mat_gw / n_particles)
error_mat_sr = np.sqrt(error_mat_sr / n_particles)
error_mmgw_gw = np.sqrt(error_mmgw_gw / n_particles)
error_mmgw_sr = np.sqrt(error_mmgw_sr / n_particles)
fig, ax = plt.subplots(1, 1)
samples = np.random.choice(n_particles, 20, replace=False)
for i in range(20):
al_particle, l_particle, w_particle = get_ellipse_params_from_sr(
vec_particle[samples[i], SR])
el_particle = Ellipse(
(vec_particle[samples[i], X1], vec_particle[samples[i], X2]),
2 * l_particle,
2 * w_particle,
np.rad2deg(al_particle),
fill=True,
linewidth=2.0)
el_particle.set_alpha(0.4)
el_particle.set_fc('grey')
ax.add_artist(el_particle)
el_gt = Ellipse((orig[X1], orig[X2]),
2 * orig[L],
2 * orig[W],
np.rad2deg(orig[AL]),
fill=False,
linewidth=2.0)
el_gt.set_alpha(0.7)
el_gt.set_ec('red')
ax.add_artist(el_gt)
plt.plot([0], [0], color='red', label='Euclidean')
ela_final = Ellipse((vec_mat[X1], vec_mat[X2]),
2 * l_mat,
2 * w_mat,
np.rad2deg(al_mat),
fill=False,
linewidth=2.0)
ela_final.set_alpha(0.7)
ela_final.set_ec('magenta')
ax.add_artist(ela_final)
plt.plot([0], [0], color='magenta', label='Shape mean')
elb_final = Ellipse((vec_mmsr[X1], vec_mmsr[X2]),
2 * l_mmsr,
2 * w_mmsr,
np.rad2deg(al_mmsr),
fill=False,
linewidth=2.0)
elb_final.set_alpha(0.7)
elb_final.set_ec('lightgreen')
ax.add_artist(elb_final)
plt.plot([0], [0], color='lightgreen', label='ESR')
el_res = Ellipse((bary[X1], bary[X2]),
2 * l_mmgw,
2 * w_mmgw,
np.rad2deg(al_mmgw),
fill=False,
linewidth=2.0,
linestyle='--')
el_res.set_alpha(0.7)
el_res.set_ec('green')
ax.add_artist(el_res)
plt.plot([0], [0], color='green', label='GW')
plt.axis([-10 + orig[0], 10 + orig[0], -10 + orig[1], 10 + orig[1]])
ax.set_aspect('equal')
ax.set_title('MMSE Estimates')
plt.xlabel('x in m')
plt.ylabel('y in m')
plt.legend()
tikzplotlib.save(save_path + 'mmseEstimates.tex',
add_axis_environment=False)
# plt.savefig(save_path + 'mmseEstimates.svg')
plt.show()
# print error
print('RMGW of original:')
print(error_orig_gw)
print('RMSR of original:')
print(error_orig_sr)
print('RMGW of mmsr:')
print(error_mmsr_gw)
print('RMSR of mmsr:')
print(error_mmsr_sr)
print('RMGW of Euclidean mmse:')
print(error_mat_gw)
print('RMSR of Euclidean mmse:')
print(error_mat_sr)
print('RMGW of mmgw_bary:')
print(error_mmgw_gw)
print('RMSR of mmgw_bary:')
print(error_mmgw_sr)
bars = np.arange(1, 8, 2)
ticks = np.array(['Euclidean', 'Shape mean', 'MMSR', 'MMGW'])
plt.bar(bars[0], error_orig_gw, width=0.25, color='red', align='center')
plt.bar(bars[1], error_mat_gw, width=0.25, color='magenta', align='center')
plt.bar(bars[2],
error_mmsr_gw,
width=0.25,
color='lightgreen',
align='center')
plt.bar(bars[3], error_mmgw_gw, width=0.25, color='black', align='center')
plt.xticks(bars, ticks)
plt.title('GW RMSE')
plt.savefig(save_path + 'meanTestGW.svg')
plt.show()
def mmsr_lin2_update(mmsr_lin2, meas, cov_meas, gt, i, steps, plot_cond,
save_path):
"""
Fuse using MMSR-Lin2; store state in square root space and estimate measurement in square root space by transforming
the measurement covariance using Hessians of the transformation function; Hessian formulas based on M. Roth and
F. Gustafsson, “An Efficient Implementation of the Second Order Extended Kalman Filter,” in Proceedings of the 14th
International Conference on Information Fusion (Fusion 2011), Chicago, Illinois, USA, July 2011.
:param mmsr_lin2: Current estimate (also stores error); will be modified as a result
:param meas: Measurement in original state space
:param cov_meas: Covariance of measurement in original state space
:param gt: Ground truth
:param i: Current measurement step
:param steps: Total measurement steps
:param plot_cond: Boolean determining whether to plot the current estimate
:param save_path: Path to save the plots
"""
# convert measurement
shape_meas_sr = to_matrix(meas[AL], meas[L], meas[W], True)
# store prior for plotting
m_prior = mmsr_lin2['x'][M]
l_prior, w_prior, al_prior = get_ellipse_params_from_sr(mmsr_lin2['x'][SR])
# precalculate values
cossin = np.cos(meas[AL]) * np.sin(meas[AL])
cos2 = np.cos(meas[AL])**2
sin2 = np.sin(meas[AL])**2
# transform per element
meas_lin2 = np.zeros(5)
meas_lin2[M] = meas[M]
hess = np.zeros((3, 5, 5))
hess[0] = np.array([
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[
0, 0, 2 * (meas[W] - meas[L]) * (cos2 - sin2), -2 * cossin,
2 * cossin
],
[0, 0, -2 * cossin, 0, 0],
[0, 0, 2 * cossin, 0, 0],
])
meas_lin2[2] = shape_meas_sr[0,
0] + 0.5 * np.trace(np.dot(hess[0], cov_meas))
hess[1] = np.array([
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[0, 0, -4 * (meas[W] - meas[L]) * cossin, cos2 - sin2, sin2 - cos2],
[0, 0, cos2 - sin2, 0, 0],
[0, 0, sin2 - cos2, 0, 0],
])
meas_lin2[3] = shape_meas_sr[0,
1] + 0.5 * np.trace(np.dot(hess[1], cov_meas))
hess[2] = np.array([
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[
0, 0, 2 * (meas[L] - meas[W]) * (cos2 - sin2), 2 * cossin,
-2 * cossin
],
[0, 0, 2 * cossin, 0, 0],
[0, 0, -2 * cossin, 0, 0],
])
meas_lin2[4] = shape_meas_sr[1,
1] + 0.5 * np.trace(np.dot(hess[2], cov_meas))
# transform covariance per element
jac = get_jacobian(meas[L], meas[W], meas[AL])
cov_meas_lin2 = np.dot(np.dot(jac, cov_meas), jac.T)
# add Hessian part where Hessian not 0
for k in range(3):
for l in range(3):
cov_meas_lin2[k + 2, l + 2] += 0.5 * np.trace(
np.dot(np.dot(np.dot(hess[k], cov_meas), hess[l]), cov_meas))
# Kalman fusion
S_lin = mmsr_lin2['cov'] + cov_meas_lin2
S_lin_inv = np.linalg.inv(S_lin)
if np.iscomplex(S_lin_inv).any():
print(cov_meas_lin2)
print(S_lin_inv)
K_lin = np.dot(mmsr_lin2['cov'], S_lin_inv)
mmsr_lin2['x'] = mmsr_lin2['x'] + np.dot(K_lin, meas_lin2 - mmsr_lin2['x'])
mmsr_lin2['cov'] = mmsr_lin2['cov'] - np.dot(np.dot(K_lin, S_lin), K_lin.T)
# save error and plot estimate
l_post, w_post, al_post = get_ellipse_params_from_sr(mmsr_lin2['x'][SR])
mmsr_lin2['error'][i::steps] += error_and_plotting(
mmsr_lin2['x'][M], l_post, w_post, al_post, m_prior, l_prior, w_prior,
al_prior, meas[M], meas[L], meas[W], meas[AL], gt[M], gt[L], gt[W],
gt[AL], plot_cond, 'Linearization', save_path + 'exampleLin%i.svg' % i)
