def gauss_wasserstein(m_x, l_x, w_x, al_x, m_y, l_y, w_y, al_y):
"""
Calculate the Gaussian Wasserstein distance of two ellipses.
:param m_x: Center of first ellipse
:param l_x: Semi-axis length of first ellipse
:param w_x: Semi-axis width of first ellipse
:param al_x: Orientation of first ellipse
:param m_y: Center of second ellipse
:param l_y: Semi-axis length of second ellipse
:param w_y: Semi-axis width of second ellipse
:param al_y: Orientation of second ellipse
:return: The Gaussian Wasserstein distance of the two ellipses
"""
gt_xc = m_x
gt_sigma = to_matrix(al_x, l_x, w_x, False)
gt_sigma += gt_sigma.T
gt_sigma /= 2
track_xc = m_y
track_sigma = to_matrix(al_y, l_y, w_y, False)
track_sigma += track_sigma.T
track_sigma /= 2
error = norm(gt_xc -
track_xc)**2 + np.trace(gt_sigma + track_sigma - 2 * sqrtm(
np.einsum('ab, bc, cd -> ad', sqrtm(gt_sigma),
track_sigma, sqrtm(gt_sigma))))
return error
def shape_mean_update(shape_mean, meas, cov_meas, gt, step_id, steps, plot_cond, save_path, tau=1.0):
"""
Treat ellipse estimates as random matrices having received an equal degree.
:param shape_mean: 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 (only m is used)
: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
:param tau: forget parameter of prediction step
"""
# store prior for plotting
m_prior = shape_mean['x'][M]
al_prior, l_prior, w_prior = get_ellipse_params(shape_mean['shape'])
# predict
shape_mean['x'] = np.dot(F[KIN][:, KIN], shape_mean['x'])
error_mat = np.array([
[0.5 * T ** 2, 0.0],
[0.0, 0.5 * T ** 2],
[T, 0.0],
[0.0, T],
])
error_cov = np.dot(np.dot(error_mat, np.diag([SIGMA_V1, SIGMA_V2]) ** 2), error_mat.T)
shape_mean['cov'] = np.dot(np.dot(F[KIN][:, KIN], shape_mean['cov']), F[KIN][:, KIN].T) + error_cov
shape_mean['gamma'] = 6.0 + np.exp(-T / tau)*(shape_mean['gamma'] - 6.0)
# convert measurement
shape_meas = to_matrix(meas[AL], meas[L], meas[W], False)
# Kalman fusion
innov_cov_k = np.dot(np.dot(H_SHAPE, shape_mean['cov']), H_SHAPE.T) + cov_meas[KIN_MEAS][:, KIN_MEAS]
gain_k = np.dot(np.dot(shape_mean['cov'], H_SHAPE.T), np.linalg.inv(innov_cov_k))
shape_mean['x'] = shape_mean['x'] + np.dot(gain_k, meas[KIN_MEAS] - np.dot(H_SHAPE, shape_mean['x']))
shape_mean['cov'] = shape_mean['cov'] - np.dot(np.dot(gain_k, innov_cov_k), gain_k.T)
shape_mean['shape'] = (shape_mean['gamma'] * shape_mean['shape'] + 6.0 * shape_meas) / (shape_mean['gamma'] + 6.0)
shape_mean['gamma'] += 6.0
# save error and plot estimate
al_post, l_post, w_post = get_ellipse_params(shape_mean['shape'])
shape_mean['error'][step_id::steps] += error_and_plotting(shape_mean['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,
shape_mean['name'],
save_path + 'example' + shape_mean['name'] + '%i.svg'
% step_id)
def rm_mean_update(rm_mean, meas, cov_meas, gt, i, steps, plot_cond,
save_path):
"""
Treat ellipse estimates as random matrices having received an equal number of measurements and fuse as proposed by
K. Granström and U. Orguner, “On Spawning and Combination of Extended/Group Targets Modeled With Random
Matrices,” IEEE Transactions on Signal Processing, vol. 61, no. 3, pp. 678–692, 2013.
:param rm_mean: Current estimate (also stores error); will be modified as a result
:param meas: Measurement in original state space (only m is used)
:param cov_meas: Covariance of measurement in original state space (only m is used)
: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 = to_matrix(meas[AL], meas[L], meas[W], False)
# store prior for plotting
m_prior = rm_mean['x'][M]
l_prior, w_prior, al_prior = get_ellipse_params(rm_mean['shape'])
# Kalman fusion
S_k = rm_mean['cov'][:2, :2] + cov_meas[:2, :2]
K_k = np.dot(rm_mean['cov'][:2, :2], np.linalg.inv(S_k))
rm_mean['x'][M] = rm_mean['x'][M] + np.dot(K_k, meas[M] - rm_mean['x'][M])
rm_mean['cov'][:2, :2] = rm_mean['cov'][:2, :2] - np.dot(
np.dot(K_k, S_k), K_k.T)
rm_mean['shape'] = (rm_mean['gamma'] * rm_mean['shape'] + shape_meas) / (rm_mean['gamma'] + 1.0) \
+ (rm_mean['gamma'] / (rm_mean['gamma'] + 1.0)**2) * np.outer(m_prior-meas[M], m_prior-meas[M])
rm_mean['gamma'] += 1
# save error and plot estimate
l_post, w_post, al_post = get_ellipse_params(rm_mean['shape'])
rm_mean['error'][i::steps] += error_and_plotting(
rm_mean['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, 'RM Mean', save_path + 'exampleRMMean%i.svg' % i)
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_convergence_pos(steps, runs, prior, cov_prior, cov_a, cov_b,
n_particles, n_particles_pf, 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. If a uniform prior is given for alpha, ength, and width, the particle
based methods use it while the others use the Gaussian prior (assumed to be a Gaussian approximation of the uniform
prior). In either case, the position is still Gaussian.
: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_a: Noise of sensor A
:param cov_b: Noise of sensor B
:param n_particles: Number of particles for MMGW-MC
:param n_particles_pf: Number of particles for MMGW-PF
:param save_path: Path for saving figures
"""
error = np.zeros(steps * 2)
# setup state for various ellipse fusion methods
mmsr_mc = np.zeros(1, dtype=state_dtype)
mmsr_mc[0]['error'] = error.copy()
mmsr_mc[0]['name'] = 'MMSR-MC'
mmsr_mc[0]['color'] = 'lightgreen'
mmsr_pf = np.zeros(1, dtype=state_dtype)
mmsr_pf[0]['error'] = error.copy()
mmsr_pf[0]['name'] = 'MMSR-PF'
mmsr_pf[0]['color'] = 'darkgreen'
regular = np.zeros(1, dtype=state_dtype)
regular[0]['error'] = error.copy()
regular[0]['name'] = 'Regular'
regular[0]['color'] = 'red'
mwdp = np.zeros(1, dtype=state_dtype)
mwdp[0]['error'] = error.copy()
mwdp[0]['name'] = 'MWDP'
mwdp[0]['color'] = 'darkcyan'
rm_mean = np.zeros(1, dtype=state_dtype)
rm_mean[0]['error'] = error.copy()
rm_mean[0]['name'] = 'RM Mean'
rm_mean[0]['color'] = 'orange'
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)
# get prior in square root space
mmsr_mc[0]['x'], mmsr_mc[0][
'cov'], particles_mc = single_particle_approx_gaussian(
prior, cov_prior, n_particles, False)
# get prior for regular state
regular[0]['x'] = prior.copy()
regular[0]['cov'] = cov_prior.copy()
# get prior for MWDP
mwdp[0]['x'] = prior.copy()
mwdp[0]['cov'] = cov_prior.copy()
# get prior for RM mean
rm_mean[0]['x'] = prior.copy()
rm_mean[0]['shape'] = to_matrix(prior[AL], prior[L], prior[W], False)
rm_mean[0]['cov'] = cov_prior.copy()
rm_mean[0]['gamma'] = 1
# get prior for particle filter
mmsr_pf[0]['x'], mmsr_pf[0][
'cov'], particles_pf = single_particle_approx_gaussian(
prior, cov_prior, n_particles_pf, False)
mmsr_pf[0]['weights'] = np.ones(n_particles_pf) / n_particles_pf
# 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 + 1 == steps)
# create measurement from gt (using alternating sensors) ===================================================
if (i % 2) == 0:
meas = sample_m(gt, cov_b, True, 1)
cov_meas = cov_b.copy()
else:
meas = sample_m(gt, cov_a, False, 1)
cov_meas = cov_a.copy()
# fusion methods ===========================================================================================
mmsr_mc_update(mmsr_mc[0], meas, cov_meas, n_particles, gt, i,
steps, plot_cond, save_path, False)
regular_update(regular[0], meas, cov_meas, gt, i, steps, plot_cond,
save_path)
mwdp_update(mwdp[0], meas, cov_meas, gt, i, steps, plot_cond,
save_path)
rm_mean_update(rm_mean[0], meas, cov_meas, gt, i, steps, plot_cond,
save_path)
mmsr_pf_update(mmsr_pf[0], meas, cov_meas, particles_pf,
n_particles_pf, gt, i, steps, plot_cond, save_path,
False)
mmsr_mc[0]['error'] = np.sqrt(mmsr_mc[0]['error'] / runs)
mmsr_pf[0]['error'] = np.sqrt(mmsr_pf[0]['error'] / runs)
regular[0]['error'] = np.sqrt(regular[0]['error'] / runs)
mwdp[0]['error'] = np.sqrt(mwdp[0]['error'] / runs)
rm_mean[0]['error'] = np.sqrt(rm_mean[0]['error'] / runs)
print(mmsr_pf['error'])
print(rm_mean['error'])
# error plotting ===================================================================================================
plot_error_bars(np.block([regular, rm_mean, mmsr_mc, mwdp, mmsr_pf]),
steps)
plot_convergence(np.block([regular, rm_mean, mmsr_mc, mwdp, mmsr_pf]),
steps, save_path)
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)
def test_convergence_pos(steps, runs, prior, cov_prior, cov_a, cov_b,
n_particles, n_particles_pf, 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. If a uniform prior is given for alpha, ength, and width, the particle
based methods use it while the others use the Gaussian prior (assumed to be a Gaussian approximation of the uniform
prior). In either case, the position is still Gaussian.
: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_a: Noise of sensor A
:param cov_b: Noise of sensor B
:param n_particles: Number of particles for MMGW-MC
:param n_particles_pf: Number of particles for MMGW-PF
:param save_path: Path for saving figures
"""
error = np.zeros(steps * 2)
# setup state for various ellipse fusion methods
mmsr_mc = np.zeros(1, dtype=state_dtype)
mmsr_mc[0]['error'] = error.copy()
mmsr_mc[0]['name'] = 'MMSR-MC'
mmsr_mc[0]['color'] = 'greenyellow'
mmsr_lin2 = np.zeros(1, dtype=state_dtype)
mmsr_lin2[0]['error'] = error.copy()
mmsr_lin2[0]['name'] = 'MMSR-Lin'
mmsr_lin2[0]['color'] = 'm'
mmsr_pf = np.zeros(1, dtype=state_dtype)
mmsr_pf[0]['error'] = error.copy()
mmsr_pf[0]['name'] = 'MMSR-PF'
mmsr_pf[0]['color'] = 'darkgreen'
regular = np.zeros(1, dtype=state_dtype)
regular[0]['error'] = error.copy()
regular[0]['name'] = 'Regular'
regular[0]['color'] = 'red'
mwdp = np.zeros(1, dtype=state_dtype)
mwdp[0]['error'] = error.copy()
mwdp[0]['name'] = 'MWDP'
mwdp[0]['color'] = 'deepskyblue'
rm_mean = np.zeros(1, dtype=state_dtype)
rm_mean[0]['error'] = error.copy()
rm_mean[0]['name'] = 'RM Mean'
rm_mean[0]['color'] = 'orange'
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)
# get prior in square root space
mmsr_mc[0]['x'], mmsr_mc[0][
'cov'], particles_mc = single_particle_approx_gaussian(
prior, cov_prior, n_particles, False)
# get prior for regular state
regular[0]['x'] = prior.copy()
regular[0]['cov'] = cov_prior.copy()
# get prior for MWDP
mwdp[0]['x'] = prior.copy()
mwdp[0]['cov'] = cov_prior.copy()
# get prior for RM mean
rm_mean[0]['x'] = prior.copy()
rm_mean[0]['shape'] = to_matrix(prior[AL], prior[L], prior[W], False)
rm_mean[0]['cov'] = cov_prior.copy()
rm_mean[0]['gamma'] = 1
# get prior for linearization
# precalculate values
prior_shape_sqrt = to_matrix(prior[AL], prior[L], prior[W], True)
cossin = np.cos(prior[AL]) * np.sin(prior[AL])
cos2 = np.cos(prior[AL])**2
sin2 = np.sin(prior[AL])**2
# transform per element
mmsr_lin2[0]['x'][M] = prior[M]
hess = np.zeros((3, 5, 5))
hess[0] = np.array([
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[
0, 0, 2 * (prior[W] - prior[L]) * (cos2 - sin2), -2 * cossin,
2 * cossin
],
[0, 0, -2 * cossin, 0, 0],
[0, 0, 2 * cossin, 0, 0],
])
mmsr_lin2[0]['x'][2] = prior_shape_sqrt[0, 0] + 0.5 * np.trace(
np.dot(hess[0], cov_prior))
hess[1] = np.array([
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[
0, 0, -4 * (prior[W] - prior[L]) * cossin, cos2 - sin2,
sin2 - cos2
],
[0, 0, cos2 - sin2, 0, 0],
[0, 0, sin2 - cos2, 0, 0],
])
mmsr_lin2[0]['x'][3] = prior_shape_sqrt[0, 1] + 0.5 * np.trace(
np.dot(hess[1], cov_prior))
hess[2] = np.array([
[0, 0, 0, 0, 0],
[0, 0, 0, 0, 0],
[
0, 0, 2 * (prior[L] - prior[W]) * (cos2 - sin2), 2 * cossin,
-2 * cossin
],
[0, 0, 2 * cossin, 0, 0],
[0, 0, -2 * cossin, 0, 0],
])
mmsr_lin2[0]['x'][4] = prior_shape_sqrt[1, 1] + 0.5 * np.trace(
np.dot(hess[2], cov_prior))
# transform covariance per element
jac = get_jacobian(prior[L], prior[W], prior[AL])
mmsr_lin2[0]['cov'] = np.dot(np.dot(jac, cov_prior), jac.T)
# add Hessian part where Hessian not 0
for i in range(3):
for j in range(3):
mmsr_lin2[0]['cov'][i + 2, j + 2] += 0.5 * np.trace(
np.dot(np.dot(np.dot(hess[i], cov_prior), hess[j]),
cov_prior))
# get prior for particle filter
mmsr_pf[0]['x'], mmsr_pf[0][
'cov'], particles_pf = single_particle_approx_gaussian(
prior, cov_prior, n_particles_pf, False)
mmsr_pf[0]['weights'] = np.ones(n_particles_pf) / n_particles_pf
# 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 + 1 == steps)
# create measurement from gt (using alternating sensors) ===================================================
if (i % 2) == 0:
meas = sample_m(gt, cov_b, True, 1)
cov_meas = cov_b.copy()
else:
meas = sample_m(gt, cov_a, False, 1)
cov_meas = cov_a.copy()
# fusion methods ===========================================================================================
mmsr_mc_update(mmsr_mc[0], meas, cov_meas, n_particles, gt, i,
steps, plot_cond, save_path, False)
regular_update(regular[0], meas, cov_meas, gt, i, steps, plot_cond,
save_path)
mwdp_update(mwdp[0], meas, cov_meas, gt, i, steps, plot_cond,
save_path)
rm_mean_update(rm_mean[0], meas, cov_meas, gt, i, steps, plot_cond,
save_path)
mmsr_lin2_update(mmsr_lin2[0], meas, cov_meas, gt, i, steps,
plot_cond, save_path)
mmsr_pf_update(mmsr_pf[0], meas, cov_meas, particles_pf,
n_particles_pf, gt, i, steps, plot_cond, save_path,
False)
mmsr_mc[0]['error'] = np.sqrt(mmsr_mc[0]['error'] / runs)
mmsr_lin2[0]['error'] = np.sqrt(mmsr_lin2[0]['error'] / runs)
mmsr_pf[0]['error'] = np.sqrt(mmsr_pf[0]['error'] / runs)
regular[0]['error'] = np.sqrt(regular[0]['error'] / runs)
mwdp[0]['error'] = np.sqrt(mwdp[0]['error'] / runs)
rm_mean[0]['error'] = np.sqrt(rm_mean[0]['error'] / runs)
# error plotting ===================================================================================================
plot_error_bars(
np.block([mmsr_mc, mmsr_pf, regular, mwdp, rm_mean, mmsr_lin2]), steps)
plot_convergence(
np.block([mmsr_mc, mmsr_pf, regular, mwdp, rm_mean, mmsr_lin2]), steps,
save_path)
