def regular_update(regular, meas, cov_meas, gt, i, steps, plot_cond,
save_path):
"""
Fuse estimate and measurement in original state space in Kalman fashion
:param regular: 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
"""
# store prior for plotting
all_prior = regular['x'].copy()
# Kalman fusion
S = regular['cov'] + cov_meas
K = np.dot(regular['cov'], np.linalg.inv(S))
regular['x'] = regular['x'] + np.dot(K, meas - regular['x'])
regular['cov'] = regular['cov'] - np.dot(np.dot(K, S), K.T)
# save error and plot estimate
regular['error'][i::steps] += error_and_plotting(
regular['x'][M], regular['x'][L], regular['x'][W], regular['x'][AL],
all_prior[M], all_prior[L], all_prior[W], all_prior[AL], meas[M],
meas[L], meas[W], meas[AL], gt[M], gt[L], gt[W], gt[AL], plot_cond,
'Fusion of Original State', save_path + 'exampleRegFus%i.svg' % i)
def mwdp_update(mwdp, meas, cov_meas, gt, i, steps, plot_cond, save_path):
"""
Fuse using MWDP; use likelihood to determine representation of ellipse in original state space with
smallest Euclidean distance weighted by uncertainties and fuse original state representations in Kalman fashion
using best representation
:param mwdp: 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
"""
# store prior for plotting
all_prior = mwdp['x'].copy()
# Kalman fusion using best representation
[mwdp['x'], mwdp['cov'], k] = mwdp_fusion(mwdp['x'], mwdp['cov'], meas,
cov_meas)
# save error and plot estimate
mwdp['error'][i::steps] += error_and_plotting(
mwdp['x'][M], mwdp['x'][L], mwdp['x'][W], mwdp['x'][AL], all_prior[M],
all_prior[L], all_prior[W], all_prior[AL], meas[M], meas[L], meas[W],
meas[AL], gt[M], gt[L], gt[W], gt[AL], plot_cond, 'Best Params',
save_path + 'exampleBestParams%i.svg' % i)
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 regular_update(regular, meas, cov_meas, gt, step_id, steps, plot_cond, save_path, use_mmgw):
"""
Fuse estimate and measurement in original state space in Kalman fashion.
:param regular: 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 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 use_mmgw: Use the MMGW instead of the ordinary estimate
"""
# predict
regular['x'] = np.dot(F, regular['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] = SIGMA_SHAPE ** 2
regular['cov'] = np.dot(np.dot(F, regular['cov']), F.T) + error_cov
# store prior for plotting
all_prior = regular['est'].copy()
# Kalman fusion
innov = meas - np.dot(H, regular['x'])
innov[AL] = (innov[AL] + np.pi) % (2*np.pi) - np.pi
innov_cov = np.dot(np.dot(H, regular['cov']), H.T) + cov_meas
gain = np.dot(np.dot(regular['cov'], H.T), np.linalg.inv(innov_cov))
regular['x'] = regular['x'] + np.dot(gain, innov)
regular['cov'] = regular['cov'] - np.dot(np.dot(gain, innov_cov), gain.T)
if use_mmgw:
particles = sample_m(regular['x'], regular['cov'], False, N_PARTICLES_MMGW)
regular['est'] = mmgw_estimate_from_particles(particles)
else:
regular['est'] = regular['x'].copy()
# save error and plot estimate
regular['error'][step_id::steps] += error_and_plotting(regular['est'][M], regular['est'][L], regular['est'][W],
regular['est'][AL], all_prior[M], all_prior[L], all_prior[W],
all_prior[AL], meas[M], meas[L], meas[W], meas[AL], gt[M],
gt[L], gt[W], gt[AL], plot_cond, regular['name'],
save_path + 'example' + regular['name'] + '%i.svg' % step_id)
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 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 red_update(red, meas, cov_meas, gt, step_id, steps, plot_cond, save_path, use_mmgw):
"""
Method utilizing RED. Fuses the four components of the RED with orientation between
0 and 2pi with those of the measurement RED and applies mixture reduction on the resulting multi modal density. The
mean of the density is estimated by taking the mean of the highest weighted component or by using the MMGW
estimator.
:param red: The state containing mean, covariance, etc.
:param meas: Measurement
:param cov_meas: Measurement covariance
:param gt: Ground truth (for plotting and error calculation)
:param step_id: Current step index (for plotting and error calculation)
:param steps: Total number of steps
:param plot_cond: Boolean for plotting the current time step
:param save_path: Path to save the plots
:param use_mmgw: Use the MMGW instead of the highest weight estimate
"""
# predict
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] = SIGMA_SHAPE ** 2
for i in range(len(red['x'])):
red['x'][i] = np.dot(F, red['x'][i])
red['cov'][i] = np.dot(np.dot(F, red['cov'][i]), F.T) + error_cov
prior = red['est']
# transform prior and measurement into reduced REDs
meas_mult, meas_cov_mult, meas_weights = turn_mult(meas, cov_meas)
# calculate posterior RED
post_mult = np.zeros((len(red['x']) * len(meas_mult), len(red['x'][0])))
post_cov_mult = np.zeros((len(red['x']) * len(meas_mult), len(red['x'][0]), len(red['x'][0])))
post_weights = np.zeros(len(red['x']) * len(meas_mult))
log_prior_weights = np.log(red['comp_weights'])
for i in range(len(red['x'])):
for j in range(len(meas_mult)):
nu = meas_mult[j] - np.dot(H, red['x'][i])
nu[AL] = (nu[AL] + np.pi) % (2 * np.pi) - np.pi
nu_cov = np.dot(np.dot(H, red['cov'][i]), H.T) + meas_cov_mult[j]
post_mult[i * len(meas_mult) + j] = red['x'][i] + np.dot(np.dot(np.dot(red['cov'][i], H.T),
np.linalg.inv(nu_cov)), nu)
post_cov_mult[i * len(meas_mult) + j] = red['cov'][i] - np.dot(np.dot(np.dot(red['cov'][i], H.T),
np.linalg.inv(nu_cov)),
np.dot(H, red['cov'][i]))
post_weights[i * len(meas_mult) + j] = -2.5 * np.log(2 * np.pi) - 0.5 * np.log(np.linalg.det(nu_cov)) \
- 0.5 * np.dot(np.dot(nu, np.linalg.inv(nu_cov)), nu)
post_weights[i * len(meas_mult) + j] += log_prior_weights[i]
post_weights -= np.log(np.sum(np.exp(post_weights)))
post_weights = np.exp(post_weights)
red['x'], red['cov'], red['comp_weights'] = reduce_mult(post_mult, post_cov_mult, post_weights)
if use_mmgw:
particles = sample_mult(post_mult, post_cov_mult, post_weights, N_PARTICLES_MMGW)
red['est'] = mmgw_estimate_from_particles(particles)
else:
red['est'] = red['x'][np.argmax(red['comp_weights'])].copy()
red['error'][step_id::steps] += error_and_plotting(red['est'][M], red['est'][L], red['est'][W], red['est'][AL],
prior[M], prior[L], prior[W], prior[AL], meas[M], meas[L],
meas[W], meas[AL], gt[M], gt[L], gt[W], gt[AL], plot_cond,
red['name'],
save_path + 'example' + red['name'] + '%i.svg' % step_id)
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)