├── EllipseFusion ├── FusionMethods │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-36.pyc │ │ ├── approximations.cpython-36.pyc │ │ └── helpers.cpython-36.pyc │ ├── constants.py │ ├── ellipse_fusion_methods.py │ ├── ellipse_fusion_support.py │ ├── error_and_plotting.py │ └── tests.py └── main.py ├── Evaluation ├── assignments_uniform_OSPA_Fig4.m ├── d_gaussian_wasserstein.m ├── d_kullback_leibler.m ├── get_uniform_points_boundary.m ├── hungarian │ ├── Hungarian.m │ └── ospa_dist.m ├── plot_extent.m ├── scenario1_Fig5.m ├── scenario2_Fig5.m └── scenario3_Fig5.m ├── MEM-EKFstar ├── demo.m ├── get_ground_truth.m ├── measurement_update.m ├── plot_extent.m └── time_update.m ├── MEM_EKF ├── d_gaussian_wasserstein.m ├── get_ground_truth.m ├── get_jacobian_hessian.m ├── get_random_matrix_ellipse.m ├── get_random_matrix_state.m ├── measurement_update_ekf.m ├── plot_extent.m ├── predictEKF.m ├── predictRMM.m ├── predictSOEKF.m ├── rmse.m ├── simulation.m ├── updateEKF.m ├── updateRMM.m └── updateSOEKF.m ├── MEM_SOEKF ├── demo.m ├── get_ground_truth.m ├── get_jacobian_hessian.m ├── measurement_update.m ├── plot_extent.m └── readme.md ├── MEOT └── linearJPDA │ ├── MEOT_JPDA.m │ ├── demo.m │ ├── getMeasGt.m │ ├── plot_extent.m │ └── prob_measurement_update.m └── RandomHypersurfaceModel └── randomHypersurfaceModel_2011.m /EllipseFusion/FusionMethods/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Fusion-Goettingen/ExtendedObjectTracking/716c66f078162f7891a40e5ef664643fd74b9101/EllipseFusion/FusionMethods/__init__.py -------------------------------------------------------------------------------- /EllipseFusion/FusionMethods/__pycache__/__init__.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Fusion-Goettingen/ExtendedObjectTracking/716c66f078162f7891a40e5ef664643fd74b9101/EllipseFusion/FusionMethods/__pycache__/__init__.cpython-36.pyc -------------------------------------------------------------------------------- /EllipseFusion/FusionMethods/__pycache__/approximations.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Fusion-Goettingen/ExtendedObjectTracking/716c66f078162f7891a40e5ef664643fd74b9101/EllipseFusion/FusionMethods/__pycache__/approximations.cpython-36.pyc -------------------------------------------------------------------------------- /EllipseFusion/FusionMethods/__pycache__/helpers.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Fusion-Goettingen/ExtendedObjectTracking/716c66f078162f7891a40e5ef664643fd74b9101/EllipseFusion/FusionMethods/__pycache__/helpers.cpython-36.pyc -------------------------------------------------------------------------------- /EllipseFusion/FusionMethods/constants.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Kolja Thormann 3 | 4 | Contains constants for consistent array indexing 5 | """ 6 | 7 | # constants for accessing x 8 | M = [0, 1] # center 9 | AL = 2 # orientation 10 | L = 3 # semi-axis length 11 | W = 4 # semi-axis width 12 | SR = [2, 3, 4] # elements of square root matrix 13 | -------------------------------------------------------------------------------- /EllipseFusion/FusionMethods/ellipse_fusion_methods.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Kolja Thormann 3 | 4 | Contains various ellipse fusion methods 5 | """ 6 | 7 | import numpy as np 8 | 9 | from FusionMethods.ellipse_fusion_support import particle_filter, mwdp_fusion, get_ellipse_params,\ 10 | get_ellipse_params_from_sr, single_particle_approx_gaussian, to_matrix 11 | from FusionMethods.error_and_plotting import error_and_plotting 12 | from FusionMethods.constants import * 13 | 14 | 15 | def mmsr_mc_update(mmsr_mc, meas, cov_meas, n_particles, gt, i, steps, plot_cond, save_path, use_pos): 16 | """ 17 | Fusion using MMSR-MC; creates particle density in square root space of measurements and approximates it as a 18 | Gaussian distribution to fuse it with the current estimate in Kalman fashion 19 | :param mmsr_mc: Current estimate (also stores error); will be modified as a result 20 | :param meas: Measurement in original state space 21 | :param cov_meas: Covariance of measurement in original state space 22 | :param n_particles: Number of particles used for approximating the transformed density 23 | :param gt: Ground truth 24 | :param i: Current measurement step 25 | :param steps: Total measurement steps 26 | :param plot_cond: Boolean determining whether to plot the current estimate 27 | :param save_path: Path to save the plots 28 | :param use_pos: If false, use particles only for shape parameters and fuse position in Kalman fashion 29 | """ 30 | # convert measurement 31 | meas_sr, cov_meas_sr, particles_meas = single_particle_approx_gaussian(meas, cov_meas, n_particles, use_pos) 32 | 33 | # store prior for plotting 34 | m_prior = mmsr_mc['x'][M] 35 | l_prior, w_prior, al_prior = get_ellipse_params_from_sr(mmsr_mc['x'][SR]) 36 | 37 | # Kalman fusion 38 | S = mmsr_mc['cov'] + cov_meas_sr 39 | K = np.dot(mmsr_mc['cov'], np.linalg.inv(S)) 40 | mmsr_mc['x'] = mmsr_mc['x'] + np.dot(K, meas_sr - mmsr_mc['x']) 41 | mmsr_mc['cov'] = mmsr_mc['cov'] - np.dot(np.dot(K, S), K.T) 42 | 43 | # save error and plot estimate 44 | l_post_sr, w_post_sr, al_post_sr = get_ellipse_params_from_sr(mmsr_mc['x'][SR]) 45 | mmsr_mc['error'][i::steps] += error_and_plotting(mmsr_mc['x'][M], l_post_sr, w_post_sr, al_post_sr, m_prior, 46 | l_prior, w_prior, al_prior, meas[M], meas[L], meas[W], meas[AL], 47 | gt[M], gt[L], gt[W], gt[AL], plot_cond, 'MC Approximated Fusion', 48 | save_path + 'exampleMCApprox%i.svg' % i, est_color='green') 49 | 50 | 51 | def regular_update(regular, meas, cov_meas, gt, i, steps, plot_cond, save_path): 52 | """ 53 | Fuse estimate and measurement in original state space in Kalman fashion 54 | :param regular: Current estimate (also stores error); will be modified as a result 55 | :param meas: Measurement in original state space 56 | :param cov_meas: Covariance of measurement in original state space 57 | :param gt: Ground truth 58 | :param i: Current measurement step 59 | :param steps: Total measurement steps 60 | :param plot_cond: Boolean determining whether to plot the current estimate 61 | :param save_path: Path to save the plots 62 | """ 63 | # store prior for plotting 64 | all_prior = regular['x'].copy() 65 | 66 | # Kalman fusion 67 | S = regular['cov'] + cov_meas 68 | K = np.dot(regular['cov'], np.linalg.inv(S)) 69 | regular['x'] = regular['x'] + np.dot(K, meas - regular['x']) 70 | regular['cov'] = regular['cov'] - np.dot(np.dot(K, S), K.T) 71 | 72 | # save error and plot estimate 73 | regular['error'][i::steps] += error_and_plotting(regular['x'][M], regular['x'][L], regular['x'][W], 74 | regular['x'][AL], all_prior[M], all_prior[L], all_prior[W], 75 | all_prior[AL], meas[M], meas[L], meas[W], meas[AL], gt[M], gt[L], 76 | gt[W], gt[AL], plot_cond, 'Fusion of Original State', 77 | save_path + 'exampleRegFus%i.svg' % i) 78 | 79 | 80 | def mwdp_update(mwdp, meas, cov_meas, gt, i, steps, plot_cond, save_path): 81 | """ 82 | Fuse using MWDP; use likelihood to determine representation of ellipse in original state space with 83 | smallest Euclidean distance weighted by uncertainties and fuse original state representations in Kalman fashion 84 | using best representation 85 | :param mwdp: Current estimate (also stores error); will be modified as a result 86 | :param meas: Measurement in original state space 87 | :param cov_meas: Covariance of measurement in original state space 88 | :param gt: Ground truth 89 | :param i: Current measurement step 90 | :param steps: Total measurement steps 91 | :param plot_cond: Boolean determining whether to plot the current estimate 92 | :param save_path: Path to save the plots 93 | """ 94 | # store prior for plotting 95 | all_prior = mwdp['x'].copy() 96 | 97 | # Kalman fusion using best representation 98 | [mwdp['x'], mwdp['cov'], k] = mwdp_fusion(mwdp['x'], mwdp['cov'], meas, cov_meas) 99 | 100 | # save error and plot estimate 101 | mwdp['error'][i::steps] += error_and_plotting(mwdp['x'][M], mwdp['x'][L], mwdp['x'][W], mwdp['x'][AL], all_prior[M], 102 | all_prior[L], all_prior[W], all_prior[AL], meas[M], meas[L], meas[W], 103 | meas[AL], gt[M], gt[L], gt[W], gt[AL], plot_cond, 'Best Params', 104 | save_path + 'exampleBestParams%i.svg' % i) 105 | 106 | 107 | def rm_mean_update(rm_mean, meas, cov_meas, gt, i, steps, plot_cond, save_path): 108 | """ 109 | Treat ellipse estimates as random matrices having received an equal number of measurements and fuse as proposed by 110 | K. Granström and U. Orguner, “On Spawning and Combination of Extended/Group Targets Modeled With Random 111 | Matrices,” IEEE Transactions on Signal Processing, vol. 61, no. 3, pp. 678–692, 2013. 112 | :param rm_mean: Current estimate (also stores error); will be modified as a result 113 | :param meas: Measurement in original state space (only m is used) 114 | :param cov_meas: Covariance of measurement in original state space (only m is used) 115 | :param gt: Ground truth 116 | :param i: Current measurement step 117 | :param steps: Total measurement steps 118 | :param plot_cond: Boolean determining whether to plot the current estimate 119 | :param save_path: Path to save the plots 120 | """ 121 | # convert measurement 122 | shape_meas = to_matrix(meas[AL], meas[L], meas[W], False) 123 | 124 | # store prior for plotting 125 | m_prior = rm_mean['x'][M] 126 | l_prior, w_prior, al_prior = get_ellipse_params(rm_mean['shape']) 127 | 128 | # Kalman fusion 129 | S_k = rm_mean['cov'][:2, :2] + cov_meas[:2, :2] 130 | K_k = np.dot(rm_mean['cov'][:2, :2], np.linalg.inv(S_k)) 131 | rm_mean['x'][M] = rm_mean['x'][M] + np.dot(K_k, meas[M] - rm_mean['x'][M]) 132 | rm_mean['cov'][:2, :2] = rm_mean['cov'][:2, :2] - np.dot(np.dot(K_k, S_k), K_k.T) 133 | rm_mean['shape'] = (rm_mean['gamma'] * rm_mean['shape'] + shape_meas) / (rm_mean['gamma'] + 1.0) \ 134 | + (rm_mean['gamma'] / (rm_mean['gamma'] + 1.0)**2) * np.outer(m_prior-meas[M], m_prior-meas[M]) 135 | rm_mean['gamma'] += 1 136 | 137 | # save error and plot estimate 138 | l_post, w_post, al_post = get_ellipse_params(rm_mean['shape']) 139 | rm_mean['error'][i::steps] += error_and_plotting(rm_mean['x'][M], l_post, w_post, al_post, m_prior, l_prior, 140 | w_prior, al_prior, meas[M], meas[L], meas[W], meas[AL], gt[M], 141 | gt[L], gt[W], gt[AL], plot_cond, 'RM Mean', 142 | save_path + 'exampleRMMean%i.svg' % i) 143 | 144 | 145 | def mmsr_pf_update(mmsr_pf, meas, cov_meas, particles_pf, n_particles_pf, gt, i, steps, plot_cond, save_path, use_pos): 146 | """ 147 | Fuse using MMSR-PF; keep estimate in square root space as particle density and update the weights over time; for the 148 | likelihood, the particles are transformed back and the sum of the likelihoods for all 4 possible representations is 149 | used 150 | :param mmsr_pf: Current estimate (also stores error); will be modified as a result 151 | :param meas: Measurement in original state space 152 | :param cov_meas: Covariance of measurement in original state space 153 | :param particles_pf: The particles of the particle filter in square root space 154 | :param n_particles_pf: The number of particles 155 | :param gt: Ground truth 156 | :param i: Current measurement step 157 | :param steps: Total measurement steps 158 | :param plot_cond: Boolean determining whether to plot the current estimate 159 | :param save_path: Path to save the plots 160 | :param use_pos: If false, use particles only for shape parameters and fuse position in Kalman fashion 161 | """ 162 | # store prior for plotting 163 | m_prior = mmsr_pf['x'][M] 164 | l_prior, w_prior, al_prior = get_ellipse_params_from_sr(mmsr_pf['x'][SR]) 165 | 166 | # use particle filter 167 | mmsr_pf['x'], mmsr_pf['weights'], mmsr_pf['cov'][:2, :2] = particle_filter(particles_pf, mmsr_pf['weights'], meas, 168 | cov_meas, n_particles_pf, 'sum', 169 | mmsr_pf['x'][M], mmsr_pf['cov'][:2, :2], 170 | use_pos) 171 | 172 | # save error and plot estimate 173 | l_post, w_post, al_post = get_ellipse_params_from_sr(mmsr_pf['x'][SR]) 174 | mmsr_pf['error'][i::steps] += error_and_plotting(mmsr_pf['x'][M], l_post, w_post, al_post, m_prior, l_prior, 175 | w_prior, al_prior, meas[M], meas[L], meas[W], meas[AL], gt[M], 176 | gt[L], gt[W], gt[AL], plot_cond, 'MMGW-PF', 177 | save_path + 'exampleMMGWPF%i.svg' % i, est_color='green') 178 | -------------------------------------------------------------------------------- /EllipseFusion/FusionMethods/ellipse_fusion_support.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Kolja Thormann 3 | 4 | Contains support functions for the ellipse fusion and test setup 5 | """ 6 | 7 | import numpy as np 8 | from numpy.random import multivariate_normal as mvn 9 | from scipy.linalg import inv, sqrtm 10 | from numpy.linalg import slogdet 11 | 12 | from FusionMethods.constants import * 13 | 14 | 15 | def rot_matrix(alpha): 16 | """ 17 | Calculates a rotation matrix based on the input orientation 18 | :param alpha: Input orientation 19 | :return: Rotation matrix for alpha 20 | """ 21 | rot = np.array([[np.cos(alpha), -np.sin(alpha)], [np.sin(alpha), np.cos(alpha)]]) 22 | if len(rot.shape) == 3: 23 | return rot.transpose((2, 0, 1)) 24 | else: 25 | return rot 26 | 27 | 28 | def to_matrix(alpha, l, w, sr): 29 | """ 30 | Turn ellipse parameters into a matrix or square root matrix depending on sr parameter 31 | :param alpha: Orientation of the ellipse 32 | :param l: Semi-axis length of the ellipse 33 | :param w: Semi-axis width of the ellipse 34 | :param sr: If True, square root matrix is calculated instead of shape matrix 35 | :return: Shape or square root matrix depending of sr 36 | """ 37 | p = 1 if sr else 2 38 | rot = rot_matrix(alpha) 39 | if len(rot.shape) == 3: 40 | lw_diag = np.array([np.diag([l[i], w[i]]) for i in range(len(l))]) 41 | return np.einsum('xab, xbc, xdc -> xad', rot, lw_diag ** p, rot) 42 | else: 43 | return np.dot(np.dot(rot, np.diag([l, w]) ** p), rot.T) 44 | 45 | 46 | def get_ellipse_params(ell): 47 | """ 48 | Calculate the ellipse semi-axis length and width and orientation based on shape matrix 49 | :param ell: Input ellipse as 2x2 shape matrix 50 | :return: Semi-axis length, width and orientation of input ellipse 51 | """ 52 | ellipse_axis, v = np.linalg.eig(ell) 53 | ellipse_axis = np.sqrt(ellipse_axis) 54 | l = ellipse_axis[0] 55 | w = ellipse_axis[1] 56 | al = np.arctan2(v[1, 0], v[0, 0]) 57 | 58 | return l, w, al 59 | 60 | 61 | def get_ellipse_params_from_sr(sr): 62 | """ 63 | Calculate ellipse semi-axis length and width and orientation based on the elements of the square root matrix 64 | :param sr: Elements of the square root matrix [top-left, corner, bottom-right] 65 | :return: Semi-axis length, width and orientation of input square root matrix 66 | """ 67 | # calculate shape matrix based on square root matrix elements 68 | ell_sr = np.array([ 69 | [sr[0], sr[1]], 70 | [sr[1], sr[2]], 71 | ]) 72 | ell = np.dot(ell_sr, ell_sr) 73 | 74 | return get_ellipse_params(ell) 75 | 76 | 77 | def single_particle_approx_gaussian(prior, cov, n_particles, use_pos, sr=True): 78 | """ 79 | Calculate the particle density of the prior in square root space and approximate it as a Gaussian 80 | :param prior: Prior in original state space 81 | :param cov: Covariance of the prior 82 | :param n_particles: Number of particles used for particle approximation 83 | :param use_pos: Boolean to determine whether to create the particle cloud for the entire or only the shape state 84 | :param sr: Utilize square root matrix or normal matrix for particles 85 | :return: Approximated mean and covariance in square root space and the particle density 86 | """ 87 | vec_sr = np.zeros((n_particles, 5)) 88 | 89 | particle = sample_m(prior, cov, False, n_particles) # sample particles from the prior density 90 | if not use_pos: 91 | particle[:, M] = prior[M] 92 | 93 | # transform particles into square root space 94 | for i in range(n_particles): 95 | # calculate square root 96 | Pa_sr = to_matrix(particle[i, AL], particle[i, L], particle[i, W], sr) 97 | 98 | # save transformed particle 99 | vec_sr[i] = np.array([particle[i, 0], particle[i, 1], Pa_sr[0, 0], Pa_sr[0, 1], Pa_sr[1, 1]]) 100 | 101 | # calculate mean and variance of particle density 102 | if use_pos: 103 | mean_sr = np.sum(vec_sr, axis=0) / n_particles 104 | var_sr = np.sum(np.einsum('xa, xb -> xab', vec_sr - mean_sr, vec_sr - mean_sr), axis=0) / n_particles 105 | else: 106 | mean_sr = np.zeros(5) 107 | var_sr = np.zeros((5, 5)) 108 | mean_sr[M] = prior[M] 109 | var_sr[:2, :2] = cov[:2, :2] 110 | mean_sr[SR] = np.sum(vec_sr[:, SR], axis=0) / n_particles 111 | var_sr[2:, 2:] = np.sum(np.einsum('xa, xb -> xab', vec_sr[:, SR] - mean_sr[SR], vec_sr[:, SR] - mean_sr[SR]), 112 | axis=0) / n_particles 113 | var_sr += var_sr.T 114 | var_sr *= 0.5 115 | 116 | return mean_sr, var_sr, vec_sr 117 | 118 | 119 | def sample_m(mean, cov, shift, amount): 120 | """ 121 | Create one or multiple samples 122 | :param mean: Mean from which to sample 123 | :param cov: Covariance with which to sample 124 | :param shift: Boolean to determine whether to shift the orientation 125 | :param amount: Number of samples to be drawn 126 | :return: The sample for amount==1 and an array of samples for amount > 1 127 | """ 128 | # shift mean by 90 degree and switch length and width if demanded 129 | s_mean = mean.copy() 130 | if shift: 131 | save_w = s_mean[W] 132 | s_mean[W] = s_mean[L] 133 | s_mean[L] = save_w 134 | s_mean[AL] += 0.5 * np.pi 135 | 136 | # draw sample 137 | samp = mvn(s_mean, cov, amount) 138 | 139 | # enforce restrictions 140 | samp[:, L] = np.maximum(0.1, samp[:, L]) 141 | samp[:, W] = np.maximum(0.1, samp[:, W]) 142 | samp[:, AL] %= 2 * np.pi 143 | 144 | # if only one sample, do not store it in a 1d array 145 | if amount == 1: 146 | samp = samp[0] 147 | 148 | return samp 149 | 150 | 151 | def particle_filter(prior, w, meas, cov_meas, n_particles, ll_type, m_prior, cov_m, use_pos): 152 | """ 153 | Update the particle cloud's (in SR space) weights based on a measurement in original state space; no resampling 154 | :param prior: Prior particle density in SR space (shape space if use_bary is True) 155 | :param w: Prior weights 156 | :param meas: Measured ellipse in original state space 157 | :param cov_meas: Covariance of measurement in original state space 158 | :param n_particles: Number of particles to be sampled from the Gaussian input density 159 | :param ll_type: Either 'sum' for using the sum of the 4 representations' likelihood or 'max' for using the 160 | maximum 161 | :param m_prior: Prior of position parameters (only used if use_pos is false) 162 | :param cov_m: Covariance of position parameters (only used if use_pos is false) 163 | :param use_pos: If false, use particles only for shape parameters and fuse position in Kalman fashion 164 | :return: Weighted mean of the particles and updated weights 165 | """ 166 | 167 | meas_mm, cov_meas_mm = turn_to_multi_modal(meas, cov_meas) 168 | # calculate inverse and determinant of measurement covariance assuming independent measurement dimensions 169 | # if use_pos: 170 | # cov_meas_inv = np.diag(1.0 / cov_meas.diagonal()) 171 | # cov_meas_det = np.linalg.det(cov_meas) 172 | # else: 173 | # cov_meas_inv = np.diag(1.0 / cov_meas[2:, 2:].diagonal()) 174 | # cov_meas_det = np.linalg.det(cov_meas[2:, 2:]) 175 | if use_pos: 176 | cov_meas_inv = np.zeros(cov_meas_mm.shape) 177 | cov_meas_det = np.zeros(len(cov_meas_mm)) 178 | else: 179 | cov_meas_inv = np.zeros(cov_meas_mm[:, 2:, 2:].shape) 180 | cov_meas_det = np.zeros(len(cov_meas_mm)) 181 | for i in range(4): 182 | if use_pos: 183 | cov_meas_inv[i] = np.diag(1.0 / cov_meas_mm[i].diagonal()) 184 | cov_meas_det[i] = np.linalg.det(cov_meas_mm[i]) 185 | else: 186 | cov_meas_inv[i] = np.diag(1.0 / cov_meas_mm[i, 2:, 2:].diagonal()) 187 | cov_meas_det[i] = np.linalg.det(cov_meas_mm[i, 2:, 2:]) 188 | 189 | # update weights with likelihood 190 | for i in range(n_particles): 191 | w[i] *= sr_likelihood(prior[i], cov_meas_inv[0], cov_meas_det[0], meas_mm[0], ll_type) 192 | w /= np.sum(w) 193 | 194 | # calculate weighted mean with updated particle weights 195 | post_mean = np.sum(w[:, None] * prior, axis=0) 196 | 197 | if not use_pos: 198 | S = cov_meas[:2, :2] + cov_m 199 | K = np.dot(cov_m, inv(S)) 200 | post_mean[M] = m_prior + np.dot(K, meas[M] - m_prior) 201 | cov_m = cov_m - np.dot(np.dot(K, S), K.T) 202 | 203 | return post_mean, w, cov_m 204 | 205 | 206 | def sr_likelihood(x, cov_inv, cov_det, meas, ll_type): 207 | """ 208 | Calculate the likelihood of a particle in SR space given a measurement in original state space 209 | :param x: The particle in SR space 210 | :param cov_inv: Inverse of measurement covariance 211 | :param cov_det: Determinant of measurement covariance 212 | :param meas: Measurement in original state space 213 | :param ll_type: Either 'sum' for using the sum of the 4 representations' likelihood or 'max' for using the maximum 214 | :return: Depedning on ll_type the sum or maximum of the likelihoods or 0 for an invalid ll_type 215 | """ 216 | # transform particle to original state space 217 | x_el = np.zeros(5) 218 | x_el[:2] = x[:2] 219 | x_shape = np.array([ 220 | [x[2], x[3]], 221 | [x[3], x[4]], 222 | ]) 223 | x_shape = np.dot(x_shape, x_shape) 224 | l, w, al = get_ellipse_params(x_shape) 225 | x_el[2] = al - 0.5*np.pi 226 | x_el[2] %= 2.0*np.pi 227 | x_el[3] = w 228 | x_el[4] = l 229 | 230 | ll = np.zeros(4) 231 | 232 | # calculate likelihood for all 4 representations in original state space 233 | for i in range(4): 234 | save = x_el[3] 235 | x_el[3] = x_el[4] 236 | x_el[4] = save 237 | x_el[2] += 0.5*np.pi 238 | x_el[2] %= 2.0*np.pi 239 | 240 | nu = meas - x_el 241 | nu[2] = ((nu[2] + np.pi) % (2*np.pi)) - np.pi 242 | 243 | if len(cov_inv) == 5: 244 | ll[i] = np.exp(-0.5*np.dot(np.dot(nu, cov_inv), nu)) / np.sqrt(32*np.pi**5*cov_det) 245 | else: 246 | ll[i] = np.exp(-0.5 * np.dot(np.dot(nu[2:], cov_inv), nu[2:])) / np.sqrt(8 * np.pi ** 3 * cov_det) 247 | 248 | # return sum or maximum depending on ll_type 249 | if ll_type == 'sum': 250 | return np.sum(ll) 251 | elif ll_type == 'max': 252 | return np.max(ll) 253 | else: 254 | print('Invalid likelihood type') 255 | return 0 256 | 257 | 258 | def mwdp_fusion(prior, cov_prior, meas, cov_meas): 259 | """ 260 | Fuse ellipses A and B in original state space using representation with highest likelihood; assumes independent 261 | measurement dimensions for the switch of covariance elements 262 | :param prior: Prior ellipse in original state space 263 | :param cov_prior: Covariance of prior ellipse 264 | :param meas: Measured ellipse in original state space 265 | :param cov_meas: Covariance of measured ellipse 266 | :return: Mean and covariance of fusion with highest likelihood representation and number of 90 degree 267 | shifts of ellipse B to get highest likelihood representation 268 | """ 269 | res_orig_alt_rots = np.zeros((4, 5)) 270 | res_orig_alt_rots_cov = np.zeros((4, 5, 5)) 271 | res_orig_log_lik = np.zeros(4) 272 | innov = np.zeros((4, 5)) 273 | meas_alt = np.zeros(5) 274 | meas_alt[M] = meas[M] 275 | 276 | # test all 4 representations 277 | for k in range(4): 278 | # shift orientation and if necessary switch semi-axis in mean and orientation 279 | meas_alt[AL] = (meas[AL] + k * np.pi * 0.5) % (2 * np.pi) 280 | if k % 2 != 0: 281 | meas_alt[L] = meas[W] 282 | meas_alt[W] = meas[L] 283 | cov_meas_alt = np.copy(cov_meas) 284 | cov_meas_alt[3, 3] = cov_meas[4, 4] 285 | cov_meas_alt[4, 4] = cov_meas[3, 3] 286 | else: 287 | meas_alt[L] = meas[L] 288 | meas_alt[W] = meas[W] 289 | cov_meas_alt = np.copy(cov_meas) 290 | 291 | # Kalman update 292 | S_orig_alt = cov_prior + cov_meas_alt 293 | K_orig_alt = np.dot(cov_prior, np.linalg.inv(S_orig_alt)) 294 | innov[k] = meas_alt - prior 295 | # use shorter angle difference 296 | innov[k, 2] = ((innov[k, 2] + np.pi) % (2*np.pi)) - np.pi 297 | res_orig_alt_rots[k] = prior + np.dot(K_orig_alt, innov[k]) 298 | res_orig_alt_rots_cov[k] = cov_prior - np.dot(np.dot(K_orig_alt, S_orig_alt), K_orig_alt.T) 299 | 300 | # calculate log-likelihood 301 | res_orig_log_lik[k] = -0.5 * np.dot(np.dot(innov[k], inv(S_orig_alt)), innov[k]) 302 | sign, logdet_inv = slogdet(inv(S_orig_alt)) 303 | res_orig_log_lik[k] += 0.5 * logdet_inv - 2.5 * np.log(2 * np.pi) 304 | 305 | return res_orig_alt_rots[np.argmax(res_orig_log_lik)], res_orig_alt_rots_cov[np.argmax(res_orig_log_lik)],\ 306 | np.argmax(res_orig_log_lik) 307 | 308 | 309 | def get_jacobian(l, w, al): 310 | """ 311 | Jacobian calculation of square root function for positive semi-definite matrices with eigenvalues l and w and 312 | rotation matrix with orientation al 313 | :param l: Semi-axis length of original representation 314 | :param w: Semi-axis width of original representation 315 | :param al: Orientation of original representation 316 | :return: Jacobian of square root transformation 317 | """ 318 | jac = np.zeros((5, 5)) 319 | jac[:2, :2] = np.eye(2) 320 | 321 | jac[2, 2] = 2 * np.cos(al) * np.sin(al) * (w - l) 322 | jac[2, 3] = np.cos(al)**2 323 | jac[2, 4] = np.sin(al)**2 324 | 325 | jac[3, 2] = (l - w) * (np.cos(al)**2 - np.sin(al)**2) 326 | jac[3, 3] = np.sin(al) * np.cos(al) 327 | jac[3, 4] = -np.sin(al) * np.cos(al) 328 | 329 | jac[4, 2] = 2 * np.cos(al) * np.sin(al) * (l - w) 330 | jac[4, 3] = np.sin(al)**2 331 | jac[4, 4] = np.cos(al)**2 332 | 333 | return jac 334 | 335 | 336 | def barycenter(particles, w, n_particles, particles_sr=np.zeros(0)): 337 | """ 338 | Determine Barycenter of particles in shape space via optimization (based on G. Puccetti, L. Rüschendorf, and 339 | S. Vanduffel, “On the Computation of Wasserstein Barycenters,” Available at SSRN 3276147, 2018). 340 | :param particles: Particles in shape space [m1, m2, c11, c12, c22] with cnm being members of the covariance 341 | matrix 342 | :param w: Weights of the particles 343 | :param n_particles: Number of particles 344 | :param particles_sr: Particles with shape parameters in SR form; if given, used for initial guess 345 | :return: Barycenter of the particles as [m1, m2, c11, c12, c22] 346 | """ 347 | # Calculate covariances 348 | covs = np.zeros((n_particles, 2, 2)) 349 | covs[:, 0, 0] = particles[:, 2] 350 | covs[:, 0, 1] = particles[:, 3] 351 | covs[:, 1, 0] = particles[:, 3] 352 | covs[:, 1, 1] = particles[:, 4] 353 | 354 | # Calculate Barycenter 355 | if len(particles_sr) > 0: 356 | covs_sr = np.zeros((n_particles, 2, 2)) 357 | covs_sr[:, 0, 0] = particles_sr[:, 2] 358 | covs_sr[:, 0, 1] = particles_sr[:, 3] 359 | covs_sr[:, 1, 0] = particles_sr[:, 3] 360 | covs_sr[:, 1, 1] = particles_sr[:, 4] 361 | bary_sr = np.sum(w[:, None, None] * covs_sr, axis=0) 362 | bary = np.dot(bary_sr, bary_sr) 363 | else: 364 | bary = np.eye(2) 365 | bary_sr = np.eye(2) 366 | conv = False 367 | # loop until convergence 368 | while not conv: 369 | res = np.zeros((n_particles, 2, 2)) 370 | for i in range(n_particles): 371 | res[i] = np.dot(np.dot(bary_sr, covs[i]), bary_sr) 372 | res[i] = sqrtm(res[i]) * w[i] 373 | bary_new = np.sum(res, axis=0) 374 | bary_sr = sqrtm(bary_new) 375 | 376 | # check convergence 377 | # bary_new = np.dot(bary_sr, bary_sr) 378 | diff = np.sum(abs(bary_new - bary)) 379 | conv = diff < 1e-6 380 | bary = bary_new 381 | 382 | # Calculate mean and Barycenter in SR space 383 | result = np.zeros(5) 384 | result[M] = np.sum(w[:, None] * particles[:, M], axis=0) 385 | result[2] = bary[0, 0] 386 | result[3] = bary[0, 1] 387 | result[4] = bary[1, 1] 388 | 389 | return result 390 | 391 | 392 | def turn_to_multi_modal(mean, cov): 393 | """ 394 | Turns an ellipse estimate in original state space into a multi modal density consisting of 4 components, one for 395 | each way of representing the same ellipse (reducing the number of possibilities to 4 utilizing the 2pi periodity of 396 | the orientation). 397 | :param mean: Mean of the density 398 | :param cov: Covariance of the density 399 | :return: Means and covariances of all 4 modes 400 | """ 401 | mmsr_mult_mean = np.zeros((4, 5)) 402 | mmsr_mult_cov = np.zeros((4, 5, 5)) 403 | mmsr_mult_mean[0] = mean.copy() 404 | mmsr_mult_cov[0] = cov.copy() 405 | for i in range(1, 4): 406 | mmsr_mult_mean[i, :2] = mmsr_mult_mean[i - 1, :2] 407 | mmsr_mult_mean[i, 2] = (mmsr_mult_mean[i - 1, 2] + 0.5 * np.pi) % (2 * np.pi) 408 | mmsr_mult_mean[i, 3] = mmsr_mult_mean[i - 1, 4] 409 | mmsr_mult_mean[i, 4] = mmsr_mult_mean[i - 1, 3] 410 | mmsr_mult_cov[i, :3, :3] = mmsr_mult_cov[i - 1, :3, :3] 411 | mmsr_mult_cov[i, 3, 3] = mmsr_mult_cov[i - 1, 4, 4] 412 | mmsr_mult_cov[i, 4, 4] = mmsr_mult_cov[i - 1, 3, 3] 413 | 414 | return mmsr_mult_mean, mmsr_mult_cov 415 | -------------------------------------------------------------------------------- /EllipseFusion/FusionMethods/error_and_plotting.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Kolja Thormann 3 | 4 | Contains functions for calculating and plotting the error and ellipses 5 | """ 6 | 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | from numpy.linalg import norm 10 | from scipy.linalg import sqrtm 11 | from matplotlib.patches import Ellipse 12 | 13 | from FusionMethods.ellipse_fusion_support import rot_matrix, to_matrix 14 | 15 | 16 | def error_and_plotting(m_res, l_res, w_res, al_res, m_prior, l_prior, w_prior, al_prior, m_meas, l_meas, w_meas, 17 | al_meas, m_gt, l_gt, w_gt, al_gt, plotting, name, filename, est_color='red'): 18 | """ 19 | If demanded, plot ellipse prior, estimate, measurement, and ground truth and provide the error as GW and SR distance 20 | :param m_res: Center of estimated ellipse 21 | :param l_res: Semi-axis length of estimated ellipse 22 | :param w_res: Semi-axis width of estimated ellipse 23 | :param al_res: Orientation of estimated ellipse 24 | :param m_prior: Center of prior ellipse 25 | :param l_prior: Semi-axis length of prior ellipse 26 | :param w_prior: Semi-axis width of prior ellipse 27 | :param al_prior: Orientation of prior ellipse 28 | :param m_meas: Center of measured ellipse 29 | :param l_meas: Semi-axis length of measured ellipse 30 | :param w_meas: Semi-axis width of measured ellipse 31 | :param al_meas: Orientation of measured ellipse 32 | :param m_gt: Center of ground truth ellipse 33 | :param l_gt: Semi-axis length of ground truth ellipse 34 | :param w_gt: Semi-axis width of ground truth ellipse 35 | :param al_gt: Orientation of ground truth ellipse 36 | :param plotting: Boolean to determine whether to plot the ellipses 37 | :param name: Name of the approach 38 | :param filename: Name of the file the plot is to be saved in 39 | :param est_color: Color for plotting the estimated ellipse 40 | :return: Error of the estimate in Gaussian Wasserstein and Square Root distance 41 | """ 42 | if plotting: 43 | plot_ellipses(m_res, l_res, w_res, al_res, m_prior, l_prior, w_prior, al_prior, m_meas, l_meas, w_meas, al_meas, 44 | m_gt, l_gt, w_gt, al_gt, name, filename, est_color) 45 | 46 | return [gauss_wasserstein(m_gt, l_gt, w_gt, al_gt, m_res, l_res, w_res, al_res), 47 | square_root_distance(m_gt, l_gt, w_gt, al_gt, m_res, l_res, w_res, al_res)] 48 | 49 | 50 | def gauss_wasserstein(m_x, l_x, w_x, al_x, m_y, l_y, w_y, al_y): 51 | """ 52 | Calculate the Gaussian Wasserstein distance of two ellipses 53 | :param m_x: Center of first ellipse 54 | :param l_x: Semi-axis length of first ellipse 55 | :param w_x: Semi-axis width of first ellipse 56 | :param al_x: Orientation of first ellipse 57 | :param m_y: Center of second ellipse 58 | :param l_y: Semi-axis length of second ellipse 59 | :param w_y: Semi-axis width of second ellipse 60 | :param al_y: Orientation of second ellipse 61 | :return: The Gaussian Wasserstein distance of the two ellipses 62 | """ 63 | gt_xc = m_x 64 | gt_sigma = to_matrix(al_x, l_x, w_x, False) 65 | gt_sigma += gt_sigma.T 66 | gt_sigma /= 2 67 | 68 | track_xc = m_y 69 | track_sigma = to_matrix(al_y, l_y, w_y, False) 70 | track_sigma += track_sigma.T 71 | track_sigma /= 2 72 | 73 | error = norm(gt_xc - track_xc) ** 2 + np.trace(gt_sigma + track_sigma 74 | - 2 * sqrtm(np.einsum('ab, bc, cd -> ad', sqrtm(gt_sigma), 75 | track_sigma, sqrtm(gt_sigma)))) 76 | 77 | return error 78 | 79 | 80 | def square_root_distance(m_x, l_x, w_x, al_x, m_y, l_y, w_y, al_y): 81 | """ 82 | Calculate the Square Root distance of two ellipses 83 | :param m_x: Center of first ellipse 84 | :param l_x: Semi-axis length of first ellipse 85 | :param w_x: Semi-axis width of first ellipse 86 | :param al_x: Orientation of first ellipse 87 | :param m_y: Center of second ellipse 88 | :param l_y: Semi-axis length of second ellipse 89 | :param w_y: Semi-axis width of second ellipse 90 | :param al_y: Orientation of second ellipse 91 | :return: The Square Root distance of the two ellipses 92 | """ 93 | x_rot = rot_matrix(al_x) 94 | y_rot = rot_matrix(al_y) 95 | X_sqrt = np.dot(np.dot(x_rot, np.diag([l_x, w_x])), x_rot.T) 96 | Y_sqrt = np.dot(np.dot(y_rot, np.diag([l_y, w_y])), y_rot.T) 97 | 98 | return np.sum((m_x-m_y)**2) + np.sum((X_sqrt - Y_sqrt)**2) 99 | 100 | 101 | def plot_ellipses(m_res, l_res, w_res, al_res, m_prior, l_prior, w_prior, al_prior, m_meas, l_meas, w_meas, al_meas, 102 | m_gt, l_gt, w_gt, al_gt, name, filename, est_color='red'): 103 | """ 104 | Plot the estimated, prior, measured, and ground truth ellipses and save the plot 105 | :param m_res: Center of estimated ellipse 106 | :param l_res: Semi-axis length of estimated ellipse 107 | :param w_res: Semi-axis width of estimated ellipse 108 | :param al_res: Orientation of estimated ellipse 109 | :param m_prior: Center of prior ellipse 110 | :param l_prior: Semi-axis length of prior ellipse 111 | :param w_prior: Semi-axis width of prior ellipse 112 | :param al_prior: Orientation of prior ellipse 113 | :param m_meas: Center of measured ellipse 114 | :param l_meas: Semi-axis length of measured ellipse 115 | :param w_meas: Semi-axis width of measured ellipse 116 | :param al_meas: Orientation of measured ellipse 117 | :param m_gt: Center of ground truth ellipse 118 | :param l_gt: Semi-axis length of ground truth ellipse 119 | :param w_gt: Semi-axis width of ground truth ellipse 120 | :param al_gt: Orientation of ground truth ellipse 121 | :param name: Name of the approach 122 | :param filename: Name of the file the plot is to be saved in 123 | :param est_color: Color for plotting the estimated ellipse 124 | """ 125 | fig, ax = plt.subplots(1, 1) 126 | 127 | el_gt = Ellipse((m_gt[0], m_gt[1]), 2 * l_gt, 2 * w_gt, np.rad2deg(al_gt), fill=True, linewidth=2.0) 128 | el_gt.set_alpha(0.7) 129 | el_gt.set_fc('grey') 130 | el_gt.set_ec('grey') 131 | ax.add_artist(el_gt) 132 | 133 | ela_final = Ellipse((m_prior[0], m_prior[1]), 2 * l_prior, 2 * w_prior, np.rad2deg(al_prior), fill=False, 134 | linewidth=2.0) 135 | ela_final.set_alpha(0.7) 136 | ela_final.set_ec('mediumpurple') 137 | ax.add_artist(ela_final) 138 | 139 | elb_final = Ellipse((m_meas[0], m_meas[1]), 2 * l_meas, 2 * w_meas, np.rad2deg(al_meas), fill=False, linewidth=2.0) 140 | elb_final.set_alpha(0.7) 141 | elb_final.set_ec('darkturquoise') 142 | ax.add_artist(elb_final) 143 | 144 | el_res = Ellipse((m_res[0], m_res[1]), 2 * l_res, 2 * w_res, np.rad2deg(al_res), fill=False, linewidth=2.0) 145 | el_res.set_alpha(0.9) 146 | el_res.set_ec(est_color) 147 | ax.add_artist(el_res) 148 | 149 | plt.axis([-10, 10, -10, 10]) 150 | ax.set_aspect('equal') 151 | ax.set_title(name) 152 | plt.xlabel('x in m') 153 | plt.ylabel('y in m') 154 | plt.savefig(filename) 155 | plt.show() 156 | 157 | 158 | def plot_error_bars(states, steps): 159 | """ 160 | For each approach stored in states, plot the error of the first and last measurement step as a bar chart; error in 161 | GW and SR distance 162 | :param states: Contains names and errors of approaches; expects first steps entries of error to be in GW distance 163 | and last steps entries in SR distance 164 | :param steps: Number of measurement steps 165 | """ 166 | # setup 167 | num_states = len(states) 168 | bars = np.arange(1, 2 * num_states, 2) 169 | ticks = states[:]['name'] 170 | 171 | # plot Gw distance 172 | for i in range(num_states): 173 | plt.bar(bars[i] - 0.175, states[i]['error'][0], width=0.25, color=states[i]['color'], align='center') 174 | plt.bar(bars[i] + 0.175, states[i]['error'][steps - 1], width=0.25, color=states[i]['color'], align='center') 175 | plt.xticks(bars, ticks) 176 | plt.title('GW RMSE') 177 | plt.savefig('gwRmse.svg') 178 | plt.show() 179 | 180 | # plot SR distance 181 | for i in range(num_states): 182 | plt.bar(bars[i] - 0.175, states[i]['error'][steps], width=0.25, color=states[i]['color'], align='center') 183 | plt.bar(bars[i] + 0.175, states[i]['error'][-1], width=0.25, color=states[i]['color'], align='center') 184 | plt.xticks(bars, ticks) 185 | plt.title('SR RMSE') 186 | plt.savefig('srRmse.svg') 187 | plt.show() 188 | 189 | 190 | def plot_convergence(states, steps, save_path): 191 | """ 192 | For each approach stored in states, plot the error convergence; error in GW and SR distance 193 | :param states: Contains names and errors of approaches; expects first steps entries of error to be in GW 194 | distance and last steps entries in SR distance 195 | :param steps: Number of measurement steps 196 | :param save_path: Path in which to save the plots 197 | """ 198 | num_states = len(states) 199 | 200 | # plot GW distance 201 | for i in range(num_states): 202 | plt.plot(np.arange(1, steps + 1), states[i]['error'][:steps], color=states[i]['color'], label=states[i]['name']) 203 | plt.legend() 204 | plt.xticks(np.arange(5, steps + 1, 5)) 205 | plt.savefig(save_path + 'errorCompGW.svg') 206 | plt.show() 207 | 208 | # plot SR distance 209 | for i in range(num_states): 210 | plt.plot(np.arange(1, steps + 1), states[i]['error'][steps:], color=states[i]['color'], label=states[i]['name']) 211 | plt.legend() 212 | plt.xticks(np.arange(5, steps + 1, 5)) 213 | plt.savefig(save_path + 'errorCompSR.svg') 214 | plt.show() 215 | -------------------------------------------------------------------------------- /EllipseFusion/FusionMethods/tests.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Kolja Thormann 3 | 4 | Contains test cases for different ellipse fusion methods 5 | """ 6 | 7 | import numpy as np 8 | 9 | from FusionMethods.ellipse_fusion_methods import mmsr_mc_update, regular_update, mwdp_update, rm_mean_update,\ 10 | mmsr_pf_update 11 | from FusionMethods.ellipse_fusion_support import sample_m, get_ellipse_params, single_particle_approx_gaussian,\ 12 | barycenter, to_matrix 13 | from FusionMethods.error_and_plotting import gauss_wasserstein, square_root_distance, plot_error_bars,\ 14 | plot_convergence, plot_ellipses 15 | from FusionMethods.constants import * 16 | 17 | 18 | state_dtype = np.dtype([ 19 | ('x', 'f4', 5), # [m1, m2, al, l ,w] or [m1, m2, s11, s12, s22] 20 | ('cov', 'f4', (5, 5)), # covariance 21 | ('error', 'O'), # length depends on number of steps 22 | ('shape', 'f4', (2, 2)), # for mean of shape matrix only 23 | ('weights', 'O'), # for particle filter only, length depends on number of particles 24 | ('gamma', 'i4'), # for RM mean, keep track of number of measurements 25 | ('name', 'O'), # name of the fusion method 26 | ('color', 'O'), # color for error plotting 27 | ]) 28 | 29 | 30 | def test_convergence_pos(steps, runs, prior, cov_prior, cov_a, cov_b, n_particles, n_particles_pf, save_path): 31 | """ 32 | Test convergence of error for different fusion methods. Creates plot of root mean square error convergence and 33 | errors at first and last measurement step. If a uniform prior is given for alpha, ength, and width, the particle 34 | based methods use it while the others use the Gaussian prior (assumed to be a Gaussian approximation of the uniform 35 | prior). In either case, the position is still Gaussian. 36 | :param steps: Number of measurements 37 | :param runs: Number of MC runs 38 | :param prior: Prior prediction mean (ground truth will be drawn from it each run) 39 | :param cov_prior: Prior prediction covariance (ground truth will be drawn from it each run) 40 | :param cov_a: Noise of sensor A 41 | :param cov_b: Noise of sensor B 42 | :param n_particles: Number of particles for MMGW-MC 43 | :param n_particles_pf: Number of particles for MMGW-PF 44 | :param save_path: Path for saving figures 45 | """ 46 | error = np.zeros(steps*2) 47 | 48 | # setup state for various ellipse fusion methods 49 | mmsr_mc = np.zeros(1, dtype=state_dtype) 50 | mmsr_mc[0]['error'] = error.copy() 51 | mmsr_mc[0]['name'] = 'MMSR-MC' 52 | mmsr_mc[0]['color'] = 'lightgreen' 53 | mmsr_pf = np.zeros(1, dtype=state_dtype) 54 | mmsr_pf[0]['error'] = error.copy() 55 | mmsr_pf[0]['name'] = 'MMSR-PF' 56 | mmsr_pf[0]['color'] = 'darkgreen' 57 | regular = np.zeros(1, dtype=state_dtype) 58 | regular[0]['error'] = error.copy() 59 | regular[0]['name'] = 'Regular' 60 | regular[0]['color'] = 'red' 61 | mwdp = np.zeros(1, dtype=state_dtype) 62 | mwdp[0]['error'] = error.copy() 63 | mwdp[0]['name'] = 'MWDP' 64 | mwdp[0]['color'] = 'darkcyan' 65 | rm_mean = np.zeros(1, dtype=state_dtype) 66 | rm_mean[0]['error'] = error.copy() 67 | rm_mean[0]['name'] = 'RM Mean' 68 | rm_mean[0]['color'] = 'orange' 69 | 70 | for r in range(runs): 71 | print('Run %i of %i' % (r+1, runs)) 72 | # initialize =================================================================================================== 73 | # create gt from prior 74 | gt = sample_m(prior, cov_prior, False, 1) 75 | 76 | # get prior in square root space 77 | mmsr_mc[0]['x'], mmsr_mc[0]['cov'], particles_mc = single_particle_approx_gaussian(prior, cov_prior, 78 | n_particles, False) 79 | 80 | # get prior for regular state 81 | regular[0]['x'] = prior.copy() 82 | regular[0]['cov'] = cov_prior.copy() 83 | 84 | # get prior for MWDP 85 | mwdp[0]['x'] = prior.copy() 86 | mwdp[0]['cov'] = cov_prior.copy() 87 | 88 | # get prior for RM mean 89 | rm_mean[0]['x'] = prior.copy() 90 | rm_mean[0]['shape'] = to_matrix(prior[AL], prior[L], prior[W], False) 91 | rm_mean[0]['cov'] = cov_prior.copy() 92 | rm_mean[0]['gamma'] = 1 93 | 94 | # get prior for particle filter 95 | mmsr_pf[0]['x'], mmsr_pf[0]['cov'], particles_pf = single_particle_approx_gaussian(prior, cov_prior, 96 | n_particles_pf, False) 97 | mmsr_pf[0]['weights'] = np.ones(n_particles_pf) / n_particles_pf 98 | 99 | # test different methods 100 | for i in range(steps): 101 | if i % 10 == 0: 102 | print('Step %i of %i' % (i + 1, steps)) 103 | plot_cond = (r + 1 == runs) & (i + 1 == steps) 104 | 105 | # create measurement from gt (using alternating sensors) =================================================== 106 | if (i % 2) == 0: 107 | meas = sample_m(gt, cov_b, True, 1) 108 | cov_meas = cov_b.copy() 109 | else: 110 | meas = sample_m(gt, cov_a, False, 1) 111 | cov_meas = cov_a.copy() 112 | 113 | # fusion methods =========================================================================================== 114 | mmsr_mc_update(mmsr_mc[0], meas, cov_meas, n_particles, gt, i, steps, plot_cond, save_path, False) 115 | 116 | regular_update(regular[0], meas, cov_meas, gt, i, steps, plot_cond, save_path) 117 | 118 | mwdp_update(mwdp[0], meas, cov_meas, gt, i, steps, plot_cond, save_path) 119 | 120 | rm_mean_update(rm_mean[0], meas, cov_meas, gt, i, steps, plot_cond, save_path) 121 | 122 | mmsr_pf_update(mmsr_pf[0], meas, cov_meas, particles_pf, n_particles_pf, gt, i, steps, plot_cond, save_path, 123 | False) 124 | 125 | mmsr_mc[0]['error'] = np.sqrt(mmsr_mc[0]['error'] / runs) 126 | mmsr_pf[0]['error'] = np.sqrt(mmsr_pf[0]['error'] / runs) 127 | regular[0]['error'] = np.sqrt(regular[0]['error'] / runs) 128 | mwdp[0]['error'] = np.sqrt(mwdp[0]['error'] / runs) 129 | rm_mean[0]['error'] = np.sqrt(rm_mean[0]['error'] / runs) 130 | 131 | print(mmsr_pf['error']) 132 | print(rm_mean['error']) 133 | 134 | # error plotting =================================================================================================== 135 | plot_error_bars(np.block([regular, rm_mean, mmsr_mc, mwdp, mmsr_pf]), steps) 136 | plot_convergence(np.block([regular, rm_mean, mmsr_mc, mwdp, mmsr_pf]), steps, save_path) 137 | 138 | 139 | def test_mean(orig, cov, n_particles, save_path): 140 | """ 141 | Compare the mean in original state space with mean in square root space (via MC approximation) in regards of their 142 | GW and SR error 143 | :param orig: Mean in original state space 144 | :param cov: Covariance for original state space 145 | :param n_particles: Number of particles for MC approximation of SR space mean 146 | :param save_path: Path for saving figures 147 | """ 148 | # approximate mean in SR space 149 | vec_mmsr, var_A, vec_particle = single_particle_approx_gaussian(orig, cov, n_particles, True) 150 | mat_mmsr = np.array([ 151 | [vec_mmsr[2], vec_mmsr[3]], 152 | [vec_mmsr[3], vec_mmsr[4]] 153 | ]) 154 | mat_mmsr = np.dot(mat_mmsr, mat_mmsr) 155 | l_mmsr, w_mmsr, al_mmsr = get_ellipse_params(mat_mmsr) 156 | 157 | # approximate mean in matrix space 158 | mat_mat = np.zeros((2, 2)) 159 | vec_mat = np.zeros(2) 160 | for i in range(len(vec_particle)): 161 | vec_mat += vec_particle[i, :2] 162 | mat = np.array([ 163 | [vec_particle[i, 2], vec_particle[i, 3]], 164 | [vec_particle[i, 3], vec_particle[i, 4]] 165 | ]) 166 | mat_mat += np.dot(mat, mat) 167 | vec_mat /= len(vec_particle) 168 | mat_mat /= len(vec_particle) 169 | l_mat, w_mat, al_mat = get_ellipse_params(mat_mat) 170 | 171 | # caclulate Barycenter using optimization 172 | covs_sr = np.zeros((n_particles, 2, 2)) 173 | covs_sr[:, 0, 0] = vec_particle[:, 2] 174 | covs_sr[:, 0, 1] = vec_particle[:, 3] 175 | covs_sr[:, 1, 0] = vec_particle[:, 3] 176 | covs_sr[:, 1, 1] = vec_particle[:, 4] 177 | covs = np.einsum('xab, xbc -> xac', covs_sr, covs_sr) 178 | bary_particles = np.zeros((n_particles, 5)) 179 | bary_particles[:, M] = vec_particle[:, M] 180 | bary_particles[:, 2] = covs[:, 0, 0] 181 | bary_particles[:, 3] = covs[:, 0, 1] 182 | bary_particles[:, 4] = covs[:, 1, 1] 183 | bary = barycenter(bary_particles, np.ones(n_particles) / n_particles, n_particles, particles_sr=vec_particle) 184 | mat_mmgw = np.array([ 185 | [bary[2], bary[3]], 186 | [bary[3], bary[4]], 187 | ]) 188 | l_mmgw, w_mmgw, al_mmgw = get_ellipse_params(mat_mmgw) 189 | 190 | # approximate error 191 | error_orig_gw = 0 192 | error_orig_sr = 0 193 | error_mmsr_gw = 0 194 | error_mmsr_sr = 0 195 | error_mat_gw = 0 196 | error_mat_sr = 0 197 | error_mmgw_gw = 0 198 | error_mmgw_sr = 0 199 | for i in range(n_particles): 200 | mat_particle = np.array([ 201 | [vec_particle[i, 2], vec_particle[i, 3]], 202 | [vec_particle[i, 3], vec_particle[i, 4]] 203 | ]) 204 | mat_particle = np.dot(mat_particle, mat_particle) 205 | l_particle, w_particle, al_particle = get_ellipse_params(mat_particle) 206 | error_orig_gw += gauss_wasserstein(vec_particle[i, :2], l_particle, w_particle, al_particle, orig[M], orig[L], 207 | orig[W], orig[AL]) 208 | error_orig_sr += square_root_distance(vec_particle[i, :2], l_particle, w_particle, al_particle, orig[M], 209 | orig[L], orig[W], orig[AL]) 210 | error_mmsr_gw += gauss_wasserstein(vec_particle[i, :2], l_particle, w_particle, al_particle, vec_mmsr[M], 211 | l_mmsr, w_mmsr, al_mmsr) 212 | error_mmsr_sr += square_root_distance(vec_particle[i, :2], l_particle, w_particle, al_particle, vec_mmsr[M], 213 | l_mmsr, w_mmsr, al_mmsr) 214 | error_mat_gw += gauss_wasserstein(vec_particle[i, :2], l_particle, w_particle, al_particle, vec_mat[M], 215 | l_mat, w_mat, al_mat) 216 | error_mat_sr += square_root_distance(vec_particle[i, :2], l_particle, w_particle, al_particle, vec_mat[M], 217 | l_mat, w_mat, al_mat) 218 | error_mmgw_gw += gauss_wasserstein(vec_particle[i, :2], l_particle, w_particle, al_particle, bary[M], l_mmgw, 219 | w_mmgw, al_mmgw) 220 | error_mmgw_sr += square_root_distance(vec_particle[i, :2], l_particle, w_particle, al_particle, bary[M], l_mmgw, 221 | w_mmgw, al_mmgw) 222 | error_orig_gw = np.sqrt(error_orig_gw / n_particles) 223 | error_orig_sr = np.sqrt(error_orig_sr / n_particles) 224 | error_mmsr_gw = np.sqrt(error_mmsr_gw / n_particles) 225 | error_mmsr_sr = np.sqrt(error_mmsr_sr / n_particles) 226 | error_mat_gw = np.sqrt(error_mat_gw / n_particles) 227 | error_mat_sr = np.sqrt(error_mat_sr / n_particles) 228 | error_mmgw_gw = np.sqrt(error_mmgw_gw / n_particles) 229 | error_mmgw_sr = np.sqrt(error_mmgw_sr / n_particles) 230 | 231 | plot_ellipses(vec_mmsr[M], l_mmsr, w_mmsr, al_mmsr, orig[M], orig[L], orig[W], orig[AL], bary[M], l_mmgw, w_mmgw, 232 | al_mmgw, vec_mat[M], l_mat, w_mat, al_mat, 'MMSE Estimates', save_path + 'mmse.svg', 233 | est_color='green') 234 | 235 | # print error 236 | print('RMGW of original:') 237 | print(error_orig_gw) 238 | print('RMSR of original:') 239 | print(error_orig_sr) 240 | print('RMGW of mmsr:') 241 | print(error_mmsr_gw) 242 | print('RMSR of mmsr:') 243 | print(error_mmsr_sr) 244 | print('RMGW of Euclidean mmse:') 245 | print(error_mat_gw) 246 | print('RMSR of Euclidean mmse:') 247 | print(error_mat_sr) 248 | print('RMGW of mmgw_bary:') 249 | print(error_mmgw_gw) 250 | print('RMSR of mmgw_bary:') 251 | print(error_mmgw_sr) 252 | -------------------------------------------------------------------------------- /EllipseFusion/main.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Kolja Thormann 3 | 4 | Evaluation for 5 | Thormann, Kolja; Baum, Marcus (2019): Bayesian Fusion of Orientation, Width and Length Estimates of Elliptic 6 | Extended Targets based on the Gaussian Wasserstein Distance. TechRxiv. Preprint. 7 | """ 8 | 9 | import numpy as np 10 | 11 | from FusionMethods.tests import test_convergence_pos, test_mean 12 | 13 | 14 | # setup ================================================================================================================ 15 | # Gaussian prior 16 | prior = np.array([0, 0, 0.0 * np.pi, 8, 3]) # [m1, m2, alpha, length, width] 17 | cov_prior = np.array([ 18 | [0.5, 0.0, 0, 0, 0], 19 | [0.0, 0.5, 0, 0, 0], 20 | [0, 0, 0.5*np.pi, 0, 0], 21 | [0, 0, 0, 0.5, 0], 22 | [0, 0, 0, 0, 0.5], 23 | ]) 24 | 25 | # sensor A and B noise 26 | cov_a = np.array([ 27 | [0.5, 0.0, 0, 0, 0], 28 | [0.0, 0.5, 0, 0, 0], 29 | [0, 0, 0.01*np.pi, 0, 0], 30 | [0, 0, 0, 0.5, 0], 31 | [0, 0, 0, 0, 0.1], 32 | ]) 33 | cov_b = np.array([ 34 | [0.5, 0.0, 0, 0, 0], 35 | [0.0, 0.5, 0, 0, 0], 36 | [0, 0, 0.01*np.pi, 0, 0], 37 | [0, 0, 0, 0.5, 0], # will be w 38 | [0, 0, 0, 0, 0.1], # will be l 39 | ]) 40 | 41 | runs = 100 # number of MC runs 42 | steps = 20 # number of measurements (alternating sensors A and B) 43 | n_particles = 1000 # number of particles for MMSR-MC 44 | n_particles_pf = 100000 # number of particles for MMSR-PF 45 | 46 | save_path = './' 47 | 48 | # tests ================================================================================================================ 49 | test_convergence_pos(steps, runs, prior, cov_prior, cov_a, cov_b, n_particles, n_particles_pf, save_path) 50 | # test_mean(prior, cov_prior, n_particles, save_path) 51 | -------------------------------------------------------------------------------- /Evaluation/assignments_uniform_OSPA_Fig4.m: -------------------------------------------------------------------------------- 1 | % This script shows the different assignments for uniform OSPA distances 2 | % when four points and 50 points are chosen on the boundary (Fig.4) of our paper: 3 | % Shishan Yang, Marcus Baum, and Karl Granstroem. "Metrics for Performance 4 | % Evaluation of Ellipitical Extended Object Tracking Methods", 5 | % The 2016 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2016) 6 | % ---BibTeX entry 7 | % @InProceedings{MFI16_Yang, 8 | % Title = {{Metrics for Performance Evaluation of Elliptic Extended Object Tracking Methods}}, 9 | % Author = {Shishan Yang and Marcus Baum and Karl Granstr\"om}, 10 | % Booktitle = {{IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2016)}}, 11 | % Year = {2016}, 12 | % 13 | % Owner = {yang}, 14 | % Timestamp = {2016.07.01} 15 | % } 16 | 17 | clc 18 | close all 19 | clear 20 | dbstop error 21 | 22 | addpath('hungarian/') 23 | 24 | nr_points_boundary = 50; % nr of points that used for the calculation of the uniform OSPA 25 | 26 | 27 | % ellipse parameterization: [center1,center2,angle,length 0f 28 | % semmi-axis1,length of semmi-axis2] 29 | % 30 | %% set the ground truth 31 | gt = [0 0 0 1 2]; 32 | 33 | %% set the estimate 34 | est = [0 0 pi/6 1 2]; 35 | 36 | 37 | figure 38 | hold on 39 | gt_plot = plot_extent(gt, '-', 'k', 1); 40 | 41 | est_plot = plot_extent(est,'--','g',1); 42 | axis equal 43 | xlim([-2,2]) 44 | ylim([-2,2]) 45 | grid on 46 | box on 47 | 48 | 49 | [gt_points_4, est_points_4] = get_uniform_points_boundary(gt, est,4); 50 | [gt_points_m, est_points_m] = get_uniform_points_boundary(gt,est,nr_points_boundary); 51 | 52 | 53 | 54 | [~, assignment_m]= ospa_dist(gt_points_m,est_points_m,10000,2); 55 | 56 | for j = 1:size(gt_points_m,2) 57 | [~,b(j)]=find(assignment_m(j,:)); 58 | plot([gt_points_m(1,j) est_points_m(1,b(j))],[gt_points_m(2,j) est_points_m(2,b(j))],'r--'); 59 | end 60 | 61 | 62 | [~, assignment_4]= ospa_dist(gt_points_4,est_points_4,10000,2); 63 | 64 | for j = 1:size(gt_points_4,2) 65 | [~,b(j)]=find(assignment_4(j,:)); 66 | gt_points = plot(gt_points_4(1,j),gt_points_4(2,j),'k*','LineWidth',5); 67 | est_points = plot(est_points_4(1,j),est_points_4(2,j),'g*','LineWidth',5); 68 | plot([gt_points_4(1,j) est_points_4(1,b(j))],[gt_points_4(2,j) est_points_4(2,b(j))],'k'); 69 | end 70 | legend([gt_plot,est_plot],{'Ground Truth','Estimate'}) 71 | -------------------------------------------------------------------------------- /Evaluation/d_gaussian_wasserstein.m: -------------------------------------------------------------------------------- 1 | function [distance] = d_gaussian_wasserstein( ellipse1, ellipse2) 2 | % D_GAUSSIAN_WASSERSTEIN gives the wasserstein distance between two ellipse 3 | % which are interpreted as Gaussians 4 | % 5 | % Input: 6 | % ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2] 7 | % ellipse2, 1x5, parameterization of the other ellispe [m1 m2 alpha l1 l2] 8 | % 9 | % Output: 10 | % distance, scalar, gaussian wasserstein distance 11 | % 12 | % Written by Shishan Yang 13 | 14 | m1 = ellipse1(1:2); 15 | alpha1 = ellipse1(3); 16 | eigen_val1 = [ellipse1(4), ellipse1(5)]; 17 | eigen_vec1 = [cos(alpha1), -sin(alpha1); sin(alpha1), cos(alpha1)]; 18 | sigma1 = eigen_vec1*diag(eigen_val1.^2)*eigen_vec1'; 19 | sigma1 = (sigma1+sigma1')/2; % make covariance symmetric 20 | 21 | 22 | 23 | m2 = ellipse2(1:2); 24 | alpha2 = ellipse2(3); 25 | eigen_val2 = [ellipse2(4),ellipse2(5)]; 26 | eigen_vec2 = [cos(alpha2), -sin(alpha2); sin(alpha2), cos(alpha2)]; 27 | sigma2 = eigen_vec2*diag(eigen_val2.^2)*eigen_vec2'; 28 | sigma2 = (sigma2+sigma2')/2; % make covariance symmetric 29 | 30 | error_sq = norm(m1-m2)^2 + trace(sigma1 + sigma2 -2*sqrtm((sqrtm(sigma1)*sigma2*sqrtm(sigma1)))); 31 | distance = sqrt(error_sq); 32 | 33 | 34 | end -------------------------------------------------------------------------------- /Evaluation/d_kullback_leibler.m: -------------------------------------------------------------------------------- 1 | function [kld] = d_kullback_leibler( ellipse1, ellipse2) 2 | % D_KULLBACK_LEIBLER gives the Kullback-Leibler Divergence between two 3 | % ellipses which could interpreted as Gaussians 4 | % 5 | % Input: 6 | % ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2] 7 | % ellipse2, 1x5, parameterization of the other ellispe [m1 m2 alpha l1 l2] 8 | % 9 | % Output: 10 | % kld, scalar, Kullback-Leibler divergence 11 | % 12 | % Written by Shishan Yang 13 | 14 | 15 | m1 = ellipse1(1:2); 16 | dim = numel(m1); 17 | 18 | 19 | alpha1 = ellipse1(3); 20 | eigen_val1 = [ellipse1(4), ellipse1(5)]; 21 | eigen_vec1 = [cos(alpha1), -sin(alpha1); sin(alpha1), cos(alpha1)]; 22 | sigma1 = eigen_vec1*diag(eigen_val1.^2)*eigen_vec1'; 23 | sigma1 = (sigma1 + sigma1')/2; % make covariance symmetric 24 | 25 | m2 = ellipse2(1:2); 26 | alpha2 = ellipse2(3); 27 | eigen_val2 = [ellipse2(4),ellipse2(5)]; 28 | eigen_vec2 = [cos(alpha2), -sin(alpha2); sin(alpha2), cos(alpha2)]; 29 | sigma2 = eigen_vec2*diag(eigen_val2.^2)*eigen_vec2'; 30 | sigma2 = (sigma2 + sigma2')/2; % make covariance symmetric 31 | 32 | kld = .5*(trace((sigma1^(-1))*sigma2) + (m1 - m2)'*(sigma1^(-1))*(m1 - m2)... 33 | - dim + log((det(sigma1))/det(sigma2)) ); 34 | 35 | 36 | end 37 | 38 | -------------------------------------------------------------------------------- /Evaluation/get_uniform_points_boundary.m: -------------------------------------------------------------------------------- 1 | function [points1, points2] = get_uniform_points_boundary(ellipse1, ellipse2, nr_points) 2 | % GET_UNIFORM_POINTS_BOUNDARY gives the sets of points that are 3 | % equidistantly chosen on the boudary of two ellipses, so that future OSPA 4 | % could be calculated 5 | % 6 | % Input: 7 | % ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2] 8 | % ellipse2, 1x5, parameterization of the other ellispe [m1 m2 alpha l1 l2] 9 | % nr_points, nr of points that are uniformly chosen on the boundary 10 | % to calculate OPSA distance 11 | % Output: 12 | % points1, 2xnr_points, points that are chosen on the ellipse1 13 | % poitns2, 2xnr_points, points that are chosen on the ellispe2 14 | % Written by Shishan Yang 15 | 16 | theta = (0:2*pi/nr_points:2*pi-2*pi/nr_points); 17 | 18 | alpha1 = ellipse1(3); 19 | center1 = ellipse1(1:2)'; 20 | l1 = ellipse1(4:5); 21 | gt_rotation_mat = [cos(alpha1) -sin(alpha1); sin(alpha1) cos(alpha1)]; 22 | points1(1,:) = l1(1)*cos(theta); 23 | points1(2,:) = l1(2)*sin(theta); 24 | 25 | alpha2 = ellipse2(3); 26 | center2 = ellipse2(1:2)'; 27 | l2 = ellipse2(4:5); 28 | est_rotation_mat = [cos(alpha2) -sin(alpha2); sin(alpha2) cos(alpha2)]; 29 | points2(1,:) = l2(1)*cos(theta); 30 | points2(2,:) = l2(2)*sin(theta); 31 | 32 | 33 | 34 | points1 = gt_rotation_mat*points1 + repmat(center1,1,size(points1,2)); 35 | points2 = est_rotation_mat*points2 + repmat(center2,1,size(points2,2)); 36 | end -------------------------------------------------------------------------------- /Evaluation/hungarian/Hungarian.m: -------------------------------------------------------------------------------- 1 | function [Matching,Cost] = Hungarian(Perf) 2 | % 3 | % [MATCHING,COST] = Hungarian_New(WEIGHTS) 4 | % 5 | % A function for finding a minimum edge weight matching given a MxN Edge 6 | % weight matrix WEIGHTS using the Hungarian Algorithm. 7 | % 8 | % An edge weight of Inf indicates that the pair of vertices given by its 9 | % position have no adjacent edge. 10 | % 11 | % MATCHING return a MxN matrix with ones in the place of the matchings and 12 | % zeros elsewhere. 13 | % 14 | % COST returns the cost of the minimum matching 15 | 16 | % Written by: Alex Melin 30 June 2006 17 | 18 | 19 | % Initialize Variables 20 | Matching = zeros(size(Perf)); 21 | 22 | % Condense the Performance Matrix by removing any unconnected vertices to 23 | % increase the speed of the algorithm 24 | 25 | % Find the number in each column that are connected 26 | num_y = sum(~isinf(Perf),1); 27 | % Find the number in each row that are connected 28 | num_x = sum(~isinf(Perf),2); 29 | 30 | % Find the columns(vertices) and rows(vertices) that are isolated 31 | x_con = find(num_x~=0); 32 | y_con = find(num_y~=0); 33 | 34 | % Assemble Condensed Performance Matrix 35 | P_size = max(length(x_con),length(y_con)); 36 | P_cond = zeros(P_size); 37 | P_cond(1:length(x_con),1:length(y_con)) = Perf(x_con,y_con); 38 | if isempty(P_cond) 39 | Cost = 0; 40 | return 41 | end 42 | 43 | % Ensure that a perfect matching exists 44 | % Calculate a form of the Edge Matrix 45 | Edge = P_cond; 46 | Edge(P_cond~=Inf) = 0; 47 | % Find the deficiency(CNUM) in the Edge Matrix 48 | cnum = min_line_cover(Edge); 49 | 50 | % Project additional vertices and edges so that a perfect matching 51 | % exists 52 | Pmax = max(max(P_cond(P_cond~=Inf))); 53 | P_size = length(P_cond)+cnum; 54 | P_cond = ones(P_size)*Pmax; 55 | P_cond(1:length(x_con),1:length(y_con)) = Perf(x_con,y_con); 56 | 57 | %************************************************* 58 | % MAIN PROGRAM: CONTROLS WHICH STEP IS EXECUTED 59 | %************************************************* 60 | exit_flag = 1; 61 | stepnum = 1; 62 | while exit_flag 63 | switch stepnum 64 | case 1 65 | [P_cond,stepnum] = step1(P_cond); 66 | case 2 67 | [r_cov,c_cov,M,stepnum] = step2(P_cond); 68 | case 3 69 | [c_cov,stepnum] = step3(M,P_size); 70 | case 4 71 | [M,r_cov,c_cov,Z_r,Z_c,stepnum] = step4(P_cond,r_cov,c_cov,M); 72 | case 5 73 | [M,r_cov,c_cov,stepnum] = step5(M,Z_r,Z_c,r_cov,c_cov); 74 | case 6 75 | [P_cond,stepnum] = step6(P_cond,r_cov,c_cov); 76 | case 7 77 | exit_flag = 0; 78 | end 79 | end 80 | 81 | % Remove all the virtual satellites and targets and uncondense the 82 | % Matching to the size of the original performance matrix. 83 | Matching(x_con,y_con) = M(1:length(x_con),1:length(y_con)); 84 | Cost = sum(sum(Perf(Matching==1))); 85 | 86 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 87 | % STEP 1: Find the smallest number of zeros in each row 88 | % and subtract that minimum from its row 89 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 90 | 91 | function [P_cond,stepnum] = step1(P_cond) 92 | 93 | P_size = length(P_cond); 94 | 95 | % Loop throught each row 96 | for ii = 1:P_size 97 | rmin = min(P_cond(ii,:)); 98 | P_cond(ii,:) = P_cond(ii,:)-rmin; 99 | end 100 | 101 | stepnum = 2; 102 | 103 | %************************************************************************** 104 | % STEP 2: Find a zero in P_cond. If there are no starred zeros in its 105 | % column or row start the zero. Repeat for each zero 106 | %************************************************************************** 107 | 108 | function [r_cov,c_cov,M,stepnum] = step2(P_cond) 109 | 110 | % Define variables 111 | P_size = length(P_cond); 112 | r_cov = zeros(P_size,1); % A vector that shows if a row is covered 113 | c_cov = zeros(P_size,1); % A vector that shows if a column is covered 114 | M = zeros(P_size); % A mask that shows if a position is starred or primed 115 | 116 | for ii = 1:P_size 117 | for jj = 1:P_size 118 | if P_cond(ii,jj) == 0 && r_cov(ii) == 0 && c_cov(jj) == 0 119 | M(ii,jj) = 1; 120 | r_cov(ii) = 1; 121 | c_cov(jj) = 1; 122 | end 123 | end 124 | end 125 | 126 | % Re-initialize the cover vectors 127 | r_cov = zeros(P_size,1); % A vector that shows if a row is covered 128 | c_cov = zeros(P_size,1); % A vector that shows if a column is covered 129 | stepnum = 3; 130 | 131 | %************************************************************************** 132 | % STEP 3: Cover each column with a starred zero. If all the columns are 133 | % covered then the matching is maximum 134 | %************************************************************************** 135 | 136 | function [c_cov,stepnum] = step3(M,P_size) 137 | 138 | c_cov = sum(M,1); 139 | if sum(c_cov) == P_size 140 | stepnum = 7; 141 | else 142 | stepnum = 4; 143 | end 144 | 145 | %************************************************************************** 146 | % STEP 4: Find a noncovered zero and prime it. If there is no starred 147 | % zero in the row containing this primed zero, Go to Step 5. 148 | % Otherwise, cover this row and uncover the column containing 149 | % the starred zero. Continue in this manner until there are no 150 | % uncovered zeros left. Save the smallest uncovered value and 151 | % Go to Step 6. 152 | %************************************************************************** 153 | function [M,r_cov,c_cov,Z_r,Z_c,stepnum] = step4(P_cond,r_cov,c_cov,M) 154 | 155 | P_size = length(P_cond); 156 | 157 | zflag = 1; 158 | while zflag 159 | % Find the first uncovered zero 160 | row = 0; col = 0; exit_flag = 1; 161 | ii = 1; jj = 1; 162 | while exit_flag 163 | if P_cond(ii,jj) == 0 && r_cov(ii) == 0 && c_cov(jj) == 0 164 | row = ii; 165 | col = jj; 166 | exit_flag = 0; 167 | end 168 | jj = jj + 1; 169 | if jj > P_size; jj = 1; ii = ii+1; end 170 | if ii > P_size; exit_flag = 0; end 171 | end 172 | 173 | % If there are no uncovered zeros go to step 6 174 | if row == 0 175 | stepnum = 6; 176 | zflag = 0; 177 | Z_r = 0; 178 | Z_c = 0; 179 | else 180 | % Prime the uncovered zero 181 | M(row,col) = 2; 182 | % If there is a starred zero in that row 183 | % Cover the row and uncover the column containing the zero 184 | if sum(find(M(row,:)==1)) ~= 0 185 | r_cov(row) = 1; 186 | zcol = find(M(row,:)==1); 187 | c_cov(zcol) = 0; 188 | else 189 | stepnum = 5; 190 | zflag = 0; 191 | Z_r = row; 192 | Z_c = col; 193 | end 194 | end 195 | end 196 | 197 | %************************************************************************** 198 | % STEP 5: Construct a series of alternating primed and starred zeros as 199 | % follows. Let Z0 represent the uncovered primed zero found in Step 4. 200 | % Let Z1 denote the starred zero in the column of Z0 (if any). 201 | % Let Z2 denote the primed zero in the row of Z1 (there will always 202 | % be one). Continue until the series terminates at a primed zero 203 | % that has no starred zero in its column. Unstar each starred 204 | % zero of the series, star each primed zero of the series, erase 205 | % all primes and uncover every line in the matrix. Return to Step 3. 206 | %************************************************************************** 207 | 208 | function [M,r_cov,c_cov,stepnum] = step5(M,Z_r,Z_c,r_cov,c_cov) 209 | 210 | zflag = 1; 211 | ii = 1; 212 | while zflag 213 | % Find the index number of the starred zero in the column 214 | rindex = find(M(:,Z_c(ii))==1); 215 | if rindex > 0 216 | % Save the starred zero 217 | ii = ii+1; 218 | % Save the row of the starred zero 219 | Z_r(ii,1) = rindex; 220 | % The column of the starred zero is the same as the column of the 221 | % primed zero 222 | Z_c(ii,1) = Z_c(ii-1); 223 | else 224 | zflag = 0; 225 | end 226 | 227 | % Continue if there is a starred zero in the column of the primed zero 228 | if zflag == 1; 229 | % Find the column of the primed zero in the last starred zeros row 230 | cindex = find(M(Z_r(ii),:)==2); 231 | ii = ii+1; 232 | Z_r(ii,1) = Z_r(ii-1); 233 | Z_c(ii,1) = cindex; 234 | end 235 | end 236 | 237 | % UNSTAR all the starred zeros in the path and STAR all primed zeros 238 | for ii = 1:length(Z_r) 239 | if M(Z_r(ii),Z_c(ii)) == 1 240 | M(Z_r(ii),Z_c(ii)) = 0; 241 | else 242 | M(Z_r(ii),Z_c(ii)) = 1; 243 | end 244 | end 245 | 246 | % Clear the covers 247 | r_cov = r_cov.*0; 248 | c_cov = c_cov.*0; 249 | 250 | % Remove all the primes 251 | M(M==2) = 0; 252 | 253 | stepnum = 3; 254 | 255 | % ************************************************************************* 256 | % STEP 6: Add the minimum uncovered value to every element of each covered 257 | % row, and subtract it from every element of each uncovered column. 258 | % Return to Step 4 without altering any stars, primes, or covered lines. 259 | %************************************************************************** 260 | 261 | function [P_cond,stepnum] = step6(P_cond,r_cov,c_cov) 262 | a = find(r_cov == 0); 263 | b = find(c_cov == 0); 264 | minval = min(min(P_cond(a,b))); 265 | 266 | P_cond(find(r_cov == 1),:) = P_cond(find(r_cov == 1),:) + minval; 267 | P_cond(:,find(c_cov == 0)) = P_cond(:,find(c_cov == 0)) - minval; 268 | 269 | stepnum = 4; 270 | 271 | function cnum = min_line_cover(Edge) 272 | 273 | % Step 2 274 | [r_cov,c_cov,M,stepnum] = step2(Edge); 275 | % Step 3 276 | [c_cov,stepnum] = step3(M,length(Edge)); 277 | % Step 4 278 | [M,r_cov,c_cov,Z_r,Z_c,stepnum] = step4(Edge,r_cov,c_cov,M); 279 | % Calculate the deficiency 280 | cnum = length(Edge)-sum(r_cov)-sum(c_cov); -------------------------------------------------------------------------------- /Evaluation/hungarian/ospa_dist.m: -------------------------------------------------------------------------------- 1 | function [dist, assignment,varargout]= ospa_dist(X,Y,c,p) 2 | 3 | % This is the MATLAB code for OSPA distance proposed in 4 | % 5 | % D. Schuhmacher, B.-T. Vo, and B.-N. Vo, "A consistent metric for performance evaluation in multi-object filtering," IEEE Trans. Signal Processing, Vol. 56, No. 8 Part 1, pp. 3447� 3457, 2008. 6 | % http://ba-ngu.vo-au.com/vo/SVV08_OSPA.pdf 7 | % ---BibTeX entry 8 | % @ARTICLE{OSPA, 9 | % author={D. Schuhmacher and B.-T. Vo and B.-N. Vo}, 10 | % journal={IEEE Transactions on Signal Processing}, 11 | % title={A Consistent Metric for Performance Evaluation of Multi-Object Filters}, 12 | % year={2008}, 13 | % month={Aug}, 14 | % volume={56}, 15 | % number={8}, 16 | % pages={3447-3457}} 17 | %--- 18 | 19 | % Compute OSPA distance between two finite sets X and Y 20 | % Inputs: X,Y- matrices of column vectors 21 | % c - cut-off parameter 22 | % p - p-parameter for the metric 23 | % Output: scalar distance between X and Y 24 | % Note: the Euclidean 2-norm is used as the "base" distance on the region 25 | 26 | % if nargout ~=1 & nargout ~=3 27 | % error('Incorrect number of outputs'); 28 | % end 29 | 30 | if isempty(X) & isempty(Y) 31 | dist = 0; 32 | 33 | if nargout == 3 34 | varargout(1)= {0}; 35 | varargout(2)= {0}; 36 | end 37 | 38 | return; 39 | end 40 | 41 | if isempty(X) | isempty(Y) 42 | dist = c; 43 | 44 | if nargout == 3 45 | varargout(1)= {0}; 46 | varargout(2)= {c}; 47 | end 48 | 49 | return; 50 | end 51 | 52 | 53 | %Calculate sizes of the input point patterns 54 | n = size(X,2); 55 | m = size(Y,2); 56 | 57 | %Calculate cost/weight matrix for pairings - fast method with vectorization 58 | XX= repmat(X,[1 m]); 59 | YY= reshape(repmat(Y,[n 1]),[size(Y,1) n*m]); 60 | % original 61 | D = reshape(sqrt(sum((XX-YY).^2)),[n m]); 62 | 63 | D = min(c,D).^p; 64 | 65 | % %Calculate cost/weight matrix for pairings - slow method with for loop 66 | % D= zeros(n,m); 67 | % for j=1:m 68 | % D(:,j)= sqrt(sum( ( repmat(Y(:,j),[1 n])- X ).^2 )'); 69 | % end 70 | % D= min(c,D).^p; 71 | 72 | %Compute optimal assignment and cost using the Hungarian algorithm 73 | [assignment,cost]= Hungarian(D); 74 | 75 | %Calculate final distance 76 | dist= ( 1/max(m,n)*( c^p*abs(m-n)+ cost ) ) ^(1/p); 77 | 78 | %Output components if called for in varargout 79 | if nargout == 3 80 | varargout(1)= {(1/max(m,n)*cost)^(1/p)}; 81 | varargout(2)= {(1/max(m,n)*c^p*abs(m-n))^(1/p)}; 82 | end 83 | -------------------------------------------------------------------------------- /Evaluation/plot_extent.m: -------------------------------------------------------------------------------- 1 | function [handle_extent] = plot_extent(ellipse,line_style, color, line_width) 2 | % PLOT_EXTENT plots the extent of an ellipse or circle 3 | % Input: 4 | % ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2] 5 | % line_style, definedthe same as in Matlab plot function 6 | % color, definedthe same as in Matlab plot function 7 | % line_width, definedthe same as in Matlab plot function 8 | % 9 | % Output: 10 | % handle_extent, the handle of the plot 11 | % 12 | % Written by Shishan Yang 13 | 14 | 15 | 16 | center = ellipse(1:2); 17 | theta = ellipse(3); 18 | l = ellipse(4:5); 19 | R = [cos(theta) -sin(theta); sin(theta) cos(theta)]; %rotation matrix 20 | 21 | 22 | alpha = 0:pi/100:2*pi; 23 | xunit = l(1)*cos(alpha); 24 | yunit = l(2)*sin(alpha); 25 | 26 | rotated = R* [xunit; yunit]; 27 | xpoints = rotated(1,:) + center(1); 28 | ypoints = rotated(2,:) + center(2); 29 | 30 | 31 | handle_extent = plot(xpoints,ypoints,'LineStyle',line_style,'color',color,'LineWidth',line_width); 32 | 33 | end -------------------------------------------------------------------------------- /Evaluation/scenario1_Fig5.m: -------------------------------------------------------------------------------- 1 | % This script gives the scenario 1 and its error plot (first column of Fig.5) of our paper: 2 | % Shishan Yang, Marcus Baum, and Karl Granstroem. "Metrics for Performance 3 | % Evaluation of Ellipitical Extended Object Tracking Methods", 4 | % The 2016 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2016) 5 | % ---BibTeX entry 6 | % @InProceedings{MFI16_Yang, 7 | % Title = {{Metrics for Performance Evaluation of Elliptic Extended Object Tracking Methods}}, 8 | % Author = {Shishan Yang and Marcus Baum and Karl Granstr\"om}, 9 | % Booktitle = {{IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2016)}}, 10 | % Year = {2016}, 11 | % 12 | % Owner = {yang}, 13 | % Timestamp = {2016.07.01} 14 | % } 15 | 16 | 17 | clc 18 | close all 19 | clear 20 | dbstop error 21 | 22 | 23 | addpath('hungarian/') 24 | 25 | 26 | 27 | nr_points_boundary = 100; % nr of points that used for the calculation of the uniform OSPA 28 | nr_bins = 100; 29 | 30 | % ellipse parameterization: [center1,center2,angle,length 0f 31 | % semmi-axis1,length of semmi-axis2] 32 | % 33 | %% set the ground truth 34 | gt_m = [0 0]'; 35 | gt_alpha = 0; 36 | gt_l1 = 1; 37 | gt_l2 = 2; 38 | gt = [gt_m; gt_alpha; gt_l1; gt_l2]'; 39 | 40 | %% set the estimate 41 | est_m(:,1) = linspace(0,5,nr_bins); 42 | est_m(:,2) = linspace(0,5,nr_bins); 43 | est_alpha = 0; 44 | est_l1 = 1; 45 | est_l2 = 2; 46 | est = [est_m, repmat(est_alpha,nr_bins,1), repmat(est_l1',nr_bins,1), repmat(est_l2',nr_bins,1) ]; 47 | 48 | 49 | 50 | %% initialization 51 | d_gw= zeros(nr_bins,1); 52 | d_kl= zeros(nr_bins,1); 53 | d_uniform_OSPA4= zeros(nr_bins,1); 54 | d_uniform_OSPA100 = zeros(nr_bins,1); 55 | centers_err = zeros(nr_bins,1); 56 | 57 | %% visulization 58 | figure 59 | subplot(1,2,1) 60 | gt_plot = plot_extent(gt, '-', 'k',1); 61 | axis equal 62 | xlim([-2.5,7]) 63 | ylim([-2.5,7]) 64 | grid on 65 | box on 66 | 67 | 68 | subplot(1,2,2) 69 | xlim([0,8]) 70 | ylim([0,15]) 71 | grid on 72 | box on 73 | 74 | %% compute the errors 75 | for i = 1:nr_bins 76 | 77 | if i>1 78 | delete(est_plot) 79 | end 80 | centers_err(i)=norm(est_m(i,:)); 81 | 82 | [gt_points_4, est_points_4] = get_uniform_points_boundary(gt, est(i,:),4); 83 | [gt_points_m, est_points_m] = get_uniform_points_boundary(gt,est(i,:),nr_points_boundary); 84 | 85 | d_gw(i) = d_gaussian_wasserstein(gt, est(i,:)); 86 | d_kl(i) = d_kullback_leibler(gt', est(i,:)'); 87 | 88 | % the cut-off value in OSPA is setted as a very big value:1000000 89 | [d_uniform_OSPA4(i), ~]= ospa_dist(gt_points_4, est_points_4,1000000,2); 90 | [d_uniform_OSPA100(i),~]= ospa_dist(gt_points_m,est_points_m,1000000,2); 91 | 92 | %% visulization 93 | subplot(1,2,1) 94 | hold on 95 | est_plot = plot_extent(est(i,:), '--', 'g', 1); 96 | legend([gt_plot,est_plot],{'Ground Truth','Estimate'}); 97 | 98 | subplot(1,2,2) 99 | hold on 100 | plot(centers_err(i),d_kl(i),'b+',centers_err(i),d_gw(i),'r*',centers_err(i),d_uniform_OSPA4(i),'go',centers_err(i),d_uniform_OSPA100(i),'k.') 101 | error_leg=legend({'$d_{KL}$','$d_{GW}$','$d_{OSPA_4}$','$d_{OSPA_{100}}$'}); 102 | set(error_leg,'Interpreter','latex'); 103 | pause(0.000001) 104 | 105 | 106 | end 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /Evaluation/scenario2_Fig5.m: -------------------------------------------------------------------------------- 1 | % This script gives the scenario 2 and its error plot (2nd column of Fig.5) of our paper: 2 | % Shishan Yang, Marcus Baum, and Karl Granstroem. "Metrics for Performance 3 | % Evaluation of Ellipitical Extended Object Tracking Methods", 4 | % The 2016 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2016) 5 | % ---BibTeX entry 6 | % @InProceedings{MFI16_Yang, 7 | % Title = {{Metrics for Performance Evaluation of Elliptic Extended Object Tracking Methods}}, 8 | % Author = {Shishan Yang and Marcus Baum and Karl Granstr\"om}, 9 | % Booktitle = {{IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2016)}}, 10 | % Year = {2016}, 11 | % 12 | % Owner = {yang}, 13 | % Timestamp = {2016.07.01} 14 | % } 15 | 16 | 17 | clc 18 | close all 19 | clear 20 | dbstop error 21 | 22 | addpath('hungarian/') 23 | 24 | nr_points_boundary = 100; 25 | nr_bins = 100; 26 | 27 | 28 | % ellipse parameterization: [center1,center2,angle,length 0f 29 | % semmi-axis1,length of semmi-axis2] 30 | % 31 | %% set the ground truth 32 | gt_m = [0 0]'; 33 | gt_alpha = 0; 34 | gt_l1 = 1; 35 | gt_l2 = 2; 36 | gt = [gt_m; gt_alpha; gt_l1; gt_l2]'; 37 | 38 | %% set the estimate 39 | est_m =[0 0]'; 40 | est_alpha = 5*pi/6; 41 | est_l1 = 1; 42 | est_l2 = linspace(0.1,2*gt_l2,nr_bins)'; 43 | est = [repmat(est_m',nr_bins,1), est_alpha*ones(nr_bins,1), est_l1*ones(nr_bins,1), est_l2]; 44 | 45 | 46 | %% initialization 47 | d_gw= zeros(nr_bins,1); 48 | d_kl= zeros(nr_bins,1); 49 | d_uniform_OSPA4= zeros(nr_bins,1); 50 | d_uniform_OSPA100 = zeros(nr_bins,1); 51 | centers_err = zeros(nr_bins,1); 52 | 53 | 54 | %% visuliztion 55 | figure 56 | subplot(1,2,1) 57 | gt_plot = plot_extent(gt, '-', 'k',1); 58 | axis equal 59 | xlim([-4,4]) 60 | ylim([-4,4]) 61 | grid on 62 | box on 63 | 64 | 65 | subplot(1,2,2) 66 | xlim([0,2]) 67 | ylim([0,3]) 68 | grid on 69 | box on 70 | 71 | 72 | for i = 1:nr_bins 73 | 74 | if i>1 75 | delete(est_plot) 76 | end 77 | 78 | len_scale = est_l2/gt_l2; 79 | [gt_points_4, est_points_4] = get_uniform_points_boundary(gt, est(i,:),4); 80 | [gt_points_m, est_points_m] = get_uniform_points_boundary(gt,est(i,:),nr_points_boundary); 81 | 82 | d_gw(i) = d_gaussian_wasserstein(gt, est(i,:)); 83 | d_kl(i) = d_kullback_leibler(gt', est(i,:)'); 84 | 85 | % the cut-off value in OSPA is setted as a very big value:1000000 86 | [d_uniform_OSPA4(i), ~]= ospa_dist(gt_points_4, est_points_4,1000000,2); 87 | [d_uniform_OSPA100(i),~]= ospa_dist(gt_points_m,est_points_m,1000000,2); 88 | 89 | 90 | %% visulization 91 | subplot(1,2,1) 92 | hold on 93 | est_plot = plot_extent(est(i,:), '--', 'g', 1); 94 | legend([gt_plot,est_plot],{'Ground Truth','Estimate'}); 95 | 96 | subplot(1,2,2) 97 | hold on 98 | plot(len_scale(i), d_kl(i),'b+', len_scale(i), d_gw(i), 'r*',... 99 | len_scale(i), d_uniform_OSPA4(i),'go', len_scale(i),d_uniform_OSPA100(i),'k.') 100 | error_leg=legend({'$d_{KL}$','$d_{GW}$','$d_{OSPA_4}$','$d_{OSPA_{100}}$'}); 101 | set(error_leg,'Interpreter','latex'); 102 | pause(0.000001) 103 | end 104 | 105 | 106 | -------------------------------------------------------------------------------- /Evaluation/scenario3_Fig5.m: -------------------------------------------------------------------------------- 1 | % This script gives the scenario 3 and its error plot (first column of Fig.5) of our paper: 2 | % Shishan Yang, Marcus Baum, and Karl Granstroem. "Metrics for Performance 3 | % Evaluation of Ellipitical Extended Object Tracking Methods", 4 | % The 2016 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2016) 5 | % ---BibTeX entry 6 | % @InProceedings{MFI16_Yang, 7 | % Title = {{Metrics for Performance Evaluation of Elliptic Extended Object Tracking Methods}}, 8 | % Author = {Shishan Yang and Marcus Baum and Karl Granstr\"om}, 9 | % Booktitle = {{IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2016)}}, 10 | % Year = {2016}, 11 | % 12 | % Owner = {yang}, 13 | % Timestamp = {2016.07.01} 14 | % } 15 | 16 | 17 | clc 18 | close all 19 | clear 20 | dbstop error 21 | 22 | 23 | addpath('hungarian/') 24 | 25 | 26 | 27 | nr_points_boundary = 100; % nr of points that used for the calculation of the uniform OSPA 28 | nr_bins = 100; 29 | 30 | % ellipse parameterization: [center1,center2,angle,length 0f 31 | % semmi-axis1,length of semmi-axis2] 32 | % 33 | %% set the ground truth 34 | gt_m = [0 0]'; 35 | gt_alpha = 0; 36 | gt_l1 = 1; 37 | gt_l2 = 2; 38 | gt = [gt_m; gt_alpha; gt_l1; gt_l2]'; 39 | 40 | %% set the estimate 41 | est_m = [0 0]'; 42 | 43 | est_alpha = linspace(0,pi,nr_bins)'; 44 | est_l1 = 1; 45 | est_l2 = 2; 46 | est = [repmat(est_m',nr_bins,1), est_alpha, repmat(est_l1',nr_bins,1), repmat(est_l2',nr_bins,1) ]; 47 | 48 | 49 | %% initialization 50 | d_gw= zeros(nr_bins,1); 51 | d_kl= zeros(nr_bins,1); 52 | d_uniform_OSPA4= zeros(nr_bins,1); 53 | d_uniform_OSPA100 = zeros(nr_bins,1); 54 | centers_err = zeros(nr_bins,1); 55 | 56 | 57 | figure 58 | subplot(1,2,1) 59 | gt_plot = plot_extent(gt, '-', 'k', 1); 60 | axis equal 61 | xlim([-3,3]) 62 | ylim([-3,3]) 63 | grid on 64 | box on 65 | 66 | 67 | subplot(1,2,2) 68 | xlim([0,3.2]) 69 | ylim([0,1.5]) 70 | grid on 71 | box on 72 | 73 | 74 | for i = 1:nr_bins 75 | if i>1 76 | delete(est_plot) 77 | end 78 | 79 | [gt_points_4, est_points_4] = get_uniform_points_boundary(gt, est(i,:),4); 80 | [gt_points_m, est_points_m] = get_uniform_points_boundary(gt,est(i,:),nr_points_boundary); 81 | 82 | d_gw(i) = d_gaussian_wasserstein(gt, est(i,:)); 83 | d_kl(i) = d_kullback_leibler(gt', est(i,:)'); 84 | 85 | % the cut-off value in OSPA is setted as a very big value:1000000 86 | [d_uniform_OSPA4(i), ~]= ospa_dist(gt_points_4, est_points_4,1000000,2); 87 | [d_uniform_OSPA100(i),~]= ospa_dist(gt_points_m,est_points_m,1000000,2); 88 | 89 | 90 | %% visulization 91 | subplot(1,2,1) 92 | hold on 93 | est_plot = plot_extent(est(i,:), '--', 'g', 1); 94 | legend([gt_plot,est_plot],{'Ground Truth','Estimate'}); 95 | 96 | subplot(1,2,2) 97 | hold on 98 | plot(est_alpha(i), d_kl(i),'b+', est_alpha(i), d_gw(i), 'r*',... 99 | est_alpha(i), d_uniform_OSPA4(i),'go', est_alpha(i),d_uniform_OSPA100(i),'k.') 100 | error_leg=legend({'$d_{KL}$','$d_{GW}$','$d_{OSPA_4}$','$d_{OSPA_{100}}$'}); 101 | set(error_leg,'Interpreter','latex'); 102 | pause(0.000001) 103 | 104 | 105 | end 106 | 107 | -------------------------------------------------------------------------------- /MEM-EKFstar/demo.m: -------------------------------------------------------------------------------- 1 | % Implementation of the MEM-EKF* algorithm based on the article 2 | % 3 | % "Tracking the Orientation and Axes Lengths of an Elliptical Extended Object" 4 | % Shishan Yang and Marcus Baum 5 | % arXiv preprint, 2018, 6 | % https://arxiv.org/abs/1805.03276 7 | % 8 | % Further information: 9 | % http://www.fusion.informatik.uni-goettingen.de 10 | % https://github.com/Fusion-Goettingen 11 | % 12 | % Source code written by Shishan Yang 13 | 14 | close all 15 | clc 16 | clear 17 | dbstop error 18 | 19 | % generate ground truth 20 | [gt_center, gt_rotation, gt_orient, gt_length, gt_vel, time_steps, time_interval] =get_ground_truth; 21 | gt = [gt_center;gt_orient;gt_length;gt_vel]; 22 | 23 | % nearly constant velocity model 24 | H = [1 0 0 0; 0 1 0 0]; 25 | Ar =[1 0 10 0; 0 1 0 10; 0 0 1 0; 0 0 0 1] ; 26 | Ap = eye(3); 27 | 28 | Ch = diag([1/4, 1/4]); % covariance of the multiplicative noise 29 | Cv = diag([200 8]); % covariance of the measurement noise 30 | Cwr = diag([100 100 1 1]); % covariance of the process noise for the kinematic state 31 | Cwp = diag([0.05 0.001 0.001]); %covariance of the process noise for the shape parameters 32 | 33 | lambda = 5;% Nr of measurements is Poisson distributed with mean lambda 34 | 35 | 36 | %% Prior 37 | r = [100, 100,10, -17]'; 38 | p = [-pi/3 200 90]'; 39 | 40 | Cr = diag([900 900 16 16]); 41 | Cp = diag([0.2 400 400]); 42 | 43 | figure; 44 | hold on 45 | for t = 1:time_steps 46 | %% generate measurements 47 | nk = poissrnd(lambda); 48 | while nk == 0 49 | nk = poissrnd(lambda); 50 | end 51 | disp(['Time step: ' num2str(t) ', ' num2str(nk) ' Measurements']); 52 | 53 | y = zeros(2, nk); 54 | for n = 1:nk 55 | h(n, :) = -1 + 2.*rand(1, 2); 56 | while norm(h(n, :)) > 1 57 | h(n, :) = -1 + 2.*rand(1, 2); 58 | end 59 | 60 | y(:, n) = gt(1:2, t) + h(n, 1)*gt(4, t)*... 61 | [cos(gt(3, t)); sin(gt(3, t))] + h(n, 2)*gt(5, t)*... 62 | [-sin(gt(3, t)); cos(gt(3, t))] + mvnrnd([0 0], Cv, 1)'; 63 | end 64 | 65 | %% measurement update 66 | [r,p,Cr,Cp] = measurement_update(y,H,r,p,Cr,Cp,Ch,Cv); 67 | 68 | 69 | %% visualize estimate and ground truth for every 3rd scan 70 | if mod(t, 3)==1 71 | meas_points=plot( y(1, :), y(2, :), '.k', 'lineWidth', 0.5); 72 | hold on 73 | axis equal 74 | gt_plot = plot_extent(gt(:, t), '-', 'k', 1); 75 | est_plot = plot_extent([r(1:2);p ], '-', 'r', 1); 76 | pause(0.1) 77 | end 78 | 79 | %% time update 80 | [r,p,Cr,Cp]= time_update(r,p,Cr,Cp,Ar,Ap,Cwr,Cwp); 81 | 82 | 83 | end 84 | legend([gt_plot, est_plot, meas_points], {'Ground truth', 'Estimate', 'Measurement'},'Location','northwest'); 85 | -------------------------------------------------------------------------------- /MEM-EKFstar/get_ground_truth.m: -------------------------------------------------------------------------------- 1 | % Implementation of the MEM-EKF* algorithm based on the article 2 | % 3 | % "Tracking the Orientation and Axes Lengths of an Elliptical Extended Object" 4 | % Shishan Yang and Marcus Baum 5 | % arXiv preprint, 2018, 6 | % https://arxiv.org/abs/1805.03276 7 | % 8 | % Further information: 9 | % http://www.fusion.informatik.uni-goettingen.de 10 | % https://github.com/Fusion-Goettingen 11 | % 12 | % Source code written by Shishan Yang 13 | % ============================= 14 | 15 | 16 | function [gt_center, gt_rotation, gt_orient, gt_length, gt_vel, time_steps, time_interval] = get_ground_truth 17 | %% -------------Setting ground truth--------------------------------------- 18 | gt_center(:, 1) = [0, 0]'; 19 | % trajectory 20 | gt_orient = [ repmat(-pi/4, 1, 20), (-pi/4: pi/40:0), ... 21 | zeros(1, 10), (0: pi/20:2*pi/4), .... 22 | repmat(2*pi/4, 1, 20), (2*pi/4: pi/20:pi), .... 23 | repmat(pi, 1, 20)]; 24 | % assume object is aligned along its velocity 25 | gt_vel = [(500/36)*cos(gt_orient);(500/36)*sin(gt_orient)]; 26 | gt_length = repmat([340/2;80/2], 1, size(gt_vel, 2)); 27 | 28 | time_steps = size(gt_vel, 2); 29 | time_interval = 10; 30 | 31 | 32 | gt_rotation = zeros(2, 2, time_steps); 33 | for t = 1 : time_steps 34 | gt_rotation(:, :, t) = [cos(gt_orient(t)), -sin(gt_orient(t)); sin(gt_orient(t)), cos(gt_orient(t))]; 35 | if t>1 36 | gt_center(:, t) = gt_center(:, t-1) + gt_vel(:, t)*time_interval; 37 | end 38 | end 39 | 40 | 41 | end 42 | 43 | -------------------------------------------------------------------------------- /MEM-EKFstar/measurement_update.m: -------------------------------------------------------------------------------- 1 | % Implementation of the MEM-EKF* algorithm based on the article 2 | % 3 | % "Tracking the Orientation and Axes Lengths of an Elliptical Extended Object" 4 | % Shishan Yang and Marcus Baum 5 | % arXiv preprint, 2018, 6 | % https://arxiv.org/abs/1805.03276 7 | % 8 | % Further information: 9 | % http://www.fusion.informatik.uni-goettingen.de 10 | % https://github.com/Fusion-Goettingen 11 | % 12 | % Source code written by Shishan Yang 13 | % ============================= 14 | 15 | function [r,p,Cr,Cp] = measurement_update(y,H,r,p,Cr,Cp,Ch,Cv) 16 | nk = size(y,2); % number of measurements at time k 17 | 18 | for i = 1:nk 19 | 20 | [CI,CII,M,F,Ftilde] = get_auxiliary_variables(p,Cp,Ch); 21 | 22 | yi = y(:,i); 23 | 24 | % calculate moments for the kinematic state update 25 | yibar = H*r; 26 | Cry = Cr*H'; 27 | Cy = H*Cr*H'+CI+CII+Cv; 28 | % udpate kinematic estimate 29 | r = r + Cry*(Cy)^(-1)*(yi-yibar); 30 | Cr = Cr - Cry*(Cy)^(-1)*Cry'; 31 | % Enforce symmetry of the covariance 32 | Cr = (Cr+Cr')/2; 33 | 34 | % construct pseudo-measurement for the shape update 35 | Yi = F*kron(yi-yibar,yi-yibar); 36 | % calculate moments for the shape update 37 | Yibar = F*reshape(Cy,[4,1]); 38 | CpY = Cp*M'; 39 | CY = F*kron(Cy,Cy)*(F + Ftilde)'; 40 | % update shape 41 | p = p + CpY*CY^(-1)*(Yi-Yibar); 42 | Cp = Cp - CpY*CY^(-1)*CpY'; 43 | % Enforce symmetry of the covariance 44 | Cp = (Cp+Cp')/2; 45 | end 46 | end 47 | 48 | 49 | function [CI,CII,M,F,Ftilde] = get_auxiliary_variables(p,Cp,Ch) 50 | alpha = p(1); 51 | l1 = p(2); 52 | l2 = p(3); 53 | 54 | S = [cos(alpha) -sin(alpha); sin(alpha) cos(alpha)]*diag([l1 l2]); 55 | S1 = S(1,:); 56 | S2 = S(2,:); 57 | 58 | J1 = [-l1*sin(alpha) cos(alpha) 0; -l2*cos(alpha) 0 -sin(alpha)]; 59 | J2 = [ l1*cos(alpha) sin(alpha) 0; -l2*sin(alpha) 0 cos(alpha)]; 60 | 61 | CI = S*Ch*S'; 62 | CII(1,1) = trace(Cp*J1'*Ch*J1); 63 | CII(1,2) = trace(Cp*J2'*Ch*J1); 64 | CII(2,1) = trace(Cp*J1'*Ch*J2); 65 | CII(2,2) = trace(Cp*J2'*Ch*J2); 66 | 67 | M = [2*S1*Ch*J1; 2*S2*Ch*J2; S1*Ch*J2 + S2*Ch*J1]; 68 | 69 | F = [1 0 0 0; 0 0 0 1; 0 1 0 0]; 70 | Ftilde = [1 0 0 0; 0 0 0 1; 0 0 1 0]; 71 | end -------------------------------------------------------------------------------- /MEM-EKFstar/plot_extent.m: -------------------------------------------------------------------------------- 1 | % Implementation of the MEM-EKF* algorithm based on the article 2 | % 3 | % "Tracking the Orientation and Axes Lengths of an Elliptical Extended Object" 4 | % Shishan Yang and Marcus Baum 5 | % arXiv preprint, 2018, 6 | % https://arxiv.org/abs/1805.03276 7 | % 8 | % Further information: 9 | % http://www.fusion.informatik.uni-goettingen.de 10 | % https://github.com/Fusion-Goettingen 11 | % 12 | % Source code written by Shishan Yang 13 | % ============================= 14 | 15 | function [handle_extent] = plot_extent(ellipse,line_style, color, line_width) 16 | % PLOT_EXTENT plots the extent of an ellipse or circle 17 | % Input: 18 | % ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2] 19 | % line_style, defined as in the Matlab plot function 20 | % color, defined as in the Matlab plot function 21 | % line_width, defined as in the Matlab plot function 22 | % 23 | % Output: 24 | % handle_extent, the handle of the plot 25 | % 26 | % Written by Shishan Yang 27 | 28 | 29 | 30 | center = ellipse(1:2); 31 | theta = ellipse(3); 32 | l = ellipse(4:5); 33 | R = [cos(theta) -sin(theta); sin(theta) cos(theta)]; %rotation matrix 34 | 35 | 36 | alpha = 0:pi/100:2*pi; 37 | xunit = l(1)*cos(alpha); 38 | yunit = l(2)*sin(alpha); 39 | 40 | rotated = R* [xunit; yunit]; 41 | xpoints = rotated(1,:) + center(1); 42 | ypoints = rotated(2,:) + center(2); 43 | 44 | 45 | handle_extent = plot(xpoints,ypoints,'LineStyle',line_style,'color',color,'LineWidth',line_width); 46 | 47 | end -------------------------------------------------------------------------------- /MEM-EKFstar/time_update.m: -------------------------------------------------------------------------------- 1 | % Implementation of the MEM-EKF* algorithm based on the article 2 | % 3 | % "Tracking the Orientation and Axes Lengths of an Elliptical Extended Object" 4 | % Shishan Yang and Marcus Baum 5 | % arXiv preprint, 2018, 6 | % https://arxiv.org/abs/1805.03276 7 | % 8 | % Further information: 9 | % http://www.fusion.informatik.uni-goettingen.de 10 | % https://github.com/Fusion-Goettingen 11 | % 12 | % Source code written by Shishan Yang 13 | % ============================= 14 | 15 | function [r,p,Cr,Cp]= time_update(r,p,Cr,Cp,Ar, Ap,Cwr, Cwp) 16 | r = Ar*r; 17 | Cr = Ar*Cr*Ar'+Cwr; 18 | 19 | p = Ap*p; 20 | Cp = Ap*Cp*Ap'+Cwp; 21 | end -------------------------------------------------------------------------------- /MEM_EKF/d_gaussian_wasserstein.m: -------------------------------------------------------------------------------- 1 | function [distance] = d_gaussian_wasserstein( ellipse1, ellipse2) 2 | % Gaussian wasserstein distance between two ellipses 3 | % 4 | % Reference: 5 | % S. Yang, M. Baum, and K. Granström. Metrics for Performance Evaluation of 6 | % of Elliptic Extended Object Tracking Methods. Proceedings of the 2016 7 | % IEEE International Conference on Multisensor Fusion and Integration for 8 | % Intelligent Systems (MFI 2016), Baden-Baden, Germany, 2016. 9 | % 10 | % Input: 11 | % ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2] 12 | % ellipse2, 1x5, parameterization of the other ellispe [m1 m2 alpha l1 l2] 13 | % 14 | % Output: 15 | % distance, scalar, gaussian wasserstein distance 16 | % 17 | % Written by Shishan Yang 18 | 19 | m1 = ellipse1(1:2); 20 | alpha1 = ellipse1(3); 21 | eigen_val1 = [ellipse1(4), ellipse1(5)]; 22 | eigen_vec1 = [cos(alpha1), -sin(alpha1); sin(alpha1), cos(alpha1)]; 23 | sigma1 = eigen_vec1*diag(eigen_val1.^2)*eigen_vec1'; 24 | sigma1 = (sigma1 + sigma1')/2; % make covariance symmetric 25 | 26 | 27 | 28 | m2 = ellipse2(1:2); 29 | alpha2 = ellipse2(3); 30 | eigen_val2 = [ellipse2(4),ellipse2(5)]; 31 | eigen_vec2 = [cos(alpha2), -sin(alpha2); sin(alpha2), cos(alpha2)]; 32 | sigma2 = eigen_vec2*diag(eigen_val2.^2)*eigen_vec2'; 33 | sigma2 = (sigma2 + sigma2')/2; % make covariance symmetric 34 | 35 | error_sq = norm(m1-m2)^2 + trace(sigma1 + sigma2 -2*sqrtm((sqrtm(sigma1)*sigma2*sqrtm(sigma1)))); 36 | distance = sqrt(error_sq); 37 | 38 | 39 | end -------------------------------------------------------------------------------- /MEM_EKF/get_ground_truth.m: -------------------------------------------------------------------------------- 1 | function [gt_kin,gt_par, time_steps, time_interval] = get_ground_truth 2 | 3 | 4 | gt_center(:, 1) = [0, 0]'; 5 | % trajectory 6 | gt_orient = [ repmat(-pi/4, 1, 25), (-pi/4: pi/40:0), ... 7 | zeros(1, 20), (0: pi/20:2*pi/4), .... 8 | repmat(2*pi/4, 1, 15), (2*pi/4: pi/20:pi), .... 9 | repmat(pi, 1, 40)]; 10 | % assume object is aligned along its velocity 11 | gt_vel = [(500/36)*cos(gt_orient);(500/36)*sin(gt_orient)]; 12 | gt_length = repmat([340/2;80/2], 1, size(gt_vel, 2)); 13 | 14 | time_steps = size(gt_vel, 2); 15 | time_interval = 10; 16 | 17 | 18 | gt_rotation = zeros(2, 2, time_steps); 19 | for t = 1 : time_steps 20 | gt_rotation(:, :, t) = [cos(gt_orient(t)), -sin(gt_orient(t)); sin(gt_orient(t)), cos(gt_orient(t))]; 21 | if t>1 22 | gt_center(:, t) = gt_center(:, t-1) + gt_vel(:, t)*time_interval; 23 | end 24 | end 25 | gt_kin = [gt_center;gt_vel]; 26 | gt_par = [gt_center;gt_orient;gt_length]; 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /MEM_EKF/get_jacobian_hessian.m: -------------------------------------------------------------------------------- 1 | function [ f_g, f_jac, f_hes] = get_jacobian_hessian(motionmodel, C_h) 2 | % GET_JACOBIAN_HESSIAN: calculates the Jacobian and Hessians (using the Matlab symbolic toolbox) 3 | % Input: 4 | % motionmodel: string, either 'static' or 'NCV' 5 | % h1_var: variance of multiplicatice error h1 6 | % h2_var: variance of multiplicatice error h2 7 | % Output: 8 | % f_g: quadratic function handle 9 | % f_jac: handle of Jacobian matrix of func_g with modified substitution 10 | % f_hes: handle of Hessian matrix of funct_g with modified substitution 11 | 12 | syms m1 m2 h1 h2 l1 l2 v1 v2 s1 s2 a 13 | y1 = (m1 + h1*l1*cos(a) - h2*l2*sin(a) + v1); 14 | y2 = (m2 + h1*l1*sin(a) + h2*l2*cos(a) + v2); 15 | 16 | func_g =[y1;y2;y1^2;y1*y2;y2^2]; 17 | h1_var = C_h(1,1); 18 | h2_var = C_h(2,2); 19 | if strcmp(motionmodel, 'static') 20 | X = [m1;m2; a;l1;l2;h1;h2;v1;v2]; 21 | 22 | elseif strcmp(motionmodel, 'NCV') 23 | X = [m1;m2;s1;s2; a;l1;l2;h1;h2;v1;v2]; 24 | 25 | else 26 | error('unknown dynamic model'); 27 | end 28 | 29 | jacobian_mat = jacobian(func_g, X); 30 | 31 | for i = 1:numel(func_g) 32 | hessian_mat(:, :, i) = hessian(func_g(i), X); 33 | end 34 | jacobian_mat = subs(expand(jacobian_mat), [h1^2, h2^2], [h1_var, h2_var]); 35 | hessian_mat = subs(expand(hessian_mat), [h1^2, h2^2], [h1_var, h2_var]); 36 | 37 | f_g = matlabFunction(func_g, 'Vars', {[m1, m2, a, l1, l2, h1, h2, v1, v2]}); 38 | f_jac = matlabFunction(jacobian_mat, 'Vars', {[m1, m2, a, l1, l2, h1, h2, v1, v2]}); 39 | f_hes = matlabFunction(hessian_mat, 'Vars', {[m1, m2, a, l1, l2, h1, h2, v1, v2]}); 40 | 41 | end -------------------------------------------------------------------------------- /MEM_EKF/get_random_matrix_ellipse.m: -------------------------------------------------------------------------------- 1 | function [rmm_rotation, rmm_l, fai] = ... 2 | get_random_matrix_ellipse(rmm_extent) 3 | % notations and formmulas are from "Ellipse fitting based approach for extended object tracking" 4 | % Equation (9) 5 | rho = (rmm_extent(1,1) - rmm_extent(2,2))/(2*rmm_extent(1,2)); 6 | 7 | fai = atan(-rho + sqrt(1 + rho^2)); 8 | 9 | rmm_rotation = [cos(fai), -sin(fai); sin(fai), cos(fai)]; 10 | rmm_l_sq = diag(rmm_rotation'*rmm_extent*rmm_rotation); 11 | rmm_l = rmm_l_sq.^(1/2); 12 | end -------------------------------------------------------------------------------- /MEM_EKF/get_random_matrix_state.m: -------------------------------------------------------------------------------- 1 | function A = get_random_matrix_state(ellipse_extent) 2 | % only ellipse parameterization alpha is implemented 3 | % by Shishan Yang 4 | alpha = ellipse_extent(1); 5 | eigen_val = ellipse_extent(2:3); 6 | eigen_vec = [cos(alpha), -sin(alpha); sin(alpha), cos(alpha)]; 7 | A = eigen_vec*diag(eigen_val.^2)*eigen_vec'; 8 | A = (A+A')/2; % make A symmetric 9 | end -------------------------------------------------------------------------------- /MEM_EKF/measurement_update_ekf.m: -------------------------------------------------------------------------------- 1 | function [ x_est, x_cov ] = measurement_update_ekf(mem_x_pre, mem_cov_pre, meas, ... 2 | f_Jh, f_Jp, meas_noise_cov, multi_noise_cov) 3 | 4 | 5 | 6 | 7 | est_mm = [mem_x_pre(1:2);mem_x_pre(6:7)]; 8 | cov_mm = [mem_cov_pre(1:2,1:2) mem_cov_pre(1:2,6:7);mem_cov_pre(6:7,1:2) mem_cov_pre(6:7,6:7)]; 9 | cov_pp = mem_cov_pre(3:5,3:5); 10 | Jm = [eye(2) zeros(2,2)]; 11 | 12 | subs_Jh = f_Jh(mem_x_pre(3:5)'); 13 | subs_Jp = f_Jp(mem_x_pre(3:5)'); 14 | %% construct pseudo-measurement 15 | 16 | Y= [(meas(1) - mem_x_pre(1))^2;... 17 | (meas(2) - mem_x_pre(2))^2;... 18 | (meas(1) - mem_x_pre(1))*(meas(2) - mem_x_pre(2))]; 19 | %% kinematic update 20 | cov_yy = Jm*cov_mm*Jm' + subs_Jh*multi_noise_cov*subs_Jh' + meas_noise_cov; 21 | cov_my = cov_mm*Jm'; 22 | 23 | est_kin = est_mm + cov_my* cov_yy^-1* (meas-Jm*est_mm(1:4)); 24 | kin_cov = cov_mm - cov_my* cov_yy^-1*cov_my'; 25 | kin_cov = (kin_cov+kin_cov')/2; 26 | %% shape update 27 | 28 | sg11 = cov_yy(1,1); 29 | sg12 = cov_yy(1,2); 30 | sg22 = cov_yy(2,2); 31 | 32 | expect_Y = [sg11;sg22;sg12]; 33 | 34 | cov_xY = cov_pp*subs_Jp'; 35 | cov_YY = [3*sg11^2, sg11*sg22+ 2*sg12^2, 3*sg11*sg12;... 36 | sg11*sg22+ 2*sg12^2,3*sg22^2 , 3*sg22*sg12;... 37 | 3*sg11*sg12, 3*sg22*sg12,sg11*sg22+ 2*sg12^2]; 38 | 39 | 40 | 41 | 42 | 43 | est_shape = mem_x_pre(3:5) + cov_xY* cov_YY^-1* (Y- expect_Y); 44 | shape_cov = cov_pp - cov_xY* cov_YY^-1*cov_xY'; 45 | 46 | x_est = [est_kin(1:2);est_shape;est_kin(3:4)]; 47 | x_cov = [kin_cov(1:2,1:2), zeros(2,3), kin_cov(1:2,3:4);... 48 | zeros(3,2), shape_cov,zeros(3,2); ... 49 | kin_cov(3:4,1:2), zeros(2,3),kin_cov(3:4,3:4)]; 50 | 51 | end -------------------------------------------------------------------------------- /MEM_EKF/plot_extent.m: -------------------------------------------------------------------------------- 1 | function [handle_extent] = plot_extent(ellipse,line_style, color, line_width) 2 | % PLOT_EXTENT plots the extent of an ellipse or circle 3 | % Input: 4 | % ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2] 5 | % line_style, definedthe same as in Matlab plot function 6 | % color, definedthe same as in Matlab plot function 7 | % line_width, definedthe same as in Matlab plot function 8 | % 9 | % Output: 10 | % handle_extent, the handle of the plot 11 | % 12 | % Written by Shishan Yang 13 | 14 | 15 | 16 | center = ellipse(1:2); 17 | theta = ellipse(3); 18 | l = ellipse(4:5); 19 | R = [cos(theta) -sin(theta); sin(theta) cos(theta)]; %rotation matrix 20 | 21 | 22 | alpha = 0:pi/100:2*pi; 23 | xunit = l(1)*cos(alpha); 24 | yunit = l(2)*sin(alpha); 25 | 26 | rotated = R* [xunit; yunit]; 27 | xpoints = rotated(1,:) + center(1); 28 | ypoints = rotated(2,:) + center(2); 29 | 30 | 31 | handle_extent = plot(xpoints,ypoints,'LineStyle',line_style,'color',color,'LineWidth',line_width); 32 | 33 | end -------------------------------------------------------------------------------- /MEM_EKF/predictEKF.m: -------------------------------------------------------------------------------- 1 | function [hat_r, C_r, hat_p, C_p] = predictEKF(Ar,Ap, hat_r, hat_p, C_r, C_p, C_w_r, C_w_p) 2 | hat_r = Ar*hat_r; 3 | hat_p = Ap*hat_p; 4 | C_r = Ar*C_r*Ar' + C_w_r; 5 | C_p = Ap*C_p*Ap' + C_w_p; 6 | end -------------------------------------------------------------------------------- /MEM_EKF/predictRMM.m: -------------------------------------------------------------------------------- 1 | function [x_pred, X_pred,P_pred, alpha_pred] = predictRMM(.... 2 | x, X, P, alpha,F,Q,T,tau) 3 | % formulas are from : 4 | % M. Feldmann and D. Fraenken. "Tracking of Extended 5 | % Objects and Group Targets using Random Matrices - A Pereformance 6 | % Analysis" 7 | 8 | % by Shishan Yang 9 | 10 | x_pred = F * x; 11 | P_pred = F * P * F' + Q; 12 | 13 | X_pred = X; 14 | alpha_pred = 2 + exp(-T/tau)*(alpha - 2); 15 | 16 | end -------------------------------------------------------------------------------- /MEM_EKF/predictSOEKF.m: -------------------------------------------------------------------------------- 1 | function [ hat_x,Cx]= predictSOEKF(A,hat_x,C_x,C_w) 2 | 3 | Cx = A*C_x*A' + C_w; 4 | hat_x = A*hat_x; 5 | end -------------------------------------------------------------------------------- /MEM_EKF/rmse.m: -------------------------------------------------------------------------------- 1 | % This script compares three elliptical extended object trackig methods. 2 | % It gives Fig.3 in 3 | % S. Yang and M. Baum Extended Kalman Filter for Extended Object Tracking. 4 | % Proceedings of the 2017 IEEE International Conference on Acoustics, Speech, 5 | % and Signal Processing (ICASSP), New Orleans, USA, 2017. 6 | 7 | 8 | close all 9 | clc 10 | clear 11 | dbstop warning 12 | set(0,'defaulttextinterpreter','latex') 13 | 14 | %% parameters 15 | 16 | mc_runs = 500; 17 | % nr of measurements we get follows a possion distribution 18 | possion_lambda = 5; 19 | H = [1 0 0 0; 0 1 0 0]; % matrix maps kinematic state into position 20 | H_velo = [zeros(2,2),eye(2)]; 21 | H_par_SOEKF = [eye(2),zeros(2,5);zeros(3,4),eye(3,3)]; 22 | H_velo_SOEKF = [zeros(2,2),eye(2),zeros(2,3)]; 23 | motionmodel = {'NCV'}; 24 | 25 | %% generate ground truth 26 | [gt_kin,gt_par, time_steps, delta_t] =get_ground_truth; 27 | 28 | d_RMM=zeros(mc_runs,time_steps); 29 | d_SOEKF=zeros(mc_runs,time_steps); 30 | d_EKF=zeros(mc_runs,time_steps); 31 | 32 | v_RMM=zeros(mc_runs,time_steps); 33 | v_SOEKF=zeros(mc_runs,time_steps); 34 | v_EKF=zeros(mc_runs,time_steps); 35 | %% setting prior 36 | hat_r0 = [100,100,5,-8]'; % kinematic state: position and velocity 37 | hat_p0 = [-pi/3,200,90]'; % shape variable: orientation and semi-axes lengths 38 | hat_x0 = [hat_r0; hat_p0]; 39 | 40 | C_r0 = blkdiag( 900*eye(2),16*eye(2)); 41 | C_p0 = blkdiag(0.02*eye(1),400*eye(2)); 42 | C_x0 = blkdiag(C_r0,C_p0); 43 | 44 | 45 | % parameters for EKF 46 | C_h = diag([1/4, 1/4]); 47 | C_v = 0.2*diag([100^2,20^2]); 48 | 49 | Ar = [eye(2),delta_t*eye(2); zeros(2,2),eye(2)]; 50 | Ap = eye(3); 51 | 52 | C_w_r = blkdiag(100*eye(2),eye(2)); % process noise covariance for kinematic state 53 | C_w_p = blkdiag(0.04,0.5*eye(2)); % process noise covariance for shape variable 54 | 55 | for it = 1:mc_runs 56 | 57 | hat_r_EKF = [100,100,5,-8]'; % kinematic state: position and velocity 58 | hat_p_EKF = [-pi/3,200,90]'; 59 | Cr_EKF = C_r0; 60 | Cp_EKF = C_p0; 61 | 62 | % parameters for SOEKF 63 | C_w = blkdiag(C_w_r, C_w_p); 64 | Ax = blkdiag(Ar,Ap); 65 | 66 | hat_x_SOEKF = hat_x0; 67 | Cx_SOEKF = C_x0; 68 | 69 | [ f_g_ekf2, f_jacobian_ekf2, f_hessian_ekf2] = get_jacobian_hessian(motionmodel,C_h); 70 | 71 | 72 | % parameters for Random Matrix 73 | alpha = 50; 74 | tau = 10; 75 | T = 10; 76 | const_z = 1/4; 77 | hat_x_RMM = hat_r0; 78 | hat_X_RMM = get_random_matrix_state(hat_p0); 79 | Cx_RMM = C_r0; 80 | 81 | for t = 1:time_steps 82 | N = poissrnd(possion_lambda); 83 | while N == 0 84 | N = poissrnd(possion_lambda); 85 | end 86 | disp(['time step:' num2str(t) ', ' num2str(N) ' measurements']); 87 | 88 | %% ------------------get measurements------------------------------------ 89 | gt_cur_par = gt_par(:,t); 90 | gt_velo = H_velo*gt_kin(:,t); 91 | gt_rot = [cos(gt_cur_par(3)), -sin(gt_cur_par(3)); sin(gt_cur_par(3)), cos(gt_cur_par(3))]; 92 | gt_len = gt_cur_par(4:5); 93 | y = zeros(2,N); 94 | for n = 1:N 95 | h_noise(n,:) = -1 + 2.*rand(1,2); 96 | while norm(h_noise(n,:)) > 1 97 | h_noise(n,:) = -1 + 2.*rand(1,2); 98 | end 99 | y(:,n) = H*gt_kin(:,t) + gt_rot*diag(gt_len)*h_noise(n,:)'+ mvnrnd([0 0], C_v, 1)'; 100 | end 101 | %% update RMM 102 | meas_mean = mean(y,2); 103 | meas_spread = (N - 1) * cov(y'); 104 | [hat_x_RMM, hat_X_RMM, C_x_RMM, alpha_update]... 105 | = updateRMM(hat_x_RMM, hat_X_RMM, Cx_RMM, alpha,meas_mean, ... 106 | meas_spread, C_v,N,H,const_z); 107 | 108 | [~, len_RMM,ang_RMM] = get_random_matrix_ellipse(hat_X_RMM); 109 | rmm_par = [H*hat_x_RMM; ang_RMM;len_RMM]; 110 | %% update EKF and SOEKF 111 | for n = 1:N 112 | [hat_x_SOEKF, Cx_SOEKF] = updateSOEKF(hat_x_SOEKF, Cx_SOEKF, y(:,n),... 113 | f_g_ekf2, f_jacobian_ekf2, f_hessian_ekf2, C_v, C_h); 114 | [ hat_r_EKF, Cr_EKF,hat_p_EKF, Cp_EKF ] = updateEKF(hat_r_EKF, Cr_EKF, hat_p_EKF, Cp_EKF, y(:,n), C_v, C_h); 115 | 116 | end 117 | 118 | 119 | %% evaluation 120 | 121 | d_RMM(it,t) = d_gaussian_wasserstein(gt_cur_par,rmm_par); 122 | d_SOEKF(it,t) = d_gaussian_wasserstein(gt_cur_par,H_par_SOEKF*hat_x_SOEKF); 123 | d_EKF(it,t) = d_gaussian_wasserstein(gt_cur_par,[H*hat_r_EKF; hat_p_EKF]); 124 | 125 | v_RMM(it,t) = norm(gt_velo - H_velo*hat_x_RMM); 126 | v_SOEKF(it,t)=norm(gt_velo - H_velo_SOEKF*hat_x_SOEKF); 127 | v_EKF(it,t) = norm(gt_velo - H_velo*hat_r_EKF); 128 | 129 | 130 | %% predict　RMM 131 | [hat_x_RMM, hat_X_RMM,Cx_RMM, alpha] = predictRMM(.... 132 | hat_x_RMM, hat_X_RMM, C_x_RMM, alpha_update,Ar,C_w_r,T,tau); 133 | if alpha_update<=2 134 | error('alpha<2') 135 | end 136 | %% predict SOEKF 137 | [hat_x_SOEKF,Cx_SOEKF] = predictSOEKF(Ax,hat_x_SOEKF,Cx_SOEKF,C_w); 138 | %% predict EKF 139 | [hat_r_EKF,Cr_EKF, hat_p_EKF,Cp_EKF] = predictEKF(Ar,Ap, hat_r_EKF, hat_p_EKF, Cr_EKF, Cp_EKF,C_w_r, C_w_p); 140 | 141 | end 142 | end 143 | figure; 144 | hold on 145 | plot(1:time_steps,mean(d_RMM),'g',1:time_steps,mean(d_SOEKF),'r',1:time_steps,mean(d_EKF),'b') 146 | legend({'Random Matrices','SOEKF','EKF'}) 147 | title('Extension RMSE') 148 | box on 149 | grid on 150 | 151 | figure; 152 | hold on 153 | plot(1:time_steps,mean(v_RMM),'g',1:time_steps,mean(v_SOEKF),'r',1:time_steps,mean(v_EKF),'b') 154 | legend({'Random Matrices','SOEKF','EKF'}) 155 | title('Velocity RMSE') 156 | box on 157 | grid on 158 | -------------------------------------------------------------------------------- /MEM_EKF/simulation.m: -------------------------------------------------------------------------------- 1 | % This script compares three elliptical extended object trackig methods. 2 | % It gives Fig.2 in 3 | % S. Yang and M. Baum Extended Kalman Filter for Extended Object Tracking. 4 | % Proceedings of the 2017 IEEE International Conference on Acoustics, Speech, 5 | % and Signal Processing (ICASSP), New Orleans, USA, 2017. 6 | 7 | 8 | close all 9 | clc 10 | clear 11 | dbstop warning 12 | set(0,'defaulttextinterpreter','latex') 13 | 14 | %% parameters 15 | motionmodel = {'NCV'}; 16 | % nr of measurements we get follows a possion distribution 17 | possion_lambda = 5; 18 | H = [1 0 0 0; 0 1 0 0]; % matrix maps kinematic state into position 19 | H_velo = [zeros(2,2),eye(2)]; 20 | % parameters for EKF 21 | C_h = diag([1/4, 1/4]); 22 | C_v = 0.2*diag([100^2,20^2]); 23 | 24 | 25 | %% generate ground truth 26 | [gt_kin,gt_par, time_steps, delta_t] =get_ground_truth; 27 | 28 | %% setting prior 29 | hat_r0 = [100,100,5,-8]'; % kinematic state: position and velocity 30 | hat_p0 = [-pi/3,200,90]'; % shape variable: orientation and semi-axes lengths 31 | hat_x0 = [hat_r0; hat_p0]; 32 | 33 | C_r0 = blkdiag( 900*eye(2),16*eye(2)); 34 | C_p0 = blkdiag(0.02*eye(1),400*eye(2)); 35 | C_x0 = blkdiag(C_r0,C_p0); 36 | 37 | 38 | 39 | Ar = [eye(2),delta_t*eye(2); zeros(2,2),eye(2)]; 40 | Ap = eye(3); 41 | 42 | C_w_r = blkdiag(100*eye(2),eye(2)); % process noise covariance for kinematic state 43 | C_w_p = blkdiag(0.04,0.5*eye(2)); % process noise covariance for shape variable 44 | 45 | hat_r_EKF = [100,100,5,-8]'; % kinematic state: position and velocity 46 | hat_p_EKF = [-pi/3,200,90]'; 47 | Cr_EKF = C_r0; 48 | Cp_EKF = C_p0; 49 | 50 | % parameters for SOEKF 51 | sgh1 = 1/4; 52 | sgh2 = 1/4; 53 | C_w = blkdiag(C_w_r, C_w_p); 54 | Ax = blkdiag(Ar,Ap); 55 | 56 | hat_x_SOEKF = hat_x0; 57 | Cx_SOEKF = C_x0; 58 | 59 | [ f_g_ekf2, f_jacobian_ekf2, f_hessian_ekf2] = get_jacobian_hessian(motionmodel,C_h); 60 | 61 | 62 | % parameters for Random Matrix 63 | alpha = 50; 64 | tau = 10; 65 | T = 10; 66 | const_z = 1/4; 67 | hat_x_RMM = hat_r0; 68 | hat_X_RMM = get_random_matrix_state(hat_p0); 69 | Cx_RMM = C_r0; 70 | 71 | figure; 72 | 73 | hold on 74 | 75 | for t = 1:time_steps 76 | N = poissrnd(possion_lambda); 77 | while N == 0 78 | N = poissrnd(possion_lambda); 79 | end 80 | disp(['time step:' num2str(t) ', ' num2str(N) ' measurements']); 81 | 82 | %% ------------------get measurements------------------------------------ 83 | gt_cur_par = gt_par(:,t); 84 | gt_velo = H_velo*gt_kin(:,t); 85 | gt_rot = [cos(gt_cur_par(3)), -sin(gt_cur_par(3)); sin(gt_cur_par(3)), cos(gt_cur_par(3))]; 86 | gt_len = gt_cur_par(4:5); 87 | y = zeros(2,N); 88 | for n = 1:N 89 | h_noise(n,:) = -1 + 2.*rand(1,2); 90 | while norm(h_noise(n,:)) > 1 91 | h_noise(n,:) = -1 + 2.*rand(1,2); 92 | end 93 | y(:,n) = H*gt_kin(:,t) + gt_rot*diag(gt_len)*h_noise(n,:)'+ mvnrnd([0 0], C_v, 1)'; 94 | end 95 | %% update RMM 96 | meas_mean = mean(y,2); 97 | meas_spread = (N - 1) * cov(y'); 98 | [hat_x_RMM, hat_X_RMM, C_x_RMM, alpha_update]... 99 | = updateRMM(hat_x_RMM, hat_X_RMM, Cx_RMM, alpha,meas_mean, ... 100 | meas_spread, C_v,N,H,const_z); 101 | 102 | [~, len_RMM,ang_RMM] = get_random_matrix_ellipse(hat_X_RMM); 103 | rmm_par = [H*hat_x_RMM; ang_RMM;len_RMM]; 104 | %% update EKF and SOEKF 105 | for n = 1:N 106 | [hat_x_SOEKF, Cx_SOEKF] = updateSOEKF(hat_x_SOEKF, Cx_SOEKF, y(:,n),... 107 | f_g_ekf2, f_jacobian_ekf2, f_hessian_ekf2, C_v, C_h); 108 | [ hat_r_EKF, Cr_EKF,hat_p_EKF, Cp_EKF ] = updateEKF(hat_r_EKF, Cr_EKF, hat_p_EKF, Cp_EKF, y(:,n), C_v, C_h); 109 | 110 | end 111 | 112 | 113 | %% visulization udpated shapes 114 | if mod(t,3)==1 115 | meas_points=plot( y(1,:)/1000, y(2,:)/1000, '.k','lineWidth',0.5); 116 | hold on 117 | gt_plot = plot_extent([gt_cur_par(1:2)/1000; gt_cur_par(3);gt_cur_par(4:5)/1000], '-','k',1); 118 | axis equal 119 | 120 | est_plot_rmm = plot_extent([rmm_par(1:2)/1000;rmm_par(3);rmm_par(4:5)/1000],'-','g',1); 121 | est_plot_ekf2 = plot_extent([hat_x_SOEKF(1:2)/1000;hat_x_SOEKF(5);hat_x_SOEKF(6:7)/1000],'-', 'r', 1); 122 | est_plot_ekf = plot_extent([H*hat_r_EKF/1000;hat_p_EKF(1);hat_p_EKF(2:3)/1000],'-', 'b', 1); 123 | end 124 | 125 | 126 | %% predict　RMM 127 | [hat_x_RMM, hat_X_RMM,Cx_RMM, alpha] = predictRMM(.... 128 | hat_x_RMM, hat_X_RMM, C_x_RMM, alpha_update,Ar,C_w_r,T,tau); 129 | if alpha_update<=2 130 | error('alpha<2') 131 | end 132 | %% predict SOEKF 133 | [hat_x_SOEKF,Cx_SOEKF] = predictSOEKF(Ax,hat_x_SOEKF,Cx_SOEKF,C_w); 134 | %% predict EKF 135 | [hat_r_EKF,Cr_EKF, hat_p_EKF,Cp_EKF] = predictEKF(Ar,Ap, hat_r_EKF, hat_p_EKF, Cr_EKF, Cp_EKF,C_w_r, C_w_p); 136 | 137 | end 138 | legend([meas_points,gt_plot,est_plot_rmm, est_plot_ekf2,est_plot_ekf],... 139 | {'measurement','ground truth','random matrix','SOEKF','EKF'}) 140 | box on 141 | grid on 142 | ylim([-3200 1200]/1000) 143 | xlim([-200 8000]/1000) 144 | -------------------------------------------------------------------------------- /MEM_EKF/updateEKF.m: -------------------------------------------------------------------------------- 1 | function [ r_hat, C_r,p_hat, C_p ] = updateEKF(r_hat, C_r, p_hat, C_p, y, C_v, C_h) 2 | 3 | [S_hat,M_hat] = getMS(p_hat,C_h); 4 | 5 | 6 | H = [1 0 0 0;0 1 0 0]; 7 | 8 | % moments of kinematic state 9 | E_y = H*r_hat; 10 | C_ry = C_r*H'; 11 | C_yy = H*C_r*H' + S_hat*C_h*S_hat'+C_v; 12 | 13 | % kinematic state update 14 | r_hat = r_hat + C_ry*(C_yy)^(-1)*(y-E_y); 15 | 16 | C_r = C_r - C_ry*(C_yy)^(-1)*C_ry'; 17 | C_r = (C_r+C_r')/2; 18 | 19 | %% pseudo measurement 20 | y_shift = y - E_y; % shift the measuremet to match central moments 21 | % Take the 2nd kronecker product of shifted measurements 22 | % and delete the duplicated element we get the pseudo measurement 23 | Y = [eye(2),zeros(2,2);0 0 0 1]*kron(y_shift,y_shift); 24 | % moments of pseudo-measurement 25 | sgm11 = C_yy(1,1); 26 | sgm12 = C_yy(1,2); 27 | sgm22 = C_yy(2,2); 28 | E_Y = [sgm11; sgm12; sgm22]; 29 | C_YY = [3*sgm11^2, 3*sgm11*sgm12,sgm11*sgm22 + 2*sgm12^2 ;... 30 | 3*sgm11*sgm12, sgm11*sgm22 + 2*sgm12^2, 3*sgm22*sgm12; 31 | sgm11*sgm22 + 2*sgm12^2, 3*sgm22*sgm12,3*sgm22^2]; 32 | C_pY = C_p*M_hat'; 33 | % shape variable update 34 | p_hat = p_hat + C_pY*(C_YY)^(-1)*(Y-E_Y); 35 | C_p = C_p - C_pY*(C_YY)^(-1)*C_pY'; 36 | C_p = (C_p + C_p')/2; 37 | end 38 | 39 | function [S, M] = getMS(p,C_h) 40 | a = p(1); 41 | l1 = p(2); 42 | l2 = p(3); 43 | S = [cos(a) -sin(a); sin(a) cos(a)]*diag([l1 l2]); 44 | M = [-sin(2*a), (cos(a))^2, (sin(a))^2 ;... 45 | cos(2*a), sin(2*a), -sin(2*a);... 46 | sin(2*a), (sin(a))^2, (cos(a))^2]*... 47 | diag([l1^2*C_h(1,1)-l2^2*C_h(2,2),2*l1*C_h(1,1),2*l2*C_h(2,2)]); 48 | end -------------------------------------------------------------------------------- /MEM_EKF/updateRMM.m: -------------------------------------------------------------------------------- 1 | function [x_update, X_update,P_update, alpha_update] = updateRMM(.... 2 | x, X, P, alpha,y,y_cov,R,n_k,H,const_z) 3 | % formulas are from : 4 | % M. Feldmann and D. Fraenken. "Tracking of Extended 5 | % Objects and Group Targets using Random Matrices - A Performance 6 | % Analysis" 7 | 8 | % by Shishan Yang 9 | X_sqrt = sqrtm(X); 10 | Y = const_z * X + R; 11 | Y_sqrt_inv = (sqrtm(Y))^(-1); 12 | 13 | S = H * P * H' + Y/n_k; 14 | S_sqrt = sqrtm(S); 15 | % the bold_dot is: inverse_sqrt_S*(y-H*x) 16 | bold_dot = S_sqrt\(y - H * x); 17 | N_hat = (X_sqrt * bold_dot) * (X_sqrt * bold_dot)'; 18 | 19 | 20 | K = P * H' * S^(-1); 21 | 22 | alpha_update = alpha + n_k; 23 | P_update = P - K*S*K'; 24 | x_update = x + K*(y-H*x); 25 | X_update = (1/alpha_update)*(alpha*X + N_hat + (X_sqrt*Y_sqrt_inv)*y_cov*(X_sqrt*Y_sqrt_inv)'); 26 | 27 | 28 | 29 | end -------------------------------------------------------------------------------- /MEM_EKF/updateSOEKF.m: -------------------------------------------------------------------------------- 1 | function [ hat_x, C_x ] = updateSOEKF(hat_x, C_x, y, ... 2 | f_g, f_jac, f_hes, C_v, C_h) 3 | 4 | % MEASUREMENT_UPDATE: estimate state using second order extended kalman filter (SOEKF) 5 | % Input: 6 | % hat_x: previous estimate, [m1, m2,(velo1, velo2), alpha, l1, l2], 5x1 for 'static', 7x1 for 'NCV' 7 | % C_x: previous covariance matrix, 5x5 for 'static', 7x7 for 'NCV' 8 | % y: measurement, 2x1 9 | % f_g: quadratic function handle, output of GET_JACOBIAN_HESSIAN function 10 | % f_jac: handle of Jacobian of func_g, output of GET_JACOBIAN_HESSIAN function 11 | % f_hes: handle of Hessians of func_g, output of GET_JACOBIAN_HESSIAN function 12 | % C_v: covariance of measurement noise, 2x2 13 | % C_h: covariance of multiplicative noise, 2x2, diag(h1_var, h2_var) 14 | % Output: 15 | % hat_x: updated state 16 | % C_x: updated covariance 17 | 18 | dim_state = numel(hat_x); 19 | nr_param = 5; % 20 | 21 | 22 | %% shift center to improve robustness 23 | shifted = hat_x(1:2); 24 | y = y - shifted; 25 | mem_x_pre_shifted = hat_x; 26 | mem_x_pre_shifted(1:2)=[0;0]; 27 | 28 | %% augment state and its covariance 29 | X_est = [mem_x_pre_shifted;0;0;0;0]; 30 | X_cov = blkdiag(C_x, C_h, C_v); 31 | 32 | %% construct pseudo-measurement 33 | y1 = y(1); 34 | y2 = y(2); 35 | Y = [y1;y2;y1^2;y1*y2;y2^2]; 36 | 37 | %% Substitute quadratic function, Jacobian and Hessian matrices using current estimate 38 | val_subs = [0;0;mem_x_pre_shifted(5:7);0;0;0;0]; % velocity does not appear in measurement equation 39 | 40 | hat_g = f_g(val_subs'); 41 | hat_jacobian = f_jac(val_subs'); 42 | hat_hessian = f_hes(val_subs'); 43 | 44 | %% calculate variance of pseudo-measurement 45 | cov_YY = zeros(nr_param, nr_param); 46 | for i = 1:nr_param 47 | for j = 1:nr_param 48 | cov_YY(i, j) = hat_jacobian(i, :)*X_cov*hat_jacobian(j, :)'+ ... 49 | (1/2)*trace(hat_hessian(:, :, i)*X_cov*hat_hessian(:, :, j)*X_cov); 50 | 51 | end 52 | end 53 | 54 | %% calculate mean of pseudo-measurement 55 | E_Y = zeros(1, nr_param); 56 | for i = 1:nr_param 57 | E_Y(i) = hat_g(i) + (1/2)*trace(hat_hessian(:, :, i)*X_cov); 58 | end 59 | 60 | 61 | %% calculate cross-variance of pseudo-measurement with augmented estimate 62 | cov_Xz = X_cov*hat_jacobian'; 63 | 64 | %% Kalman filter update 65 | X_est = double(X_est + cov_Xz*cov_YY^-1 *( Y - E_Y')); 66 | X_cov = double(X_cov - cov_Xz*cov_YY^-1*cov_Xz'); 67 | 68 | X_est(1:2) = X_est(1:2) + shifted; 69 | 70 | %% Truncate augmented estimate and covariance 71 | X_cov = (X_cov+X_cov')/2; % enforce covariance matrix symmetric 72 | hat_x = X_est(1:dim_state); 73 | C_x = X_cov(1:dim_state, 1:dim_state); 74 | end -------------------------------------------------------------------------------- /MEM_SOEKF/demo.m: -------------------------------------------------------------------------------- 1 | % Matlab demo for the paper 2 | % "Second-Order Extended Kalman Filter for Extended Object and Group Tracking", 3 | % Shishan Yang, Marcus Baum 4 | % https://arxiv.org/abs/1604.00219 5 | 6 | 7 | close all 8 | clear 9 | dbstop error 10 | 11 | % generate ground truth 12 | [gt_center, gt_rotation, gt_orient, gt_length, gt_vel, time_steps, time_interval] =get_ground_truth; 13 | gt = [gt_center;gt_orient;gt_length;gt_vel]; 14 | 15 | motionmodel = {'NCV'}; 16 | process_matrix = [1 0 0 0 0 10 0; 0 1 0 0 0 0 10; 0 0 1 0 0 0 0; ... 17 | 0 0 0 1 0 0 0; 0 0 0 0 1 0 0; 0 0 0 0 0 1 0; 0 0 0 0 0 0 1]; 18 | 19 | % spread of multiplicative error 20 | h1_var = 1/4; 21 | h2_var = 1/4; 22 | multi_noise_cov = diag([h1_var, h2_var]); 23 | possion_lambda = 5; 24 | 25 | meas_noise = 0.02*repmat(diag([100^2, 20^2]), 1, 1, time_steps); 26 | 27 | %% -------------Setting prior--------------------------------------- 28 | initial_guess_center = [100, 100]; 29 | initial_guess_alpha = -pi/3; 30 | initial_guess_len = [200, 90]; 31 | initial_guess_velo = 10; 32 | initial_guess = [initial_guess_center, initial_guess_alpha, initial_guess_len, ... 33 | initial_guess_velo, initial_guess_velo*tan(initial_guess_alpha)]'; 34 | 35 | initial_guess_center_cov = 900*eye(2); 36 | initial_guess_alpha_cov = 0.02*eye(1); 37 | initial_guess_len_cov = 400*eye(2); 38 | initial_guess_velo_cov = 16*eye(2); 39 | 40 | mem_process_noise = blkdiag(100*eye(2), 0.05, 0.00001*eye(2), eye(2)); 41 | 42 | mem_x_pre = initial_guess; 43 | mem_cov_pre = blkdiag(initial_guess_center_cov, initial_guess_alpha_cov, ... 44 | initial_guess_len_cov, initial_guess_velo_cov); 45 | mem_est = struct('state', []); 46 | mem_est(1).state = mem_x_pre; 47 | mem_est(1).cov = mem_cov_pre; 48 | %state_dim = numel(mem_x_pre); 49 | 50 | %% get Jacobians and Hessians 51 | [f_func_g, f_jacobian_mat, f_hessian_mat] = get_jacobian_hessian(motionmodel, h1_var, h2_var); 52 | 53 | figure; 54 | hold on 55 | 56 | for t = 1:time_steps 57 | %% get measurements 58 | meas_per_frame = poissrnd(possion_lambda); 59 | while meas_per_frame == 0 60 | meas_per_frame = poissrnd(possion_lambda); 61 | end 62 | disp(['time step:' num2str(t) ', ' num2str(meas_per_frame) ' measurements']); 63 | 64 | % generate measurements 65 | meas = zeros(2, meas_per_frame); 66 | for n = 1:meas_per_frame 67 | multi_noise(n, :) = -1 + 2.*rand(1, 2); 68 | while norm(multi_noise(n, :)) > 1 69 | multi_noise(n, :) = -1 + 2.*rand(1, 2); 70 | end 71 | 72 | meas(:, n) = gt(1:2, t) + multi_noise(n, 1)*gt(4, t)*... 73 | [cos(gt(3, t)); sin(gt(3, t))] + multi_noise(n, 2)*gt(5, t)*... 74 | [-sin(gt(3, t)); cos(gt(3, t))] + mvnrnd([0 0], meas_noise(:, :, t), 1)'; 75 | end 76 | 77 | %% measurement update 78 | for n = 1:meas_per_frame 79 | 80 | [mem_x, mem_cov] = measurement_update(mem_x_pre, mem_cov_pre, meas(:, n), ... 81 | f_func_g, f_jacobian_mat, f_hessian_mat, meas_noise(:, :, t), multi_noise_cov); 82 | 83 | mem_x_pre = mem_x; 84 | mem_cov_pre = mem_cov; 85 | end 86 | theta_est = mem_x(3); 87 | mem_center = mem_x(1:2); 88 | mem_length = mem_x(4:5); 89 | mem_rot_mat = [cos(theta_est) -sin(theta_est); sin(theta_est) cos(theta_est)]; 90 | 91 | 92 | mem_est(t).state = mem_x; 93 | mem_est(t).cov = mem_cov; 94 | %% time update 95 | mem_cov = process_matrix*mem_cov*process_matrix' + mem_process_noise; 96 | mem_x = process_matrix*mem_x; 97 | mem_x_pre = mem_x; 98 | mem_cov_pre = mem_cov; 99 | 100 | %% visualize estimate and ground truth for every 3rd scan 101 | if mod(t, 3)==1 102 | meas_points=plot( meas(1, :), meas(2, :), '.k', 'lineWidth', 0.5); 103 | hold on 104 | axis equal 105 | gt_plot = plot_extent(gt(1:2, t), gt(4:5, t), '-', 'k', 1, gt_rotation(:, :, t)); 106 | est_plot = plot_extent(mem_center, mem_length, '-', 'g', 1, mem_rot_mat); 107 | pause(0.1) 108 | end 109 | end 110 | xlabel('x'); 111 | ylabel('y'); 112 | legend([gt_plot, est_plot, meas_points], {'Ground truth', 'Estimate', 'Measurement'}); 113 | -------------------------------------------------------------------------------- /MEM_SOEKF/get_ground_truth.m: -------------------------------------------------------------------------------- 1 | function [gt_center, gt_rotation, gt_orient, gt_length, gt_vel, time_steps, time_interval] = get_ground_truth 2 | 3 | %% -------------Setting ground truth--------------------------------------- 4 | gt_center(:, 1) = [0, 0]'; 5 | % trajectory 6 | gt_orient = [ repmat(-pi/4, 1, 20), (-pi/4: pi/40:0), ... 7 | zeros(1, 10), (0: pi/20:2*pi/4), .... 8 | repmat(2*pi/4, 1, 20), (2*pi/4: pi/20:pi), .... 9 | repmat(pi, 1, 20)]; 10 | % assume object is aligned along its velocity 11 | gt_vel = [(500/36)*cos(gt_orient);(500/36)*sin(gt_orient)]; 12 | gt_length = repmat([340/2;80/2], 1, size(gt_vel, 2)); 13 | 14 | time_steps = size(gt_vel, 2); 15 | time_interval = 10; 16 | 17 | 18 | gt_rotation = zeros(2, 2, time_steps); 19 | for t = 1 : time_steps 20 | gt_rotation(:, :, t) = [cos(gt_orient(t)), -sin(gt_orient(t)); sin(gt_orient(t)), cos(gt_orient(t))]; 21 | if t>1 22 | gt_center(:, t) = gt_center(:, t-1) + gt_vel(:, t)*time_interval; 23 | end 24 | end 25 | 26 | 27 | end 28 | 29 | -------------------------------------------------------------------------------- /MEM_SOEKF/get_jacobian_hessian.m: -------------------------------------------------------------------------------- 1 | function [ f_func_g, f_jacobian, f_hessian] = get_jacobian_hessian(motionmodel, h1_var, h2_var) 2 | % GET_JACOBIAN_HESSIAN: calculates the Jacobian and Hessians (using the Matlab symbolic toolbox) 3 | % Input: 4 | % motionmodel: string, either 'static' or 'NCV' 5 | % h1_var: variance of multiplicatice error h1 6 | % h2_var: variance of multiplicatice error h2 7 | % Output: 8 | % f_func_g: quadratic function handle 9 | % f_jacobian_mat: handle of Jacobian matrix of func_g with modified 10 | % substitution 11 | % f_hessian_mat: handle of Hessian matrix of funct_g with modified 12 | % substitution 13 | 14 | syms m1 m2 h1 h2 l1 l2 v1 v2 s1 s2 a 15 | 16 | func_g =[(m1 + h1*l1*cos(a) - h2*l2*sin(a) + v1);... 17 | (m2 + h1*l1*sin(a) + h2*l2*cos(a) + v2);... 18 | (m1 + h1*l1*cos(a) - h2*l2*sin(a) + v1)^2;... 19 | (m2 + h1*l1*sin(a) + h2*l2*cos(a) + v2)^2;... 20 | (m1 + h1*l1*cos(a) - h2*l2*sin(a) + v1)*(m2 + h1*l1*sin(a) + h2*l2*cos(a) + v2)]; 21 | 22 | if strcmp(motionmodel, 'static') 23 | X = [m1;m2; a;l1;l2;h1;h2;v1;v2]; 24 | 25 | elseif strcmp(motionmodel, 'NCV') 26 | X = [m1;m2; a;l1;l2;s1;s2;h1;h2;v1;v2]; 27 | 28 | else 29 | error('unknown dynamic model'); 30 | end 31 | 32 | jacobian_mat = jacobian(func_g, X); 33 | 34 | for i = 1:numel(func_g) 35 | hessian_mat(:, :, i) = hessian(func_g(i), X); 36 | end 37 | jacobian_mat = subs(expand(jacobian_mat), [h1^2, h2^2], [h1_var, h2_var]); 38 | hessian_mat = subs(expand(hessian_mat), [h1^2, h2^2], [h1_var, h2_var]); 39 | 40 | f_func_g = matlabFunction(func_g, 'Vars', {[m1, m2, a, l1, l2, h1, h2, v1, v2]}); 41 | f_jacobian = matlabFunction(jacobian_mat, 'Vars', {[m1, m2, a, l1, l2, h1, h2, v1, v2]}); 42 | f_hessian = matlabFunction(hessian_mat, 'Vars', {[m1, m2, a, l1, l2, h1, h2, v1, v2]}); 43 | 44 | end -------------------------------------------------------------------------------- /MEM_SOEKF/measurement_update.m: -------------------------------------------------------------------------------- 1 | function [ x_est, x_cov ] = measurement_update(mem_x_pre, mem_cov_pre, meas, ... 2 | f_func_g, f_jacobian, f_hessian, meas_noise_cov, multi_noise_cov) 3 | 4 | % MEASUREMENT_UPDATE: estimate state using second order extended kalman filter (SOEKF) 5 | % Input: 6 | % mem_x_pre: previous estimate, [m1, m2, alpha, l1, l2, (velo1, velo2)] 7 | % 5x1 for 'static', 7x1 for 'NCV' 8 | % mem_cov_pre: previous covariance matrix 9 | % 5x5 for 'static', 7x7 for 'NCV' 10 | % meas: measurement, 2x1 11 | % f_func_g: quadratic function handle, output of GET_JACOBIAN_HESSIAN function 12 | % f_jacobian: handle of Jacobian of func_g, output of GET_JACOBIAN_HESSIAN function 13 | % f_hessian: handle of Hessians of func_g, output of GET_JACOBIAN_HESSIAN function 14 | % meas_noise_cov: covariance of measurement noise, 2x2 15 | % multi_noise_cov: covariance of multiplicative noise, 2x2, diag(h1_var, h2_var) 16 | % Output: 17 | % x_est: estimated state 18 | % x_cov: covariance of state estimate 19 | 20 | dim_state = numel(mem_x_pre); 21 | nr_param = 5; % 5 paramtets: m1, m2, alpha, l1, l2 22 | 23 | 24 | 25 | %% shift center to improve robustness 26 | shifted = mem_x_pre(1:2); 27 | meas = meas - shifted; 28 | mem_x_pre_shifted = mem_x_pre; 29 | mem_x_pre_shifted(1:2)=[0;0]; 30 | 31 | %% augment state and its covariance 32 | X_est = [mem_x_pre_shifted;0;0;0;0]; 33 | X_cov = blkdiag(mem_cov_pre, multi_noise_cov, meas_noise_cov); 34 | 35 | %% construct pseudo-measurement 36 | z = [meas(1);meas(2);meas(1)^2;meas(2)^2; meas(1)*meas(2)]; 37 | 38 | %% Substitute quadratic function, Jacobian and Hessian matrices using current estimate 39 | val_subs = [mem_x_pre_shifted(1:nr_param);0;0;0;0]; % velocity does not appear in measurement equation 40 | 41 | subs_func_g = f_func_g(val_subs'); 42 | subs_jacobian = f_jacobian(val_subs'); 43 | subs_hessian = f_hessian(val_subs'); 44 | 45 | %% calculate variance of pseudo-measurement 46 | cov_zz = zeros(nr_param, nr_param); 47 | for i = 1:nr_param 48 | for j = 1:nr_param 49 | cov_zz(i, j) = subs_jacobian(i, :)*X_cov*subs_jacobian(j, :)'+ ... 50 | (1/2)*trace(subs_hessian(:, :, i)*X_cov*subs_hessian(:, :, j)*X_cov); 51 | 52 | end 53 | end 54 | 55 | %% calculate mean of pseudo-measurement 56 | mean_z = zeros(1, nr_param); 57 | for i = 1:nr_param 58 | mean_z(i) = subs_func_g(i) + (1/2)*trace(subs_hessian(:, :, i)*X_cov); 59 | end 60 | 61 | %% calculate cross-variance of pseudo-measurement with augmented estimate 62 | cov_Xz = X_cov*subs_jacobian'; 63 | 64 | %% Kalman filter update 65 | X_est = double(X_est + cov_Xz*cov_zz^-1 *( z - mean_z')); 66 | X_cov = double(X_cov - cov_Xz*cov_zz^-1*cov_Xz'); 67 | 68 | X_est(1:2) = X_est(1:2) + shifted; 69 | 70 | %% Truncate augmented estimate and covariance 71 | X_cov = (X_cov+X_cov')/2; % enforce covariance matrix symmetric 72 | x_est = X_est(1:dim_state); 73 | x_cov = X_cov(1:dim_state, 1:dim_state); 74 | end -------------------------------------------------------------------------------- /MEM_SOEKF/plot_extent.m: -------------------------------------------------------------------------------- 1 | function [handle_extent] = plot_extent(c, l, line_style, color, line_width, rotation) 2 | % PLOT_EXTENT: to plot the elliptical extent 3 | % Input: 4 | % c: object center 2x1 5 | % l: semi-axis length 2x1 6 | % line_style: defined the same as in Matlab plot function 7 | % color: defined the same as in Matlab plot function 8 | % line_width: defined the same as in Matlab plot function 9 | % roation: ration matrix, 2x2 10 | % Output: 11 | % handle_extent: the handle of the object extent 12 | 13 | alpha = 0:pi/100:2*pi; 14 | 15 | points = diag(l)*[cos(alpha); sin(alpha)]; 16 | 17 | rotated_points = rotation* points; 18 | shifted_points = rotated_points + repmat(c, 1, size(points, 2)); 19 | 20 | handle_extent = plot(shifted_points(1, :),shifted_points(2,:),'LineStyle',... 21 | line_style,'color',color,'LineWidth',line_width); 22 | end -------------------------------------------------------------------------------- /MEM_SOEKF/readme.md: -------------------------------------------------------------------------------- 1 | # Second-Order Extended Kalman Filter for Extended Object and Group Tracking # 2 | ###Shishan Yang and Marcus Baum### 3 | 4 | 5 | 6 | ### About ### 7 | 8 | This is a framework for Multiplicative Error Model for Elliptical Object Tracking. 9 | 10 | * Ellipse Parameterization: center, orientation, and lengths of semi-axis 11 | * Measurement Update: Second Order Extended Kalman Filter (center is shifted for robustness) 12 | 13 | For more detatils, we refer you to our paper. 14 | 15 | ### Running ### 16 | start matlab and run 17 | 18 | ``` 19 | demo.m 20 | ``` 21 | ### Who do I talk to? ### 22 | 23 | Shishan Yang (shishan.yang@cs.uni-goettingen.de) 24 | 25 | Marcus Baum (marcus.baum@cs.uni-goettingen.de) 26 | 27 | 28 | If you use our code in your publication, please cite our paper 29 | 30 | ``` 31 | 32 | @InProceedings{Yang2016, 33 | Title = {{{Second-Order} Extended Kalman Filter for Extended Object and Group Tracking}}, 34 | Author = {Shishan Yang and Marcus Baum}, 35 | Booktitle = {19th International Conference on Information Fusion (Fusion 2016)}, 36 | Year = {2016}, 37 | 38 | Address = {Heidelberg, Germany}, 39 | Month = jul, 40 | Days = {4} 41 | } 42 | 43 | ``` 44 | -------------------------------------------------------------------------------- /MEOT/linearJPDA/MEOT_JPDA.m: -------------------------------------------------------------------------------- 1 | % Implementation of the multiple extended object tracking algorithm based on the article 2 | % 3 | % "Linear-Time Joint Probabilistic Data Association for Multiple Extended Object Tracking (to appear)" 4 | % S. Yang, K. Thormann, and M. Baum 5 | % 2018 IEEE Sensor Array and Multichannel Signal Processing Workshop (SAM 2018), Sheffield, United Kingdom, 2018. 6 | % 7 | % 8 | % Further information: 9 | % http://www.fusion.informatik.uni-goettingen.de 10 | % https://github.com/Fusion-Goettingen 11 | % 12 | % Source code written by Shishan Yang 13 | % ============================= 14 | function [r,p,Cr,Cp] = MEOT_JPDA(meas,r,p,Cr,Cp,cp,H,Cv,mlambda,clambda) 15 | 16 | M = size(meas,2); 17 | N = size(r,1); 18 | G = zeros(N,M); 19 | 20 | 21 | % calculating Eq.(4) 22 | for n = 1:N 23 | % for further information, we refer you to 24 | % "Tracking the Orientation and Axes Lengths of an Elliptical Extended Object" 25 | % Shishan Yang and Marcus Baum 26 | % arXiv preprint, 2018 27 | % https://arxiv.org/abs/1805.03276 28 | Cy(:,:,n) = get_pred_meas_cov(p(n,:),Cr(:,:,n),Cp(:,:,n),H,Cv); 29 | 30 | for m = 1:M 31 | if (meas(:,m)'-r(n,1:2))*Cy(:,:,n)^-1*(meas(:,m)-r(n,1:2)') < 16 32 | G(n,m) = mvnpdf(meas(:,m),r(n,1:2)',Cy(:,:,n)); 33 | end 34 | end 35 | end 36 | 37 | % get the mariginal association probability 38 | beta = mariginalAssocationProb(G,cp,mlambda,clambda); 39 | 40 | for n = 1:N 41 | beta_n = beta(n,:); 42 | matched_ind = beta_n>0; 43 | beta_n = beta_n(matched_ind); 44 | obs_n = meas(:,matched_ind); 45 | 46 | [r(n,:),p(n,:),Cr(:,:,n),Cp(:,:,n)] = prob_measurement_update(H,r(n,:)',p(n,:)',Cr(:,:,n),Cp(:,:,n),Cv,obs_n,beta_n); 47 | 48 | end 49 | end 50 | 51 | 52 | function beta = mariginalAssocationProb(G,cp,mlambda,clambda) 53 | % get the mariginal assocation probability accroding Eq (15) 54 | [N,M]=size(G); 55 | beta = zeros(N+1,M); 56 | betaTemp = zeros(N+1,M); 57 | 58 | for j = 1:M 59 | S(j) = clambda*cp; 60 | for n = 1:N 61 | S(j) = S(j) + mlambda(n)*G(n,j); 62 | end 63 | end 64 | for j = 1:M 65 | for i = 1:N 66 | 67 | betaTemp(i,j) = mlambda(i)*G(i,j)/S(j); 68 | end 69 | betaTemp(N+1,j) = clambda*cp/S(j); 70 | end 71 | 72 | % normalization 73 | for j = 1:M 74 | beta(:,j) = betaTemp(:,j)./sum(betaTemp(:,j)); 75 | end 76 | end 77 | 78 | function P = get_pred_meas_cov(p,Cr,Cp,H,R) 79 | % get predicted measurement covariance 80 | % 81 | % for further information, we refer you to 82 | % "Tracking the Orientation and Axes Lengths of an Elliptical Extended Object" 83 | % Shishan Yang and Marcus Baum 84 | % arXiv preprint, 2018 85 | % https://arxiv.org/abs/1805.03276 86 | 87 | Ch = diag([.25 .25]); 88 | alpha = p(1); 89 | l1 = p(2); 90 | l2 = p(3); 91 | 92 | S =[cos(alpha) -sin(alpha); sin(alpha) cos(alpha)]*diag([l1 l2]); 93 | J(:,:,1) = [-l1*sin(alpha) cos(alpha) 0; -l2*cos(alpha) 0 -sin(alpha)]; 94 | J(:,:,2) = [l1*cos(alpha) sin(alpha) 0; -l2*sin(alpha) 0 cos(alpha)]; 95 | 96 | 97 | for m = 1:2 98 | for n = 1:2 99 | CII(m,n) = trace(Cp*J(:,:,n)'*Ch*J(:,:,m)); 100 | end 101 | end 102 | CI = S*Ch*S'; 103 | P = H*Cr*H' + CI + CII + R; 104 | 105 | end -------------------------------------------------------------------------------- /MEOT/linearJPDA/demo.m: -------------------------------------------------------------------------------- 1 | % Implementation of the multiple extended object tracking algorithm based on the article 2 | % 3 | % "Linear-Time Joint Probabilistic Data Association for Multiple Extended Object Tracking (to appear)" 4 | % S. Yang, K. Thormann, and M. Baum 5 | % 2018 IEEE Sensor Array and Multichannel Signal Processing Workshop (SAM 2018), Sheffield, United Kingdom, 2018. 6 | % 7 | % 8 | % 9 | % Further information: 10 | % http://www.fusion.informatik.uni-goettingen.de 11 | % https://github.com/Fusion-Goettingen 12 | % 13 | % Source code written by Shishan Yang 14 | % ============================= 15 | 16 | clc 17 | close all 18 | clear 19 | dbstop error 20 | 21 | 22 | scenario = 1; % turn and closely-spaced 23 | % scenario = 2; % cross 24 | clambda = 40; % clutter rate 25 | 26 | 27 | % motion and measurement parameters used for multiplicative noise model 28 | Cv = diag([10 10]); 29 | Crw = diag([10 10 10 10]); 30 | Cpw(:,:,1) = diag([0.02 1 1]); 31 | Cpw(:,:,2) = diag([0.02 1 1]); 32 | Ar = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1]; 33 | Ap = eye(3); 34 | H = [eye(2);zeros(2,2)]'; 35 | 36 | 37 | 38 | figure 39 | hold on 40 | box on 41 | axis equal 42 | 43 | [gt, meas,mlambda, xbound, ybound,cp,nr_timesteps] = getMeasGt(scenario,clambda,Cv); 44 | 45 | 46 | % first guess 47 | r = [-20 -250 10 10;-20 250 10 -10]; 48 | p = [0 30 30;0 15 15 ]; 49 | 50 | N = size(r,1); 51 | 52 | Cr(:,:,1) = diag([900 900 10 10]); 53 | Cp(:,:,1) = diag([.2 400 400]); 54 | 55 | Cr(:,:,2) = diag([900 900 10 10]); 56 | Cp(:,:,2) = diag([.02 100 100]); 57 | 58 | 59 | %% plot first guess 60 | for n = 1:N 61 | plot_extent([r(n,1:2) p(n,:)],'--','r',1); 62 | end 63 | 64 | 65 | for t = 1:nr_timesteps 66 | 67 | [r,p,Cr,Cp] = MEOT_JPDA(meas{t},r,p,Cr,Cp,cp,H,Cv,mlambda,clambda); 68 | 69 | %% Visulize 70 | if mod(t,3)==1 71 | 72 | pMeas = plot(meas{t}(1,:),meas{t}(2,:),'k.'); 73 | for n = 1:N 74 | plotGT = plot_extent(gt(n,1:5,t),'-','k',1); 75 | plotEst = plot_extent([r(n,1:2),p(n,:)],'-','g',1); 76 | end 77 | pause(0.001) 78 | end 79 | 80 | 81 | %% prediction 82 | for n = 1:N 83 | r(n,:) = Ar*r(n,:)'; 84 | p(n,:) = Ap*p(n,:)'; 85 | Cr(:,:,n) = Ar*Cr(:,:,n)*Ar'+Crw; 86 | Cp(:,:,n) = Ap*Cp(:,:,n)*Ap'+Cpw(:,:,n); 87 | end 88 | end 89 | legend([plotGT plotEst],{'Ground Truth','Estimates'}) 90 | -------------------------------------------------------------------------------- /MEOT/linearJPDA/getMeasGt.m: -------------------------------------------------------------------------------- 1 | % Implementation of the multiple extended object tracking algorithm based on the article 2 | % 3 | % "Linear-Time Joint Probabilistic Data Association for Multiple Extended Object Tracking (to appear)" 4 | % S. Yang, K. Thormann, and M. Baum 5 | % 2018 IEEE Sensor Array and Multichannel Signal Processing Workshop (SAM 2018), Sheffield, United Kingdom, 2018. 6 | % 7 | % 8 | % 9 | % Further information: 10 | % http://www.fusion.informatik.uni-goettingen.de 11 | % https://github.com/Fusion-Goettingen 12 | % 13 | % Source code written by Shishan Yang 14 | % ============================= 15 | function [gt, meas,mlambda, xbound, ybound,cp,nr_timesteps] = getMeasGt(scenario,clambda,R) 16 | rot = @ (a) [cos(a) -sin(a); sin(a) cos(a)]; 17 | gt(:,:,1) = [0 -300 -pi/3 40 15; 0 300 pi/4 20 10]; 18 | mlambda = [9 7]; 19 | if scenario == 1 20 | 21 | velo_p1t1 = repmat([11;7.7],1,30); 22 | velo_p1t2 = repmat([11;-7.7],1,30); 23 | 24 | dy1 = velo_p1t1(end):-.8:0; 25 | dy2 = velo_p1t2(end):.8:0; 26 | 27 | da1 = gt(1,3,1):(-pi/2-gt(1,3,1))/(numel(dy1)-1):-pi/2; 28 | da2 = gt(2,3,1):((pi/2)-gt(2,3,1))/(numel(dy2)-1):pi/2; 29 | 30 | velo_p2t1 = [repmat(velo_p1t1(1),1,numel(dy1)); dy1]; 31 | velo_p2t2 = [repmat(velo_p1t2(1),1,numel(dy1)); dy2]; 32 | 33 | velo_p3t1 = repmat([11;0],1,40); 34 | velo_p3t2 = repmat([11;0],1,40); 35 | 36 | 37 | velo(:,:,1) = [velo_p1t1 velo_p2t1 velo_p3t1]; 38 | velo(:,:,2) = [velo_p1t2 velo_p2t2 velo_p3t2]; 39 | 40 | alpha(1,:) = [repmat(gt(1,3,1),1,size(velo_p1t1,2)) da1 repmat(da1(end),1,size(velo_p3t1,2))]; 41 | alpha(2,:) = [repmat(gt(2,3,1),1,size(velo_p1t2,2)) da2 repmat(da2(end),1,size(velo_p3t2,2))]; 42 | 43 | gt(1,6:7,1)=velo(:,1,1); 44 | gt(2,6:7,1)=velo(:,1,2); 45 | elseif scenario == 2 46 | 47 | 48 | 49 | velo(:,:,1) = repmat([10;10],1,60); 50 | 51 | velo(:,:,2) = repmat([10;-10],1,60); 52 | alpha(1,:) = repmat(gt(1,3,1),1,size(velo,2)); 53 | alpha(2,:) = repmat(gt(2,3,1),1,size(velo,2)); 54 | gt(1,6:7,1)=velo(:,1,1); 55 | gt(2,6:7,1)=velo(:,1,2); 56 | end 57 | 58 | 59 | temp=[]; 60 | for i = 1:size(gt,1) 61 | nr_meas = poissrnd(mlambda(i)); 62 | for j = 1:nr_meas 63 | h(:,j) = mvnrnd([0;0],diag([.25 .25])); 64 | temp = [temp gt(i,1:2,1)' + rot(gt(i,3,1))*diag(gt(i,4:5,1))*h(:,j)+mvnrnd([0;0],R)']; 65 | end 66 | if ~isempty(temp) 67 | 68 | 69 | xmax(1) = max(temp(1,:)); 70 | xmin(1) = min(temp(1,:)); 71 | ymax(1) = max(temp(2,:)); 72 | ymin(1) = min(temp(2,:)); 73 | end 74 | end 75 | meas{1}=temp; 76 | 77 | 78 | nr_timesteps = size(velo,2); 79 | 80 | for t = 2:nr_timesteps 81 | temp = []; 82 | 83 | 84 | for i = 1:size(gt,1) 85 | if i~=1 || t< nr_timesteps%-20 % target 1 is terminated 86 | gt(i,1:2,t) = gt(i,1:2,t-1)+velo(:,t,i)'; 87 | gt(i,3,t)=alpha(i,t); 88 | gt(i,4:5,t) = gt(i,4:5,t-1); 89 | gt(i,6:7,t) = velo(:,t,i)'; 90 | nr_meas = poissrnd(mlambda(i)); 91 | for j = 1:nr_meas 92 | h(:,j) = mvnrnd([0;0],diag([.25 .25])); 93 | temp = [temp gt(i,1:2,t)' + rot(gt(i,3,t))*diag(gt(i,4:5,t))*h(:,j)+mvnrnd([0;0],R)']; 94 | end 95 | 96 | end 97 | 98 | if ~isempty(temp) 99 | xmax(t) = max(temp(1,:)); 100 | xmin(t) = min(temp(1,:)); 101 | ymax(t) = max(temp(2,:)); 102 | ymin(t) = min(temp(2,:)); 103 | end 104 | end 105 | meas{t}=temp; 106 | 107 | end 108 | xmax = round(max(xmax)); 109 | xmin = floor(min(xmin)); 110 | xbound = [xmin xmax]; 111 | 112 | ymax = round(max(ymax)); 113 | ymin = floor(min(ymin)); 114 | ybound = [ymin ymax]; 115 | xlim(xbound) 116 | ylim(ybound) 117 | 118 | for t= 1:nr_timesteps 119 | nr_clutter = poissrnd(clambda); 120 | clutter{t}(1,:) = (rand(nr_clutter,1)'* (xmax-xmin)) + xmin; 121 | clutter{t}(2,:) = (rand(nr_clutter,1)' * (ymax-ymin)) + ymin; 122 | 123 | cp = 1 / ((xmax-xmin)*(ymax-ymin)); 124 | meas{t} = [meas{t} clutter{t}]; 125 | end 126 | 127 | end -------------------------------------------------------------------------------- /MEOT/linearJPDA/plot_extent.m: -------------------------------------------------------------------------------- 1 | % Implementation of the multiple extended object tracking algorithm based on the article 2 | % 3 | % "Linear-Time Joint Probabilistic Data Association for Multiple Extended Object Tracking (to appear)" 4 | % S. Yang, K. Thormann, and M. Baum 5 | % 2018 IEEE Sensor Array and Multichannel Signal Processing Workshop (SAM 2018), Sheffield, United Kingdom, 2018. 6 | % 7 | % 8 | % 9 | % Further information: 10 | % http://www.fusion.informatik.uni-goettingen.de 11 | % https://github.com/Fusion-Goettingen 12 | % 13 | % Source code written by Shishan Yang 14 | % ============================= 15 | function [handle_extent] = plot_extent(ellipse,line_style, color, line_width) 16 | % PLOT_EXTENT plots the extent of an ellipse or circle 17 | % Input: 18 | % ellipse1, 1x5, parameterization of one ellispe [m1 m2 alpha l1 l2] 19 | % line_style, definedthe same as in Matlab plot function 20 | % color, definedthe same as in Matlab plot function 21 | % line_width, definedthe same as in Matlab plot function 22 | % 23 | % Output: 24 | % handle_extent, the handle of the plot 25 | % 26 | % Written by Shishan Yang 27 | 28 | 29 | 30 | center = ellipse(1:2); 31 | theta = ellipse(3); 32 | l = ellipse(4:5); 33 | R = [cos(theta) -sin(theta); sin(theta) cos(theta)]; %rotation matrix 34 | 35 | 36 | alpha = 0:pi/30:2*pi; 37 | xunit = l(1)*cos(alpha); 38 | yunit = l(2)*sin(alpha); 39 | 40 | rotated = R* [xunit; yunit]; 41 | xpoints = rotated(1,:) + center(1); 42 | ypoints = rotated(2,:) + center(2); 43 | 44 | 45 | handle_extent = plot(xpoints,ypoints,'LineStyle',line_style,'color',color,'LineWidth',line_width); 46 | 47 | end -------------------------------------------------------------------------------- /MEOT/linearJPDA/prob_measurement_update.m: -------------------------------------------------------------------------------- 1 | % Implementation of the multiple extended object tracking algorithm based on the article 2 | % 3 | % "Linear-Time Joint Probabilistic Data Association for Multiple Extended Object Tracking (to appear)" 4 | % S. Yang, K. Thormann, and M. Baum 5 | % 2018 IEEE Sensor Array and Multichannel Signal Processing Workshop (SAM 2018), Sheffield, United Kingdom, 2018. 6 | % 7 | % The extended object kinematic and shape state update formulas see 8 | % "Tracking the Orientation and Axes Lengths of an Elliptical Extended Object" 9 | % Shishan Yang and Marcus Baum 10 | % arXiv preprint, 2018 11 | % https://arxiv.org/abs/1805.03276 12 | % 13 | % 14 | % Further information: 15 | % http://www.fusion.informatik.uni-goettingen.de 16 | % https://github.com/Fusion-Goettingen 17 | % 18 | % Source code written by Shishan Yang 19 | % ============================= 20 | function [r,p,Cr,Cp] = prob_measurement_update(H,r,p,Cr,Cp,Cv,y,beta_j) 21 | 22 | nk = size(y,2); % number of measurements at time k 23 | Ch = diag([.25 .25]); 24 | 25 | 26 | 27 | 28 | for i = 1:nk 29 | 30 | [CI,CII,M,F,Ftilde] = get_auxiliary_variables(p,Cp,Ch); 31 | 32 | yi = y(:,i); 33 | py = beta_j(i); 34 | 35 | 36 | % calculate moments for the kinematic state update 37 | y_bar = H*r; 38 | Cry = Cr*H'; 39 | Cy = H*Cr*H' + CI + CII + Cv; 40 | 41 | % udpate kinematic estimate 42 | Kr = Cry*Cy^-1; 43 | 44 | r = r + Kr*(py* (yi-y_bar)); 45 | Cr = Cr-py*(Kr*H*Cr) + py^3*(1-py)*Kr*( (yi-y_bar)* (yi-y_bar)')*Kr'; 46 | % Enforce symmetry of the covariance 47 | Cr = (Cr+Cr')/2; 48 | 49 | 50 | % construct pseudo-measurement for the shape update 51 | Yi = F*kron( (yi-y_bar), (yi-y_bar)); 52 | 53 | Yibar = F * reshape(Cy,4,1); 54 | CpY = Cp*M'; 55 | CYY = F*kron(Cy,Cy)*(F+Ftilde)'; 56 | 57 | % update shape 58 | Kp= CpY*CYY^(-1); 59 | 60 | p = p + Kp*py*(Yi-Yibar); 61 | Cp = Cp-py*(CpY*CYY^(-1)*CpY') + py^3*(1-py)*Kp*((Yi-Yibar)*(Yi-Yibar)')*Kp'; 62 | % Enforce symmetry of the covariance 63 | Cp = (Cp+Cp')/2; 64 | 65 | end 66 | 67 | end 68 | 69 | function [CI,CII,M,F,Ftilde] = get_auxiliary_variables(p,Cp,Ch) 70 | alpha = p(1); 71 | l1 = p(2); 72 | l2 = p(3); 73 | 74 | S = [cos(alpha) -sin(alpha); sin(alpha) cos(alpha)]*diag([l1 l2]); 75 | S1 = S(1,:); 76 | S2 = S(2,:); 77 | 78 | J1 = [-l1*sin(alpha) cos(alpha) 0; -l2*cos(alpha) 0 -sin(alpha)]; 79 | J2 = [ l1*cos(alpha) sin(alpha) 0; -l2*sin(alpha) 0 cos(alpha)]; 80 | 81 | CI = S*Ch*S'; 82 | CII(1,1) = trace(Cp*J1'*Ch*J1); 83 | CII(1,2) = trace(Cp*J2'*Ch*J1); 84 | CII(2,1) = trace(Cp*J1'*Ch*J2); 85 | CII(2,2) = trace(Cp*J2'*Ch*J2); 86 | 87 | M = [2*S1*Ch*J1; 2*S2*Ch*J2; S1*Ch*J2 + S2*Ch*J1]; 88 | 89 | F = [1 0 0 0; 0 0 0 1; 0 1 0 0]; 90 | Ftilde = [1 0 0 0; 0 0 0 1; 0 0 1 0]; 91 | end -------------------------------------------------------------------------------- /RandomHypersurfaceModel/randomHypersurfaceModel_2011.m: -------------------------------------------------------------------------------- 1 | % Implementation of Star-Convex RHMs using Fourier Coefficients and the UKF according to the paper 2 | % 3 | % M. Baum and U. D. Hanebeck, "Shape tracking of extended objects and group targets with star-convex RHMs," 4 | % 14th International Conference on Information Fusion, Chicago, IL, 2011, pp. 1-8. 5 | % URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=5977661&isnumber=5977431 6 | % 7 | % Source code written by Robin Sandkuehler & Marcus Baum 8 | 9 | function randomHypersurfaceModel_2011(numberOfMeasurement) 10 | if nargin ==0 11 | numberOfMeasurement= 100; 12 | end 13 | % Number of Fourier coefficients 14 | nr_Fourier_coeff = 11; 15 | 16 | % State describtion prior [b0--bn, x, y] 17 | x = zeros(nr_Fourier_coeff + 2, 1); 18 | x(1) = 1.5; 19 | 20 | % State covariance prior 21 | C_x = diag([ones(1, nr_Fourier_coeff).*0.02, 0.3, 0.3]); 22 | 23 | % Measurement noise 24 | measurementNoise = diag([0.2, 0.2].^2); 25 | 26 | % Scale properties 27 | scale.mean = 0.7; 28 | scale.variance = 0.08; 29 | 30 | % Angular resolution for plotting 31 | phi_vec = [0:0.01:2*pi]; 32 | 33 | % Object size 34 | a = 3; % -- width of the horizontal rectangle 35 | b = 0.5; % | height of the horizontal rectangle 36 | c = 2; % | height of the vertical rectangle 37 | d = 0.5; % -- width of the vertical rectangle 38 | 39 | sizeObject = [a b c d]; 40 | 41 | % Object shape bounds 42 | objectBounds = [[-d, -c];[d, -c];[d, -b];[a, -b];[a, b];[d, b];[d, c]; 43 | [-d, c];[-d, b];[-a, b];[-a, -b];[-d, -b]]' ./ 2; 44 | 45 | 46 | % Main 47 | 48 | % Plot 49 | h_object = fill(objectBounds(1, :), objectBounds(2, :), [.7 .7 .7]); 50 | hold on 51 | xlim([-3 3]); 52 | ylim([-3 3]); 53 | axis equal 54 | xlabel('x-Axis') 55 | ylabel('y-Axis') 56 | title('Random Hypersurface Model Simulation') 57 | 58 | 59 | 60 | for j = 1 : numberOfMeasurement 61 | 62 | % Get new measurement 63 | newMeasurement = getNewMeasurement(sizeObject, measurementNoise); 64 | 65 | % Filter step 66 | [x, C_x] = UKF_FilterStep(x, C_x, newMeasurement, [scale.mean; [0 0]'], ... 67 | blkdiag(scale.variance, measurementNoise), @f_meas_pseudo_squared, nr_Fourier_coeff); 68 | 69 | % Plot 70 | shape = calcShape(phi_vec, x, nr_Fourier_coeff); 71 | 72 | h_measure = plot(newMeasurement(1), newMeasurement(2), '+'); 73 | h_shape = plot(shape(1, :), shape(2, :), 'g-', 'linewidth', 2); 74 | legend([h_object, h_measure, h_shape],'Target', 'Measurement', 'Estimated shape') 75 | drawnow; 76 | 77 | if j ~= numberOfMeasurement 78 | delete(h_shape) 79 | end 80 | 81 | end 82 | 83 | hold off 84 | end 85 | 86 | 87 | 88 | function measurement = getNewMeasurement( sizeObject, measurementNoise ) 89 | 90 | a = sizeObject(1); % -- width of the horizontal rectangle 91 | b = sizeObject(2); % | height of the horizontal rectangle 92 | c = sizeObject(3); % | height of the vertical rectangle 93 | d = sizeObject(4); % -- width of the vertical rectangle 94 | 95 | measurementsourceNotValid = 1; 96 | 97 | while measurementsourceNotValid 98 | 99 | %Measurementsoure 100 | x = -a/2 + a.*rand(1, 1); 101 | y = -c/2 + c.*rand(1, 1); 102 | 103 | if y > b/2 && x < -d/2 || y > b/2 && x > d/2 ||... 104 | y < -b/2 && x < -d/2 || y < -b/2 && x > d/2 105 | 106 | x = -a/2 + a.*rand(1, 1); 107 | y = -c/2 + c.*rand(1, 1); 108 | 109 | else 110 | measurementsourceNotValid = 0; 111 | end 112 | end 113 | 114 | % Add zero-mean Gaussian noise to the measurement sources 115 | measurement = [x; y] + (randn(1, 2) * chol(measurementNoise))'; 116 | end 117 | 118 | function pseudoMeasurement = f_meas_pseudo_squared(x, noise, y, nr_Fourier_coeff) 119 | 120 | numberOfSigmaPoints = size(x, 2); 121 | pseudoMeasurement = zeros(1, numberOfSigmaPoints); 122 | 123 | for j = 1 : numberOfSigmaPoints 124 | 125 | s = noise(1, j); 126 | v = noise(2:3, j); 127 | b = x(1:nr_Fourier_coeff, j); 128 | m = x(nr_Fourier_coeff + 1:nr_Fourier_coeff + 2, j); 129 | 130 | theta = atan2(y(2) - m(2), y(1) - m(1))+2*pi; 131 | 132 | R = calcFourierCoeff(theta, nr_Fourier_coeff); 133 | 134 | e = [cos(theta); sin(theta)]; 135 | 136 | pseudoMeasurement(j) = (norm( m - y ))^2 - (s^2 *(R * b).^2 + 2 * s * R * b * e' * v + norm(v)^2); 137 | end 138 | end 139 | 140 | 141 | 142 | 143 | function fourie_coff = calcFourierCoeff(theta, nr_Fourier_coeff) 144 | 145 | fourie_coff(1) = 0.5; 146 | 147 | index = 1; 148 | for i = 2 : 2 : (nr_Fourier_coeff - 1) 149 | fourie_coff(i : i + 1) = [cos(index * theta) sin(index * theta)]; 150 | index = index + 1; 151 | end 152 | 153 | 154 | end 155 | 156 | 157 | function shape = calcShape(phi_vec, x, nr_Fourier_coeff) 158 | 159 | shape = zeros(2, length(phi_vec)); 160 | 161 | for i = 1 : length(phi_vec) 162 | phi = phi_vec(i); 163 | R = calcFourierCoeff(phi, nr_Fourier_coeff); 164 | e = [cos(phi) sin(phi)]'; 165 | shape(:, i) = R * x(1:end - 2) * e + x(end - 1:end); 166 | end 167 | 168 | end 169 | 170 | function [x_e, C_e] = UKF_FilterStep(x, C, measurement, measurementNoiseMean, ... 171 | measurementNoiseCovariance, measurementFunctionHandle, numberOfFourierCoef) 172 | 173 | alpha = 1; 174 | beta = 0; 175 | kappa = 0; 176 | 177 | % Calculate Sigma Points 178 | 179 | %Stack state and noise mean 180 | x_ukf = [x; measurementNoiseMean]; 181 | 182 | 183 | %Stack state and noise Covariance 184 | C_ukf = blkdiag(C, measurementNoiseCovariance); 185 | 186 | n = size(x_ukf, 1); 187 | n_state = size(x, 1); 188 | 189 | lamda = alpha^2 * (n + kappa) - n; 190 | 191 | 192 | % Calculate Weights Mean 193 | WM(1) = lamda / (n + lamda); 194 | WM(2 : 2 * n + 1) = 1 / (2 * (n + lamda)); 195 | % Calculate Weights Covariance 196 | WC(1) = (lamda / (n + lamda)) + (1 - alpha^2 + beta); 197 | WC(2 : 2 * n + 1) = 1 / (2 * (n + lamda)); 198 | 199 | %Calculate Sigma Points 200 | A = sqrt(n + lamda) * chol(C_ukf)'; 201 | 202 | xSigma = [zeros(size(x_ukf)) -A A]; 203 | xSigma = xSigma + repmat(x_ukf, 1, size(xSigma, 2)); 204 | 205 | % Filterstep 206 | z = 0; 207 | C_yy = 0; 208 | C_xy = 0; 209 | 210 | zSigmaPredict = feval(measurementFunctionHandle, xSigma(1:n_state,:), xSigma(n_state + 1:n, :), measurement, numberOfFourierCoef ); 211 | 212 | for i = 1 : size(zSigmaPredict, 2); 213 | z = z + ( zSigmaPredict(:,i) * WM(i) ); 214 | end 215 | 216 | 217 | for i = 1 : size(zSigmaPredict, 2) 218 | C_yy = C_yy + WC(i) * ( (zSigmaPredict(:,i) - z ) * ( zSigmaPredict(:,i) - z )') ; 219 | C_xy = C_xy + WC(i) * ( (xSigma(1:size(x, 1),i) - x ) * ( zSigmaPredict(:,i) - z )'); 220 | end 221 | 222 | K = C_xy / C_yy; 223 | x_e = x + K * (zeros(size(z)) - z); 224 | C_e = C - K * (C_yy) * K'; 225 | 226 | end --------------------------------------------------------------------------------