def NN_LG(p_prior, Z, PD, lamb_c, F, Q, H, R): """ Nearest neighbour algoritm for linear and gaussian models with constant probability of detection and uniform clutter. """ # Predict x_k_kmin1, P_k_kmin1 = kalman_prediction(p_prior.x, p_prior.P, F, Q) # Compute w_tilde for all data associations theta mk = len(Z) w_tilde = np.zeros(mk+1) # - Precompute predicted likelihood z_bar, S = kalman_prediction(x_k_kmin1, P_k_kmin1, H, R) for theta in range(mk + 1): if theta == 0: w_tilde[theta] = 1 - PD else: z = Z[theta - 1] w_tilde[theta] = (PD / lamb_c) * multivariate_normal.pdf(z, mean=z_bar, cov=S) # Find most probable data association hypotesis theta_star = np.argmax(w_tilde) # Compute posterior based on theta_star x_k_k, P_k_k = x_k_kmin1, P_k_kmin1 # Assume theta_star is 0 if theta_star > 0: z = Z[theta_star - 1] x_k_k, P_k_k = kalman_update(x_k_kmin1, P_k_kmin1, H, R, z) return Gaussian(x_k_k, P_k_k), Gaussian(x_k_kmin1, P_k_kmin1), theta_star
def GNN_LG(p_prior, Z, n, PD, lamb_c, F, Q, H, R): """ Global nearest neighbour algorithm for linear and gaussian models with constant probability of detection and uniform clutter. """ # Dimensions nk = p_prior.components[0].x.shape[0] mk = len(Z) # Allocate p_pred = [] x_k_k = np.zeros((nk,n)) P_k_k = np.zeros((nk,nk,n)) # Predict for p_i in p_prior.components: # for each objects prior x_k_kmin1, P_k_kmin1 = kalman_prediction(p_i.x, p_i.P, F, Q) p_pred.append( Gaussian(x_k_kmin1, P_k_kmin1, p_i.w) ) p_pred = GaussianMixture(p_pred) # Create cost matrix # - Allocate L_a = np.zeros((n,mk)) L_ua = np.ones((n,n)) * np.inf L = np.hstack((L_a,L_ua)) # - Compute log weights for i, p_i in enumerate(p_pred.components): # - Predicted likelihood z_h_i = H @ p_i.x S_h_i = H @ p_i.P @ H.T + R for theta in range(mk+1): # - Cost if theta == 0: L[i,mk+i] = -np.log(1 - PD) else: j = theta - 1 z = Z[j] L[i,j] = -(np.log(PD/lamb_c) - 0.5 * np.log(np.linalg.det(2*np.pi*S_h_i)) - \ 0.5 * (z - z_h_i).T @ np.linalg.inv(S_h_i) @ (z - z_h_i)) # Find optimal assignment print(L) theta_star = optimal_assignment(L) # Compute posterior density for i in range(n): print(f'theta={theta_star[i]}') x_k_kmin1, P_k_kmin1 = p_pred.components[i].x, p_pred.components[i].P if theta_star[i] >= mk: x_k_k[:,i], P_k_k[:,:,i] = x_k_kmin1, P_k_kmin1 else: z = Z[theta_star[i]] x_k_k[:,i], P_k_k[:,:,i] = kalman_update(x_k_kmin1, P_k_kmin1, H, R, z) p = GaussianMixture( [Gaussian(x_k_k[:,i], P_k_k[:,:,i]) for i in range(n)] ) return p, p_pred, theta_star
def exact_posterior_LG(p_prior, Z, PD, lamb_c, F, Q, H, R): """ Exact posterior distribution given linear and gaussian models with constant probability of detection and uniform clutter. """ # Dimensions nk = p_prior.components[0].x.shape[0] mk = len(Z) # Allocate p_pred = [] n_h_k = p_prior.n_components * (mk + 1) x_k_k = np.zeros((nk, n_h_k)) P_k_k = np.zeros((nk, nk, n_h_k)) w_tilde = np.zeros(n_h_k) h = -1 # Index for hypotesis at time k for p_h in p_prior.components: # Predict # - Gaussian, p_k|k-1(theta_1:k-1) x_k_kmin1, P_k_kmin1 = kalman_prediction(p_h.x, p_h.P, F, Q) # - Density, p(xk|Z_1:k-1) p_pred.append(Gaussian(x_k_kmin1, P_k_kmin1, p_h.w)) # Update # - Precompute predicted likelihood z_bar, S = kalman_prediction(x_k_kmin1, P_k_kmin1, H, R) # - Consider all data associations for theta in range(mk + 1): # Update h += 1 if theta == 0: # - Gaussian x_k_k[:, h], P_k_k[:, :, h] = x_k_kmin1, P_k_kmin1 # - Weight w_tilde[h] = p_h.w * (1 - PD) else: z = Z[theta - 1] # - Gaussian x_k_k[:, h], P_k_k[:, :, h] = kalman_update(x_k_kmin1, P_k_kmin1, H, R, z) # - Weight w_tilde[h] = (p_h.w * PD / lamb_c) * multivariate_normal.pdf( z, mean=z_bar, cov=S) # Normalize weights w_k = w_tilde / sum(w_tilde) # Predicted and posterior densities p_pred = GaussianMixture(p_pred) p = GaussianMixture( [Gaussian(x_k_k[:, i], P_k_k[:, :, i], w_k[i]) for i in range(n_h_k)]) return p, p_pred
def PDA_LG(p_prior, Z, PD, lamb_c, F, Q, H, R): """ Probabilistic data association filtering for linear and gaussian models with constant probability of detection and uniform clutter. """ # Dimensions nk = len(p_prior.x) mk = len(Z) # Predict x_k_kmin1, P_k_kmin1 = kalman_prediction(p_prior.x, p_prior.P, F, Q) # Update x_k_k = np.zeros((nk,mk+1)) P_k_k = np.zeros((nk,nk,mk+1)) w_tilde_theta = np.zeros(mk+1) # - Precompute predicted likelihood z_bar, S = kalman_prediction(x_k_kmin1, P_k_kmin1, H, R) for theta in range(mk + 1): if theta == 0: # - Gaussian x_k_k[:,theta], P_k_k[:,:,theta] = x_k_kmin1, P_k_kmin1 # - Weight w_tilde_theta[theta] = 1 - PD else: # - Gaussian z = Z[theta - 1] x_k_k[:,theta], P_k_k[:,:,theta] = kalman_update(x_k_kmin1, P_k_kmin1, H, R, z) # - Weight w_tilde_theta[theta] = (PD / lamb_c) * multivariate_normal.pdf(z, z_bar, S) # - Normalize weights w = w_tilde_theta / sum(w_tilde_theta) # Reduce to single gaussian density # - Mean x_PDA = np.zeros((nk,)) for i in range(mk + 1): x_PDA += w[i] * x_k_k[:,i] # - Covariance P_PDA = np.zeros((nk,nk)) for i in range(mk + 1): P_PDA += w[i] * P_k_k[:,:,i] + w[i] * (x_PDA - x_k_k[:,i]) @ (x_PDA - x_k_k[:,i]).T return Gaussian(x_PDA, P_PDA), Gaussian(x_k_kmin1, P_k_kmin1)
def HO_MHT_LG(p_prior, Z, n, PD, lamb_c, F, Q, H, R, Nmax, M, tol_prune): # Dimensions mk = len(Z) # Predict p_pred = [] for p_h in p_prior.mixtures: p_pred_h = [] for p_h_i in p_h.components: x_k_kmin1, P_k_kmin1 = kalman_prediction(p_h_i.x, p_h_i.P, F, Q) p_pred_h.append(Gaussian(x_k_kmin1, P_k_kmin1)) p_pred.append(GaussianMixture(p_pred_h, p_h.w)) p_pred = MultiGaussianMixture(p_pred, p_prior.ws) # Update # hk = 0 # Hypotesis index p = [] for p_h_kmin1 in p_pred.mixtures: L = create_cost_matrix(p_h_kmin1, Z, n, PD, lamb_c, H, R) M = 10 theta_stars, costs = compute_M_associations(L, M) print(f'theta_stars = {theta_stars}') print(f'costs = {costs}') for m in range(theta_stars.shape[0]): p_h_k = [] # hk += 1 l_tilde = p_h_kmin1.w for i in range(n): if theta_stars[m, i] > mk: # Not associated p_h_k.append(Gaussian(p_h_kmin1.x, p_h_kmin1.P)) else: # Associated z = Z[theta_stars[m, i]] x_k_k, P_k_k = kalman_update(p_h_kmin1.x, p_h_kmin1.P, H, R, z) p_h_k.append(Gaussian(x_k_k, P_k_k)) l_tilde += L[i, theta_stars[m, i]] p.append(GaussianMixture(p_h_k, l_tilde)) # Yes but no # Normalize log-weights l = l_tilde / sum(l_tilde) # Yes but no p = MultiGaussianMixture(p, l)
def GSF_linear_gaussian_models_simple_scenario(): arr = lambda scalar: np.array([[scalar]]) # Prior x_prior = arr(0.5) P_prior = arr(0.2) p_prior_exact = GaussianMixture(Gaussian(x_prior, P_prior, weight=1)) p_prior_GSF = GaussianMixture(Gaussian(x_prior, P_prior, weight=1)) # Models # - Motion Q = arr(0.35) F = arr(1) # - Measurement R = arr(0.2) H = arr(1) PD = 0.9 # - Clutter lamb = 0.4 # GSF tuning params Nmax = 5 tol_prune = 0.01 # or None # Sensor space = Space1D(-4, 4) # Create measurement vector Z1 = [-1.3, 1.7] Z2 = [1.3] Z3 = [-0.3, 2.3] Z4 = [-2, 3] Z5 = [2.6] Z6 = [-3.5, 2.8] Zs = [Z1, Z2, Z3, Z4, Z5, Z6] # Plot settings ax = plt.subplot() res = 100 marker_size = 100 x_lim = [space.min, space.max] # Compute exact posterior for k, Z in enumerate(Zs, start=1): print(f'Measurements: {Z}') # Compute posterior p_exact, p_pred_exact = exact_posterior_LG(p_prior_exact, Z, PD, lamb, F, Q, H, R) p_GSF, p_pred_GSF = GSF_LG(p_prior_GSF, Z, PD, lamb, F, Q, H, R, Nmax, tol_prune) # Number of hypotesis print(f'Number of hypotesis, exact posterior: {p_exact.n_components}') print(f'Number of hypotesis, GSF posterior: {p_GSF.n_components}') # Plot ax.clear() ax.set(title=f'k = {k}', xlabel='x') ax.axhline(0, color='gray', linewidth=0.5) # - Predicted density according to GSF plt1 = plot_gaussian_mixture_pdf(ax, p_pred_GSF, x_lim, res=res, color='r', linestyle='--', zorder=0) # - Exact posterior density plt2 = plot_gaussian_mixture_pdf(ax, p_exact, x_lim, res=res, color='k', zorder=1) # - Posterior density according to GSF plt3 = plot_gaussian_mixture_pdf(ax, p_GSF, x_lim, res=res, color='orange', linestyle='--', marker='x', zorder=2) # - Measurements plot_1D_measurements(ax, Z, color='b', marker='*', s=marker_size, zorder=3) # - Final details ax.set_xlim([space.min, space.max]) ax.set_ylim([-0.05, 1.05]) ax.legend((plt1, plt2, plt3), ('pred', 'exact', 'GSF')) plt.pause(0.0001) p_prior_exact = p_exact p_prior_GSF = p_GSF input('Press to continue')
def GNN_LG_models_2_obj_1D(): arr = lambda scalar: np.array([[scalar]]) # Prior x_prior = arr(2.5) P_prior = arr(0.36) p_prior = GaussianMixture( [Gaussian(-x_prior, P_prior), Gaussian(x_prior, P_prior)]) n = 2 # g_mix = GaussianMixture([Gaussian(-3, 0.2), Gaussian(3, 0.2)]) # p_prior = MultiGaussianMixture(g_mix) # Models # - Motion Q = arr(0.25) F = arr(1) # - Measurement R = arr(0.2) H = arr(1) PD = 0.85 # - Clutter lamb = 0.3 # Measurements Z = [-3.2, -2.4, 1.9, 2.2, 2.7, 2.9] Z = [arr(val) for val in Z] p, p_pred, theta_star = GNN_LG(p_prior, Z, n, PD, lamb, F, Q, H, R) # Plot ax = plt.subplot() res = 100 marker_size = 100 x_lim = [-4, 4] colors = ['orange', 'purple'] ax.axhline(0, color='gray', linewidth=0.5) # - Measurements plot_1D_measurements(ax, Z, color='b', marker='*', s=marker_size, zorder=3) # - Object densities plts = [] for i in range(n): # - Prior density plt1 = plot_gaussian_pdf(ax, p_prior.components[i], x_lim, res=res, color='g', zorder=1) # - Predicted density plt2 = plot_gaussian_pdf(ax, p_pred.components[i], x_lim, res=res, color='r', linestyle='--', zorder=0) # - Posterior density plt3 = plot_gaussian_pdf(ax, p.components[i], x_lim, res=res, color=colors[i], linestyle='--', zorder=2) # - Association if theta_star[i] >= len(Z): ax.scatter(p_pred.components[i].x, 0, color=colors[i], marker='o', s=marker_size, zorder=3) else: ax.scatter(Z[theta_star[i]], 0, color=colors[i], marker='*', s=marker_size, zorder=3) [plts.append(plt) for plt in [plt1, plt2, plt3]] # - Final details # ax.set_xlim(x_lim) ax.set_ylim([-0.05, 1.5]) ax.legend( plts, ('o1_prior', 'o1_pred', 'o1_GNN', 'o2_prior', 'o2_pred', 'o2_GNN')) plt.pause(0.0001)
def GNN_LG_models_2_obj_sequence_1D(): arr = lambda scalar: np.array([[scalar]]) # Space space = Space1D(-4, 4) # Prior x_prior = arr(2.5) P_prior = arr(0.36) p_prior = GaussianMixture( [Gaussian(-x_prior, P_prior), Gaussian(x_prior, P_prior)]) # Models # - Motion Q = arr(0.25) F = arr(1) # - Measurement R = arr(0.2) H = arr(1) PD = 0.85 _PD = lambda x: PD # - Clutter lamb = 0.4 clutter_model = UniformClutterModel1D(space, lamb) # Create true sequence n = 2 n_ks = 50 X = np.vstack((-x_prior * np.ones(n_ks), x_prior * np.ones(n_ks))) # Measurements sensor = Detector1D(R, _PD, clutter_model, space) Z = sensor.get_measurements(X) # Estimate trajectories x_hat = np.zeros((n, n_ks)) P_hat = np.zeros((n, n_ks)) thetas = np.zeros((n, n_ks)) for k in range(n_ks): print(f'---k={k+1}---') print(Z[k]) p, p_pred, theta_star = GNN_LG(p_prior, Z[k], n, PD, lamb, F, Q, H, R) for i in range(n): x_hat[i, k] = p.components[i].x P_hat[i, k] = p.components[i].P thetas[i, k] = theta_star[i] p_prior = p # Plot ax = plt.subplot() marker_size = 100 colors = ['orange', 'purple'] ass_colors = ['g', 'g'] y_axis = np.linspace(1, n_ks, num=n_ks) # - Heatmaps for k in range(n_ks): for i in range(n): plot_1D_heatmap(ax, x_hat[i, k], P_hat[i, k], k + 1) # - True trajectories for i in range(n): ax.plot(X[i, :], y_axis, color='k', marker='o') # - Measurements for k in range(n_ks): ax.axhline(k + 1.5, color='gray', linewidth=0.5) plot_1D_measurements(ax, Z[k], k=k + 1, color='b', marker='*', s=marker_size) # - Associations for k in range(n_ks): for i in range(n): if thetas[i, k] < len(Z[k]): ax.scatter(Z[k][int(thetas[i, k])], k + 1, color=ass_colors[i], marker='*', s=marker_size) # - Estimates for i in range(n): ax.plot(x_hat[i, :], y_axis, color=colors[i], marker='s') # - Final details ax.set(xlabel='x', ylabel='k') ax.set_xlim([space.min, space.max]) plt.pause(0.0001)
def PDA_linear_gaussian_models_simple_scenario(): arr = lambda scalar: np.array([[scalar]]) # Prior x_prior = arr(0.5) P_prior = arr(0.2) p_prior_exact = Gaussian(x_prior, P_prior, weight=1) p_prior_PDA = Gaussian(x_prior, P_prior) # Models # - Motion Q = arr(0.35) F = arr(1) # - Measurement R = arr(0.2) H = arr(1) PD = 0.9 # - Clutter lamb = 0.4 # Sensor space = Space1D(-4, 4) # Create measurement vector Z1 = [-1.3, 1.7] Z2 = [1.3] Z3 = [-0.3, 2.3] Z4 = [-2, 3] Z5 = [2.6] Z6 = [-3.5, 2.8] Zs = [Z1, Z2, Z3, Z4, Z5, Z6] # Plot settings ax = plt.subplot() res = 100 x_lim = [space.min, space.max] # Compute exact posterior for k, Z in enumerate(Zs, start=1): print(f'Measurements: {Z}') # Compute posterior p_exact, p_pred_exact = exact_posterior_LG(p_prior_exact, Z, PD, lamb, F, Q, H, R) p_PDA, p_pred_PDA = PDA_LG(p_prior_PDA, Z, PD, lamb, F, Q, H, R) # Number of hypotesis print(f'number of hypotesis: {p_exact.n_components}') # Plot ax.clear() ax.set(title=f'k = {k}', xlabel='x') ax.axhline(0, color='gray', linewidth=0.5) # - Predicted density according to PDA plt1 = plot_gaussian_pdf(ax, p_pred_PDA, x_lim, res=res, color='r', linestyle='--', zorder=0) # - Exact posterior density plt2 = plot_gaussian_mixture_pdf(ax, p_exact, x_lim, res=res, color='k', zorder=1) # - Posterior density according to PDA plt3 = plot_gaussian_pdf(ax, p_PDA, x_lim, res=res, color='g', marker='s', zorder=2) # - Hypotesis from PDA marker_size = 100 ax.scatter(p_pred_PDA.x, 0, color='b', marker='o', s=marker_size, zorder=3) x_axis = np.zeros(len(Z)) ax.scatter(Z, x_axis, color='b', marker='*', s=marker_size, zorder=3) ax.set_xlim(x_lim) ax.set_ylim([-0.05, 1.2]) ax.legend((plt1, plt2, plt3), ('pred', 'exact', 'PDA')) p_prior_exact = p_exact p_prior_PDA = p_PDA plt.pause(0.0001) input('Press to continue')
def GSF_LG(p_prior, Z, PD, lamb_c, F, Q, H, R, Nmax, tol_prune=None, tol_merge=None): """ Gaussian sum filtering for linear and gaussian models with constant probability of detection and uniform clutter. """ # Dimensions nk = p_prior.components[0].x.shape[0] mk = len(Z) # Allocate p_pred = [] n_h_k = p_prior.n_components * (mk + 1) x_k_k = np.zeros((nk, n_h_k)) P_k_k = np.zeros((nk, nk, n_h_k)) w_tilde = np.zeros(n_h_k) h = -1 # Index for hypotesis at time k for p_h in p_prior.components: # Predict x_k_kmin1, P_k_kmin1 = kalman_prediction(p_h.x, p_h.P, F, Q) p_pred.append(Gaussian(x_k_kmin1, P_k_kmin1, p_h.w)) # Update # - Precompute predicted likelihood z_bar, S = kalman_prediction(x_k_kmin1, P_k_kmin1, H, R) # - Consider all data associations for theta in range(mk + 1): h += 1 if theta == 0: # - Gaussian x_k_k[:, h], P_k_k[:, :, h] = x_k_kmin1, P_k_kmin1 # - Weight w_tilde[h] = p_h.w * (1 - PD) else: z = Z[theta - 1] # - Gaussian x_k_k[:, h], P_k_k[:, :, h] = kalman_update(x_k_kmin1, P_k_kmin1, H, R, z) # - Weight w_tilde[h] = (p_h.w * PD / lamb_c) * multivariate_normal.pdf( z, z_bar, S) # Normalize weights w_k = w_tilde / sum(w_tilde) # Reduction # - Pruning w_k, x_k_k, P_k_k = prune(w_k, x_k_k, P_k_k, tol_prune) # - Merging w_k, x_k_k, P_k_k = merge(w_k, x_k_k, P_k_k, tol_merge) # - Capping w_k, x_k_k, P_k_k = capp(w_k, x_k_k, P_k_k, Nmax) # Create final mixtures H_k = x_k_k.shape[1] p_pred = GaussianMixture(p_pred) p = GaussianMixture( [Gaussian(x_k_k[:, i], P_k_k[:, :, i], w_k[i]) for i in range(H_k)]) return p, p_pred