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)