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)