├── 2.fig ├── Data Association Multi ├── AIS_test.m ├── Copy_of_maze.png ├── JPDAF_EKF_Update.m ├── JPDAF_UKF_Update.m ├── JPDAF_Update.m ├── Observation_Association.m ├── PDAF_Update.m ├── Track_InitConfDel.m ├── Track_Init_Del.m ├── buildHyp.m ├── buildNet.asv ├── buildNet.m ├── buildTree.m ├── data_assoc_test.m ├── gen_clusters.m ├── gen_obs_AIS.m ├── gen_obs_cluttered.m ├── gen_obs_cluttered_multi.m ├── getAssocProb.m ├── gridLegend.m ├── jpda_ekf_test.m ├── jpda_test.m ├── jpda_test2.m ├── jpda_test_pf.m ├── jpda_test_pf_search.m ├── jpda_test_pf_search.m~ ├── jpda_ukf_test.m ├── mahalDist.m └── maze.png ├── Data Association ├── Observation_Association.m ├── PDAF_UKF_Update.m ├── PDAF_Update.m ├── Track_Init_Del.m ├── data_assoc_test.m ├── data_assoc_test_pf.m ├── data_assoc_test_pf_comp.m ├── data_assoc_test_pf_comp2.m ├── data_assoc_test_pf_comp3.m ├── gen_obs_cluttered.m ├── gen_obs_cluttered_multi.m ├── gen_obs_cluttered_multi2.m ├── mahalDist.m ├── maze.png └── plot_gaussian_ellipsoid.m ├── Data_Gen ├── Data_Generator.m ├── Data_Generator_Demo.m ├── gen_obs ├── gen_obs.m ├── gen_obs.m~ ├── gen_obs_cluttered.m └── get_vel.m ├── EHM ├── buildEHMnet2.m ├── buildEHMnet_fast.m ├── buildEHMnet_trans.m ├── buildEHMnet_trans2.m └── drawNetObj.m ├── EHM_old ├── EHM_test.m ├── EHM_trans_test.m ├── JPDAF_EHM_Update.m ├── buildAssocTree.m ├── buildEHMnet.m ├── buildEHMnet2.m ├── buildHyp.m ├── feasible_events_enum_table.m ├── getAncestors.m ├── getAssocProb.m ├── get_p_D.m ├── get_p_U.m └── my_setdiff.m ├── EHM_test ├── buildEHMnet2.m ├── buildEHMnet2_flavio.m ├── buildEHMnet_trans.m ├── buildTree.m ├── drawNetObj.m ├── drawTreeObj.m └── feasible_events_enum_table.m ├── FastSet ├── fast_find_range_sorted.m ├── fast_find_range_sorted.mexw32 ├── fast_find_range_sorted.mexw64 ├── fast_frequency.m ├── fast_frequency.mexw32 ├── fast_frequency.mexw64 ├── fast_intersect_sorted.m ├── fast_intersect_sorted.mexw32 ├── fast_intersect_sorted.mexw64 ├── fast_ismember_sorted.m ├── fast_ismember_sorted.mexw32 ├── fast_ismember_sorted.mexw64 ├── fast_setdiff_sorted.m ├── fast_setdiff_sorted.mexw32 ├── fast_setdiff_sorted.mexw64 ├── fast_setxor_sorted.m ├── fast_setxor_sorted.mexw32 ├── fast_setxor_sorted.mexw64 ├── fast_union_sorted.m ├── fast_union_sorted.mexw32 ├── fast_union_sorted.mexw64 ├── fast_unique.m ├── fast_unique.mexw32 └── fast_unique.mexw64 ├── Figures └── CH-CHR │ ├── noise=0.15.fig │ ├── noise=0.3.fig │ └── noise=0.5.fig ├── GM_PHD_Filter_v110 ├── GM_PHD_Filter │ ├── CalculateOSPAMetric.m │ ├── Calculate_Jacobian_H.m │ ├── ConvertPlusMinusPi.m │ ├── GM_EKF_PHD_Construct_Update_Components.m │ ├── GM_EKF_PHD_Create_Birth.m │ ├── GM_EKF_PHD_Initialise_Jacobians.m │ ├── GM_EKF_PHD_Predict_Birth.m │ ├── GM_EKF_PHD_Predict_Existing.m │ ├── GM_EKF_PHD_Simulate_Initialise.m │ ├── GM_EKF_PHD_Simulate_Measurements.m │ ├── GM_EKF_PHD_Simulate_Plot.m │ ├── GM_EKF_PHD_Update.m │ ├── GM_PHD_Calculate_Performance_Metric.m │ ├── GM_PHD_Construct_Update_Components.m │ ├── GM_PHD_Create_Birth.m │ ├── GM_PHD_Estimate.m │ ├── GM_PHD_Filter.m │ ├── GM_PHD_Initialisation.m │ ├── GM_PHD_Predict_Birth.m │ ├── GM_PHD_Predict_Existing.m │ ├── GM_PHD_Prune.m │ ├── GM_PHD_Simulate_Initialise.m │ ├── GM_PHD_Simulate_Measurements.m │ ├── GM_PHD_Simulate_Plot.m │ ├── GM_PHD_Update.m │ ├── Hungarian.m │ ├── README.txt │ ├── ReleaseNotes.txt │ ├── Test_Jacobian_Calculation.m │ ├── error_ellipse.m │ ├── ospa_dist.m │ └── unifpdf_2d.m └── Older_Version_GM_PHD_Filter_Without_EKF │ └── GM_PHD_Filter_v105b │ ├── ConvertPlusMinusPi.m │ ├── GM_PHD_Calculate_Performance_Metric.m │ ├── GM_PHD_Construct_Update_Components.m │ ├── GM_PHD_Create_Birth.m │ ├── GM_PHD_Estimate.m │ ├── GM_PHD_Filter.m │ ├── GM_PHD_Initialisation.m │ ├── GM_PHD_Predict_Birth.m │ ├── GM_PHD_Predict_Existing.m │ ├── GM_PHD_Prune.m │ ├── GM_PHD_Simulate_Initialise.m │ ├── GM_PHD_Simulate_Measurements.m │ ├── GM_PHD_Simulate_Plot.m │ ├── GM_PHD_Update.m │ ├── Hungarian.m │ ├── README.txt.txt │ ├── ReleaseNotes.txt │ ├── error_ellipse.m │ ├── ospa_dist.m │ └── unifpdf_2d.m ├── JPDAF ├── JPDAF_EHM_Update.m ├── JPDAF_UKF_Update.m ├── KalmanFilter_new.m ├── Observation_Association.m ├── Simulation Ground truth samples │ ├── ground_truth.xlsx │ ├── ground_truth1.mat │ ├── ground_truth1.xlsx │ ├── ground_truth2.mat │ ├── ground_truth2.xlsx │ ├── ground_truth3.mat │ ├── ground_truth3.xlsx │ ├── ground_truth_multi.mat │ ├── ground_truth_multi.xlsx │ ├── ground_truth_new.mat │ └── ground_truth_new.xlsx ├── Track_InitConfDel.m ├── UKalmanFilter.m ├── buildEHMnet.m ├── buildEHMnet2.m ├── buildHyp.m ├── gen_obs_cluttered_multi.m ├── getAncestors.m ├── getAssocProb.m ├── jpda_test.m ├── mahalDist.m ├── maze.png └── plot_gaussian_ellipsoid.m ├── JPDAF_Class ├── JPDAF.m ├── Track_InitConfDel.m ├── Track_InitConfDel_MN.m ├── buildEHMnet_trans.m ├── gen_obs_cluttered_multi2.m ├── mahalDist.m ├── plot_gaussian_ellipsoid.m ├── test.m ├── test_n_of_m.m └── test_search.m ├── JPDAF_EHM.rar ├── JPDAF_EHM ├── Example datasets │ ├── ground_truth.xlsx │ ├── ground_truth1.mat │ ├── ground_truth1.xlsx │ ├── ground_truth2.mat │ ├── ground_truth2.xlsx │ ├── ground_truth3.mat │ ├── ground_truth3.xlsx │ ├── ground_truth_multi.mat │ ├── ground_truth_multi.xlsx │ ├── ground_truth_new.mat │ └── ground_truth_new.xlsx ├── JPDAF_EHM_PF_Update.m ├── JPDAF_EHM_PF_Update.m~ ├── JPDAF_EHM_Update.m ├── JPDAF_TO_EHM_PF_Update.m ├── KalmanFilter_new.m ├── Observation_Association.m ├── Track_InitConfDel.m ├── UKalmanFilter.m ├── buildEHMnet2.m ├── buildEHMnet_fast.m ├── feasible_events_enum_table.m ├── gen_obs_cluttered_multi2.m ├── gen_obs_cluttered_multi_old.m ├── jpda_test.m ├── mahalDist.m ├── maze.png └── plot_gaussian_ellipsoid.m ├── Li_SMC_PHD pack ├── CommonFiles │ ├── CompWeight_distance_bearing.m │ ├── GENRANDN.m │ ├── Hungarian.m │ ├── Particle_State_Prediction.m │ ├── Trajectory_turn_complicated.m │ ├── cluster.m │ ├── compute_pD.m │ ├── myphd.m │ ├── ospa_dist.m │ ├── plotTrajectory.m │ └── resampling.m ├── MEAP(test).mat ├── Trajectory_turn.mat └── main_MEAP.m ├── Li_i.m ├── MEAP(test).mat ├── NearestSymmetricPositiveDefinite ├── html │ ├── nearestSPD_demo.html │ ├── nearestSPD_demo.png │ └── nearestSPD_demo_01.png ├── nearestSPD.m └── nearestSPD_demo.m ├── PDAF_2D3D ├── GenerateTrajectories.m ├── Kalman_Filter_Init.m ├── Structure_PDAF_Association.m ├── Structure_PDAF_Init_Data.m ├── Structure_PDAF_Init_Parameters.m ├── Structure_PDAF_Init_Track.m ├── Structure_PDAF_Show.m ├── Structure_PDAF_Track_Separation.m ├── Structure_PDAF_Track_Start.m ├── Structure_PDAF_Track_Update.m ├── Structure_PDAF_Tracking_Demo.m └── gaussian_prob.m ├── ProjectX ├── Data Association │ └── Probabilistic Data Association Filters │ │ ├── JPDAFilterX_Class │ │ ├── JPDAFX.asv │ │ ├── JPDAFilterX.m │ │ ├── buildEHMnet_trans.m │ │ ├── gen_obs_cluttered_multi2.m │ │ ├── plot_gaussian_ellipsoid.m │ │ └── test_jpdaf.m │ │ └── PDAFilterX_Class │ │ ├── JPDAFX.asv │ │ ├── PDAFilterX.asv │ │ ├── PDAFilterX.m │ │ ├── buildEHMnet_trans.m │ │ ├── gen_obs_cluttered_multi2.m │ │ ├── plot_gaussian_ellipsoid.m │ │ └── test_pdaf.m ├── Filters │ ├── Kalman Filters │ │ ├── EKalmanFilterX │ │ │ ├── EKalmanFilterX_Class │ │ │ │ ├── EKalmanFilterX.m │ │ │ │ ├── test2_ekfx.m │ │ │ │ └── test_ekfx.m │ │ │ └── EKalmanFilterX_Functions │ │ │ │ ├── Jacobians │ │ │ │ └── jaccsd.m │ │ │ │ ├── Prediction │ │ │ │ ├── EKalmanFilterX_Predict.m │ │ │ │ ├── EKalmanFilterX_PredictObs.m │ │ │ │ └── EKalmanFilterX_PredictState.m │ │ │ │ ├── Smoothing │ │ │ │ └── EKalmanFilterX_SmoothRTS.m │ │ │ │ └── Update │ │ │ │ ├── EKalmanFilterX_Update.m │ │ │ │ └── EKalmanFilterX_UpdatePDA.m │ │ ├── KalmanFilterX │ │ │ ├── KalmanFilterX_Class │ │ │ │ ├── KalmanFilterX.m │ │ │ │ ├── KalmanFilterX2.m │ │ │ │ ├── test2_kfx.m │ │ │ │ └── test_kfx.m │ │ │ └── KalmanFilterX_Functions │ │ │ │ ├── Prediction │ │ │ │ ├── KalmanFilterX_Predict.m │ │ │ │ ├── KalmanFilterX_PredictObs.m │ │ │ │ └── KalmanFilterX_PredictState.m │ │ │ │ ├── Smoothing │ │ │ │ └── KalmanFilterX_SmoothRTS.m │ │ │ │ └── Update │ │ │ │ ├── KalmanFilterX_Update.m │ │ │ │ └── KalmanFilterX_UpdatePDA.m │ │ └── UKalmanFilterX │ │ │ ├── UKalmanFilterX_Class │ │ │ ├── UKalmanFilterX.m │ │ │ ├── nearestSPD.m │ │ │ ├── test2_ukfx.m │ │ │ └── test_ukfx.m │ │ │ └── UKalmanFilterX_Functions │ │ │ ├── Prediction │ │ │ └── UKalmanFilterX_Predict.m │ │ │ ├── Smoothing │ │ │ ├── EKalmanFilterX_SmoothRTS.asv │ │ │ └── UKalmanFilterX_SmoothRTS.m │ │ │ ├── UnscentedTranform │ │ │ ├── FormAugmentedSigmas.m │ │ │ ├── UnscentedTransform.m │ │ │ └── nearestSPD.m │ │ │ └── Update │ │ │ ├── UKalmanFilterX_Update.m │ │ │ └── UKalmanFilterX_UpdatePDA.m │ ├── Particle Filters │ │ ├── EParticleFilterX │ │ │ ├── EParticleFilterX_Class │ │ │ │ ├── EParticleFilterX.m │ │ │ │ └── weightedcov.m │ │ │ └── EParticleFilterX_Functions │ │ │ │ ├── Prediction │ │ │ │ └── EParticleFilterX_Predict.m │ │ │ │ ├── Resampling │ │ │ │ ├── ParticleFilterX_Resample.m │ │ │ │ └── ParticleFilterX_UpdateWeights.asv │ │ │ │ └── Update │ │ │ │ ├── ParticleFilterX_Update.asv │ │ │ │ ├── ParticleFilterX_Update.m │ │ │ │ ├── ParticleFilterX_UpdatePDA.asv │ │ │ │ ├── ParticleFilterX_UpdatePDA.m │ │ │ │ ├── ParticleFilterX_UpdateWeights.asv │ │ │ │ └── ParticleFilterX_UpdateWeights.m │ │ ├── ParticleFilterX │ │ │ ├── ParticleFilterX_Class │ │ │ │ ├── ParticleFilterX.asv │ │ │ │ ├── ParticleFilterX.m │ │ │ │ └── test_bpfx.m │ │ │ └── ParticleFilterX_Functions │ │ │ │ ├── Prediction │ │ │ │ └── ParticleFilterX_Predict.m │ │ │ │ ├── Resampling │ │ │ │ └── ParticleFilterX_Resample.m │ │ │ │ └── Update │ │ │ │ ├── ParticleFilterX_Update.m │ │ │ │ ├── ParticleFilterX_UpdatePDA.m │ │ │ │ └── ParticleFilterX_UpdateWeights.m │ │ └── UParticleFilterX_Class │ │ │ ├── UParticleFilterX.m │ │ │ └── weightedcov.m │ └── Probability Hypothesis Density Filters │ │ └── SMC_PHDFilterX_Class │ │ ├── SMC_PHDFilterX.m │ │ ├── SMC_PHDX.asv │ │ ├── gen_obs_cluttered_multi3.m │ │ ├── kde2d.m │ │ └── phd_test.m ├── Models │ ├── Dynamic Models │ │ ├── ConstantHeadingModelX_Class │ │ │ ├── ConstantHeadingModel.asv │ │ │ ├── ConstantHeadingModel2X.m │ │ │ ├── ConstantHeadingModelX.m │ │ │ └── testCH.m │ │ ├── ConstantVelocityModelX_Class │ │ │ ├── ConstantVelocityModelX.m │ │ │ └── testCV.m │ │ ├── DynamicModelX_ClassTemplate │ │ │ ├── DynamicModelX.m │ │ │ └── testDynModelTemp.m │ │ └── GenericDynamicModelX_Class │ │ │ └── GenericDynamicModelX.m │ └── Observation Models │ │ ├── GenericObservationModelX_Class │ │ └── GenericObservationModelX.m │ │ ├── LinearGaussianObsModel_Class │ │ ├── PositionalObsModelX.m │ │ └── testPOM.m │ │ └── ObservationModelX_ClassTemplate │ │ ├── ObservationModelX.m │ │ └── testDynModelTemp.m ├── Track Management │ ├── ExistProbPHDSearch │ │ └── ExistProbPHDSearchX.m │ ├── LogLikRatio_TM │ │ ├── LogLikRatio_TM.asv │ │ └── LogLikRatio_TM.m │ └── MoutOfN_TM │ │ ├── MoutOfN_TM.asv │ │ └── MoutOfN_TM.m └── Workspace │ ├── EM │ └── EM_test.m │ ├── MTT │ ├── LogLik │ │ ├── test_lpr.asv │ │ └── test_lpr.m │ ├── MoutOfN │ │ ├── test_m_of_n.asv │ │ └── test_m_of_n.m │ └── PHD search │ │ ├── radar_test_search.m │ │ ├── test_search.asv │ │ └── test_search.m │ └── STT │ └── test_all.m ├── RMSE_σ=0.6.fig ├── RMSE_σ=1.fig ├── Radar Processing ├── Track_InitConfDelRadar.m ├── extract_data.m ├── lpr_initConfDel.m ├── phd_search.m ├── radar.mat └── restrict_data.m ├── Results └── EKFvUKFvPF │ └── 26-01-2017 │ └── OptimalCCVonly_r=1.mat ├── SMC_PHD ├── SMC_PHD.m ├── gen_obs_cluttered_multi3.m ├── kde2d.m └── phd_test.m ├── Task1_3.m ├── coin ├── coinflip0.m ├── coinflip1.m └── coinflip2.m ├── compute_ospa.m ├── config.J_k2 ├── configuremingw.p ├── datasets ├── 2_crossing_targets.mat ├── 3_roaming_targets.mat ├── AIS.mat ├── ground_truth.xlsx ├── ground_truth1.xlsx ├── ground_truth2.xlsx ├── ground_truth3.xlsx ├── ground_truth_ch1.xlsx ├── ground_truth_ch1[1].xlsx ├── ground_truth_ch2.xlsx ├── ground_truth_ch3.xlsx ├── ground_truth_multi.xlsx ├── ground_truth_new.xlsx └── maze.png ├── ecef.py ├── ecef.pyc ├── ell2xyz.m ├── expectedLi.m ├── expectedLi2.m ├── expectedLi3.m ├── fastAuction_v2.6 ├── auctionAlgorithmSparseMex.cpp ├── sparseAssignmentProblemAuctionAlgorithm.m └── yprime.c ├── fit_ellipse.m ├── flip.m~ ├── geodetic ├── Contents.m ├── DirInv.m ├── DirProb.m ├── Dist3D.m ├── InvProb.m ├── PltNetEl.m ├── ToUTM.m ├── cal2jd.m ├── cart2euler.m ├── cct2clg.m ├── clg2cct.m ├── ct2lg.m ├── dates.m ├── deg2rad.m ├── dg2lg.m ├── direct.m ├── dms2deg.m ├── dms2rad.m ├── doy2jd.m ├── ell2utm.m ├── ell2xyz.m ├── ellradii.m ├── errell2.m ├── errell3.m ├── euler2cart.m ├── findfixed.m ├── gps2jd.m ├── inverse.m ├── jd2cal.m ├── jd2dow.m ├── jd2doy.m ├── jd2gps.m ├── jd2mjd.m ├── jd2yr.m ├── jd2yr2.m ├── lg2ct.m ├── lg2dg.m ├── mjd2jd.m ├── plterrel.m ├── pltmap.m ├── pltnet.m ├── rad2deg.m ├── rad2dms.m ├── rad2sec.m ├── refell.m ├── rotct2lg.m ├── rotlg2ct.m ├── sec2rad.m ├── simil.m ├── sph2xyz.m ├── test_in.m ├── utm2ell.m ├── xyz2ell.m ├── xyz2ell2.m ├── xyz2ell3.m ├── xyz2sph.m └── yr2jd.m ├── geolife_preproc.py ├── geolife_preproc.txt ├── jdbcConfig.mat ├── kalmanf ├── EKFvUKFvPF_CH.m ├── EKFvUKFvPF_CH.m~ ├── EKalmanFilter.m ├── KFvEKFvPF_CH.m ├── KalmanFilter.m ├── KalmanFilter_new.m ├── KalmanFilter_old.m ├── PF_CH_MTT.m ├── UKalmanFilter.m ├── ekf.m ├── error_ellipse.m ├── kalmanf.m ├── maze.png ├── nearestSPD.m ├── pendulum_sim.m ├── pf_geo_test.m ├── sigmas.m ├── test_ekf.m ├── test_ekf_3.m ├── test_ekf_vs_ukf.m ├── test_ekf_vs_ukf.m~ ├── test_ekf_vs_ukf_1d.m ├── test_ekf_vs_ukf_ros.m ├── test_ekf_vs_ukf_ros.m~ ├── test_ekf_vs_ukf_vs_pf_ros.m ├── test_kf.m ├── test_kf_new.m ├── test_ukf.m ├── test_ukf_original.m ├── ukf.m ├── ukf_fl.m └── ukf_new.m ├── lla2ecef.m ├── matlab.mat ├── matlabx2.mat ├── maxmin.m ├── maze.png ├── needle ├── needle.pptx ├── needlesTandem.m ├── throwNeedles.m ├── throwNeedles.m~ └── throwNeedles2.m ├── particlef ├── ParticleFilter.m ├── ParticleFilterMin.m ├── ParticleFilterMin2.m ├── ParticleFilterMin2.m~ ├── ParticleFilter_new.m ├── ParticleFilter_old.m ├── example.m ├── particle_filter.m ├── test_pf.m ├── test_pf_new.m ├── test_pfilter.m ├── test_pfilter_ros.m ├── test_pfilter_ros_ch.m ├── test_pfilter_ros_chVScv.m ├── untitled.fig └── weightedcov.m ├── pollutant.m ├── refell.m ├── releasecodes ├── _common │ ├── BFMSpathwrap.m │ ├── Hungarian.m │ ├── assignmentoptimal.c │ ├── assignmentoptimal.mexa64 │ ├── assignmentoptimal.mexw32 │ ├── assignmentoptimal.mexw64 │ ├── ekf_predict_multiple.m │ ├── ekf_update_multiple.m │ ├── esf.m │ ├── gate_meas_ekf.m │ ├── gate_meas_gms.m │ ├── gate_meas_ukf.m │ ├── gaus_cap.m │ ├── gaus_cap_1.m │ ├── gaus_cap_3.m │ ├── gaus_merge.m │ ├── gaus_merge_1.m │ ├── gaus_prune.m │ ├── gaus_prune_1.m │ ├── gaus_prune_3.m │ ├── gen_gms.m │ ├── hist2d.m │ ├── histnd.m │ ├── kShortestPath_any.m │ ├── kalman_predict_multiple.m │ ├── kalman_update_multiple.m │ ├── kshortestwrap_pred.m │ ├── logsumexp.m │ ├── mbestwrap_updt_custom.m │ ├── murty_custom.m │ ├── ospa_dist.m │ ├── our_kmeans.m │ ├── resample.m │ ├── ukf_predict_multiple.m │ ├── ukf_update_multiple.m │ └── ut.m ├── bernoulli │ ├── ekf │ │ ├── demo.m │ │ ├── ekf_predict_mat.m │ │ ├── ekf_update_mat.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── gms │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── smc │ │ ├── compute_likelihood.m │ │ ├── compute_pD.m │ │ ├── compute_pS.m │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ └── ukf │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m ├── cbmember │ ├── ekf │ │ ├── demo.m │ │ ├── ekf_predict_mat.m │ │ ├── ekf_update_mat.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── gms │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── smc │ │ ├── compute_likelihood.m │ │ ├── compute_pD.m │ │ ├── compute_pS.m │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ └── ukf │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m ├── cphd │ ├── ekf │ │ ├── demo.m │ │ ├── ekf_predict_mat.m │ │ ├── ekf_update_mat.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── gms │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── smc │ │ ├── compute_likelihood.m │ │ ├── compute_pD.m │ │ ├── compute_pS.m │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ └── ukf │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m ├── glmb │ ├── ekf │ │ ├── demo.m │ │ ├── ekf_predict_mat.m │ │ ├── ekf_update_mat.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── gms │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── smc │ │ ├── compute_likelihood.m │ │ ├── compute_pD.m │ │ ├── compute_pS.m │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ └── ukf │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m ├── lmb │ ├── ekf │ │ ├── demo.m │ │ ├── ekf_predict_mat.m │ │ ├── ekf_update_mat.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── gms │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── smc │ │ ├── compute_likelihood.m │ │ ├── compute_pD.m │ │ ├── compute_pS.m │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ └── ukf │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m ├── phd │ ├── ekf │ │ ├── demo.m │ │ ├── ekf_predict_mat.m │ │ ├── ekf_update_mat.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── gms │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ ├── smc │ │ ├── compute_likelihood.m │ │ ├── compute_pD.m │ │ ├── compute_pS.m │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ └── ukf │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m ├── readme.txt ├── robust │ ├── jointcbmember │ │ └── smc │ │ │ ├── compute_likelihood.m │ │ │ ├── compute_likelihood_clt.m │ │ │ ├── compute_likelihood_tg.m │ │ │ ├── compute_pD.m │ │ │ ├── compute_pS.m │ │ │ ├── compute_pS_clt.m │ │ │ ├── compute_pS_tg.m │ │ │ ├── demo.m │ │ │ ├── gen_meas.m │ │ │ ├── gen_model.m │ │ │ ├── gen_newstate_clt.m │ │ │ ├── gen_newstate_fn.m │ │ │ ├── gen_newstate_fn_clt.m │ │ │ ├── gen_newstate_fn_tg.m │ │ │ ├── gen_newstate_tg.m │ │ │ ├── gen_observation_clt.m │ │ │ ├── gen_observation_fn.m │ │ │ ├── gen_observation_tgt.m │ │ │ ├── gen_truth.m │ │ │ ├── plot_results.m │ │ │ └── run_filter.m │ ├── jointcphd │ │ ├── ekf │ │ │ ├── demo.m │ │ │ ├── ekf_predict_mat.m │ │ │ ├── ekf_update_mat.m │ │ │ ├── gen_meas.m │ │ │ ├── gen_model.m │ │ │ ├── gen_newstate_fn.m │ │ │ ├── gen_observation_fn.m │ │ │ ├── gen_truth.m │ │ │ ├── plot_results.m │ │ │ └── run_filter.m │ │ ├── gms │ │ │ ├── demo.m │ │ │ ├── gen_meas.m │ │ │ ├── gen_model.m │ │ │ ├── gen_newstate_fn.m │ │ │ ├── gen_observation_fn.m │ │ │ ├── gen_truth.m │ │ │ ├── plot_results.m │ │ │ └── run_filter.m │ │ ├── smc │ │ │ ├── compute_likelihood_clt.m │ │ │ ├── compute_likelihood_tg.m │ │ │ ├── compute_pD.m │ │ │ ├── compute_pS_clt.m │ │ │ ├── compute_pS_tg.m │ │ │ ├── demo.m │ │ │ ├── gen_meas.m │ │ │ ├── gen_model.m │ │ │ ├── gen_newstate_clt.m │ │ │ ├── gen_newstate_fn.m │ │ │ ├── gen_newstate_fn_clt.m │ │ │ ├── gen_newstate_fn_tg.m │ │ │ ├── gen_newstate_tg.m │ │ │ ├── gen_observation_fn.m │ │ │ ├── gen_truth.m │ │ │ ├── plot_results.m │ │ │ └── run_filter.m │ │ └── ukf │ │ │ ├── demo.m │ │ │ ├── gen_meas.m │ │ │ ├── gen_model.m │ │ │ ├── gen_newstate_fn.m │ │ │ ├── gen_observation_fn.m │ │ │ ├── gen_truth.m │ │ │ ├── plot_results.m │ │ │ └── run_filter.m │ ├── lcphd │ │ ├── ekf │ │ │ ├── demo.m │ │ │ ├── ekf_predict_mat.m │ │ │ ├── ekf_update_mat.m │ │ │ ├── gen_meas.m │ │ │ ├── gen_model.m │ │ │ ├── gen_newstate_fn.m │ │ │ ├── gen_observation_fn.m │ │ │ ├── gen_truth.m │ │ │ ├── plot_results.m │ │ │ └── run_filter.m │ │ ├── gms │ │ │ ├── demo.m │ │ │ ├── gen_meas.m │ │ │ ├── gen_model.m │ │ │ ├── gen_newstate_fn.m │ │ │ ├── gen_observation_fn.m │ │ │ ├── gen_truth.m │ │ │ ├── plot_results.m │ │ │ └── run_filter.m │ │ ├── smc │ │ │ ├── compute_likelihood.m │ │ │ ├── compute_pD.m │ │ │ ├── compute_pS.m │ │ │ ├── demo.m │ │ │ ├── gen_meas.m │ │ │ ├── gen_model.m │ │ │ ├── gen_newstate_fn.m │ │ │ ├── gen_observation_fn.m │ │ │ ├── gen_truth.m │ │ │ ├── plot_results.m │ │ │ └── run_filter.m │ │ └── ukf │ │ │ ├── demo.m │ │ │ ├── gen_meas.m │ │ │ ├── gen_model.m │ │ │ ├── gen_newstate_fn.m │ │ │ ├── gen_observation_fn.m │ │ │ ├── gen_truth.m │ │ │ ├── plot_results.m │ │ │ └── run_filter.m │ └── pdcphd │ │ ├── ekf │ │ ├── demo.m │ │ ├── ekf_predict_mat.m │ │ ├── ekf_update_mat.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ │ ├── gms │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ │ ├── smc │ │ ├── compute_likelihood.m │ │ ├── compute_likelihood_tg.m │ │ ├── compute_pD.m │ │ ├── compute_pS.m │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_newstate_fn_tg.m │ │ ├── gen_newstate_tg.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m │ │ └── ukf │ │ ├── demo.m │ │ ├── gen_meas.m │ │ ├── gen_model.m │ │ ├── gen_newstate_fn.m │ │ ├── gen_observation_fn.m │ │ ├── gen_truth.m │ │ ├── plot_results.m │ │ └── run_filter.m └── singletarget │ ├── ekf │ ├── demo.m │ ├── ekf_predict_mat.m │ ├── ekf_update_mat.m │ ├── gen_meas.m │ ├── gen_model.m │ ├── gen_newstate_fn.m │ ├── gen_observation_fn.m │ ├── gen_truth.m │ ├── plot_results.m │ └── run_filter.m │ ├── gms │ ├── demo.m │ ├── gen_meas.m │ ├── gen_model.m │ ├── gen_newstate_fn.m │ ├── gen_observation_fn.m │ ├── gen_truth.m │ ├── plot_results.m │ └── run_filter.m │ ├── smc │ ├── compute_likelihood.m │ ├── compute_pD.m │ ├── demo.m │ ├── gen_meas.m │ ├── gen_model.m │ ├── gen_newstate_fn.m │ ├── gen_observation_fn.m │ ├── gen_truth.m │ ├── plot_results.m │ └── run_filter.m │ └── ukf │ ├── demo.m │ ├── gen_meas.m │ ├── gen_model.m │ ├── gen_newstate_fn.m │ ├── gen_observation_fn.m │ ├── gen_truth.m │ ├── plot_results.m │ └── run_filter.m ├── results1.mat ├── results1x2.mat ├── results2.mat ├── results3.mat ├── sample_mvgm ├── configuremingw.p ├── license.txt ├── mexme_sample_mvgm.m ├── sample_mvgm.c ├── sample_mvgm.mexw64 └── test_sample_mvgm.m ├── sim_σ_0,4.mat ├── sim_σ_0,8.mat ├── task3.m ├── traj_gen ├── createTraj3.m ├── createTraj5.m ├── license.txt └── test.m ├── ukf_vs_pf.m ├── untitled.fig └── wind.m /2.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/2.fig -------------------------------------------------------------------------------- /Data Association Multi/Copy_of_maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Data Association Multi/Copy_of_maze.png -------------------------------------------------------------------------------- /Data Association Multi/Track_Init_Del.m: -------------------------------------------------------------------------------- 1 | function Track_Init_Del(TrackList){ 2 | 3 | } -------------------------------------------------------------------------------- /Data Association Multi/gen_obs_AIS.m: -------------------------------------------------------------------------------- 1 | function [DataList, x, y] = gen_obs_AIS(data) 2 | % q = 0.5 3 | % q_clutter = 8; 4 | DataList = {}; 5 | x = []; 6 | y = []; 7 | for i=1:size(data,1) 8 | DataList{i} = [data(i,1); data(i,2)]; 9 | x(i,1) = data(i,1); 10 | y(i,1) = data(i,2); 11 | end 12 | end -------------------------------------------------------------------------------- /Data Association Multi/gen_obs_cluttered.m: -------------------------------------------------------------------------------- 1 | r = 0.5 2 | r_clutter = 2; 3 | DataList = []; 4 | for i=1:size(x_true) 5 | DataList(:,1,i) = [ x_true(i)+r*normrnd(0,r^2), y_true(i)+r*normrnd(0,r^2)]; 6 | for j=2:6 7 | DataList(:,j,i) = [ x_true(i)+r_clutter*normrnd(0,r_clutter^2), y_true(i)+r_clutter*normrnd(0,r_clutter^2)]; 8 | end 9 | end -------------------------------------------------------------------------------- /Data Association Multi/getAssocProb.m: -------------------------------------------------------------------------------- 1 | function betta = getAssocProb(MeasInd, TrackInd, HypMap, HypProb) 2 | ValidHyps = []; 3 | keys(HypMap) 4 | size(keys(HypMap)) 5 | 6 | % get the hypotheses where the node [MeasInd, TrackInd] exists 7 | for i = 1:size(keys(HypMap),2)-1 8 | i; 9 | HypMap(i) 10 | a = ismember([MeasInd, TrackInd], HypMap(i), 'rows'); 11 | if (a==1) 12 | ValidHyps = [ValidHyps, i]; 13 | end 14 | end 15 | betta = 0; 16 | ValidHyps 17 | 18 | % sum the Probabilities for all Valid hypothses 19 | for i = 1:size(ValidHyps,2) 20 | betta = betta + HypProb(ValidHyps(i)); 21 | end 22 | 23 | % Normalise betta 24 | betta = betta/(sum(HypProb(1:end-1))); 25 | 26 | end 27 | -------------------------------------------------------------------------------- /Data Association Multi/mahalDist.m: -------------------------------------------------------------------------------- 1 | function D = mahalDist(x, m, C, use_log) 2 | % p=gaussian_prob(x, m, C, use_log) 3 | % 4 | % Evaluate the multi-variate density with mean vector m and covariance 5 | % matrix C for the input vector x. 6 | % Vectorized version: Here X is a matrix of column vectors, and p is 7 | % a vector of probabilities for each vector. 8 | 9 | if nargin<4, use_log = 0; end 10 | 11 | d = length(m); 12 | 13 | if size(x,1)~=d 14 | x=x'; 15 | end 16 | N = size(x,2); 17 | 18 | m = m(:); 19 | M = m*ones(1,N); 20 | denom = (2*pi)^(d/2)*sqrt(abs(det(C))); 21 | invC = inv(C); 22 | mahal = sum(((x-M)'*invC).*(x-M)',2); % Chris Bregler's trick 23 | 24 | switch use_log, 25 | case 2, 26 | D = mahal; 27 | case 1, 28 | D = -0.5*mahal - log(denom); 29 | case 0, 30 | numer = exp(-0.5*mahal); 31 | D = numer/denom; 32 | otherwise 33 | error('Unsupported log type') 34 | end 35 | 36 | -------------------------------------------------------------------------------- /Data Association Multi/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Data Association Multi/maze.png -------------------------------------------------------------------------------- /Data Association/Track_Init_Del.m: -------------------------------------------------------------------------------- 1 | function Track_Init_Del(TrackList){ 2 | 3 | } -------------------------------------------------------------------------------- /Data Association/gen_obs_cluttered.m: -------------------------------------------------------------------------------- 1 | q = 0.4 2 | q_clutter = 2; 3 | DataList = []; 4 | for i=1:size(x_true) 5 | DataList(:,1,i) = [ x_true(i)+q*normrnd(0,q^2), y_true(i)+q*normrnd(0,q^2)]; 6 | for j=2:6 7 | DataList(:,j,i) = [ x_true(i)+q_clutter*normrnd(0,q_clutter^2), y_true(i)+q_clutter*normrnd(0,q_clutter^2)]; 8 | end 9 | end -------------------------------------------------------------------------------- /Data Association/gen_obs_cluttered_multi.m: -------------------------------------------------------------------------------- 1 | function [DataList, x, y] = gen_obs_cluttered_multi(TrackNum, x_true, y_true, q , q_clutter, No_of_clutter) 2 | % q = 0.5 3 | % q_clutter = 8; 4 | DataList = []; 5 | k=1; 6 | x = []; 7 | y = []; 8 | for i=1:2:size(x_true,1) 9 | for j=1:TrackNum 10 | x(k,j) = x_true(i,j); 11 | y(k,j) = y_true(i,j); 12 | DataList(:,j,k) = [ x_true(i,j)+normrnd(0,q), y_true(i,j)+normrnd(0,q)]; 13 | end 14 | 15 | for j=TrackNum+1:No_of_clutter 16 | DataList(:,j,k) = [ x_true(i,1)+normrnd(0,q_clutter), y_true(i,1)+normrnd(0,q_clutter)]; 17 | end 18 | k=k+1; 19 | end 20 | end -------------------------------------------------------------------------------- /Data Association/gen_obs_cluttered_multi2.m: -------------------------------------------------------------------------------- 1 | function [DataList, x, y] = gen_obs_cluttered_multi2(TrackNum, x_true, y_true, q , q_clutter, lambda, iter) 2 | % q = 0.5 3 | % q_clutter = 8; 4 | DataList = {}; 5 | k=1; 6 | x = []; 7 | y = []; 8 | for i=1:iter:size(x_true,1) 9 | for j=1:TrackNum 10 | x(k,j) = x_true(i,j); 11 | y(k,j) = y_true(i,j); 12 | if(x(k,j)==0&&y(k,j)==0) 13 | DataList{k}(:,j) = [ 0, 0]; 14 | else 15 | DataList{k}(:,j) = [ x_true(i,j)+normrnd(0,q), y_true(i,j)+normrnd(0,q)]; 16 | end 17 | end 18 | No_of_clutter = poissrnd(lambda); 19 | for j=TrackNum+1:No_of_clutter 20 | DataList{k}(:,j) = [unifrnd(0,25), unifrnd(0,20)]; 21 | end 22 | k=k+1; 23 | end 24 | end -------------------------------------------------------------------------------- /Data Association/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Data Association/maze.png -------------------------------------------------------------------------------- /Data_Gen/Data_Generator_Demo.m: -------------------------------------------------------------------------------- 1 | dat_gen = Data_Generator(); 2 | dat_gen = dat_gen.setProperties(4, 2, @(x,Dt) [x(1)+x(3)*0.1*Dt; x(2)+x(4)*0.1*Dt; x(3); x(4)], 5, @(x)[x(1); x(2)], 1, 10, 200); 3 | [data] = dat_gen.Generate(); 4 | figure 5 | for i = 1:10 6 | plot(data{i}.trackObj.x_true(1,:), data{1}.trackObj.x_true(2,:)); 7 | hold on 8 | end -------------------------------------------------------------------------------- /Data_Gen/gen_obs: -------------------------------------------------------------------------------- 1 | q = 0.5 2 | 3 | for i=1:1019 4 | obs_x(i) = x(i)+q*randn(0,q); 5 | end -------------------------------------------------------------------------------- /Data_Gen/gen_obs.m: -------------------------------------------------------------------------------- 1 | function [x, obs_x, obs_y, obs_r, obs_phi] = gen_obs(x_true, y_true, th_true, q) 2 | obs_x = []; 3 | obs_y = []; 4 | x = []; 5 | j=1; 6 | for i=1:3:size(x_true, 1) 7 | x(:,j) = [x_true(i,1); y_true(i,1); th_true(i,1)]; 8 | noise(j,:) = mvnrnd([0,0],[q^2,0;0,q^2]); 9 | obs_x(j,1) = x_true(i)+noise(j,1); 10 | obs_y(j,1) = y_true(i)+noise(j,2); 11 | obs_r(j,1) = sqrt(x_true(i)^2+y_true(i)^2)+mvnrnd(0,q^2); 12 | obs_phi(j,1) = atan2(y_true(i),x_true(i))+mvnrnd(0,2*pi/360); 13 | j= j + 1; 14 | end 15 | end -------------------------------------------------------------------------------- /Data_Gen/gen_obs.m~: -------------------------------------------------------------------------------- 1 | q = 0.5 2 | obs_x = []; 3 | obs_y = []; 4 | x = []; 5 | j=1; 6 | for i=1:2:size(x_true, 1) 7 | x(:,j) = [x_true(i,1); y_true(i,1); th_true(i,1)]; 8 | noise(j,2) = mvnrnd([0,0],[q^2,0;0,q^2]); 9 | obs_x(j,1) = x_true(i)+noise(j,1); 10 | obs_y(j,1) = y_true(i)+n; 11 | j= j + 1; 12 | end -------------------------------------------------------------------------------- /Data_Gen/get_vel.m: -------------------------------------------------------------------------------- 1 | v = []; 2 | v(:,1) = [x_true(2,1)-x_true(1,1); y_true(2,1)-y_true(1,1)]; 3 | for i=2:size(x_true, 1) 4 | v(:,i) = [x_true(i,1)-x_true(i-1,1); y_true(i,1)-y_true(i-1,1)]; 5 | end 6 | -------------------------------------------------------------------------------- /EHM_old/getAncestors.m: -------------------------------------------------------------------------------- 1 | function Ancestors = getAncestors(ChildInd, Ancestors, EdgeList) 2 | ParentIndList = find(~cellfun(@isempty,EdgeList(:,ChildInd)))'; 3 | for i=1:size(ParentIndList,2) 4 | Ancestors = union(Ancestors,EdgeList{ParentIndList(i),ChildInd}); 5 | newChildInd = ParentIndList(i); 6 | %newParentIndList = find(~cellfun(@isempty,EdgeList{:,newChildInd}))'; 7 | Ancestors = getAncestors(newChildInd, Ancestors, EdgeList); 8 | end 9 | end -------------------------------------------------------------------------------- /EHM_old/getAssocProb.m: -------------------------------------------------------------------------------- 1 | function betta = getAssocProb(MeasInd, TrackInd, HypMap, HypProb) 2 | ValidHyps = []; 3 | keys(HypMap); 4 | size(keys(HypMap)); 5 | 6 | % get the hypotheses where the node [MeasInd, TrackInd] exists 7 | for i = 1:size(keys(HypMap),2)-1 8 | i; 9 | HypMap(i); 10 | a = ismember([MeasInd, TrackInd], HypMap(i), 'rows'); 11 | if (a==1) 12 | ValidHyps = [ValidHyps, i]; 13 | end 14 | end 15 | betta = 0; 16 | ValidHyps; 17 | 18 | % sum the Probabilities for all Valid hypothses 19 | for i = 1:size(ValidHyps,2) 20 | betta = betta + HypProb(ValidHyps(i)); 21 | end 22 | 23 | % Normalise betta 24 | betta = betta/(sum(HypProb(1:end-1))); 25 | 26 | end 27 | -------------------------------------------------------------------------------- /EHM_old/my_setdiff.m: -------------------------------------------------------------------------------- 1 | function C = my_setdiff(A,B) 2 | % MYSETDIFF Set difference of two sets of positive integers (much faster than built-in setdiff) 3 | % C = my_setdiff(A,B) 4 | % C = A \ B = { things in A that are not in B } 5 | 6 | % 7 | if isempty(A) 8 | C = []; 9 | return; 10 | elseif isempty(B) 11 | C = A; 12 | return; 13 | else % both non-empty 14 | bits = zeros(1, max(max(A), max(B))); 15 | bits(A) = 1; 16 | bits(B) = 0; 17 | C = A(logical(bits(A))); 18 | end -------------------------------------------------------------------------------- /FastSet/fast_find_range_sorted.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_find_range_sorted.mexw32 -------------------------------------------------------------------------------- /FastSet/fast_find_range_sorted.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_find_range_sorted.mexw64 -------------------------------------------------------------------------------- /FastSet/fast_frequency.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_frequency.mexw32 -------------------------------------------------------------------------------- /FastSet/fast_frequency.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_frequency.mexw64 -------------------------------------------------------------------------------- /FastSet/fast_intersect_sorted.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_intersect_sorted.mexw32 -------------------------------------------------------------------------------- /FastSet/fast_intersect_sorted.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_intersect_sorted.mexw64 -------------------------------------------------------------------------------- /FastSet/fast_ismember_sorted.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_ismember_sorted.mexw32 -------------------------------------------------------------------------------- /FastSet/fast_ismember_sorted.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_ismember_sorted.mexw64 -------------------------------------------------------------------------------- /FastSet/fast_setdiff_sorted.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_setdiff_sorted.mexw32 -------------------------------------------------------------------------------- /FastSet/fast_setdiff_sorted.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_setdiff_sorted.mexw64 -------------------------------------------------------------------------------- /FastSet/fast_setxor_sorted.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_setxor_sorted.mexw32 -------------------------------------------------------------------------------- /FastSet/fast_setxor_sorted.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_setxor_sorted.mexw64 -------------------------------------------------------------------------------- /FastSet/fast_union_sorted.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_union_sorted.mexw32 -------------------------------------------------------------------------------- /FastSet/fast_union_sorted.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_union_sorted.mexw64 -------------------------------------------------------------------------------- /FastSet/fast_unique.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_unique.mexw32 -------------------------------------------------------------------------------- /FastSet/fast_unique.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/FastSet/fast_unique.mexw64 -------------------------------------------------------------------------------- /Figures/CH-CHR/noise=0.15.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Figures/CH-CHR/noise=0.15.fig -------------------------------------------------------------------------------- /Figures/CH-CHR/noise=0.3.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Figures/CH-CHR/noise=0.3.fig -------------------------------------------------------------------------------- /Figures/CH-CHR/noise=0.5.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Figures/CH-CHR/noise=0.5.fig -------------------------------------------------------------------------------- /GM_PHD_Filter_v110/GM_PHD_Filter/ConvertPlusMinusPi.m: -------------------------------------------------------------------------------- 1 | function theta = ConvertPlusMinusPi(theta) 2 | while(theta > pi) 3 | theta = theta - 2 * pi; 4 | end 5 | while(theta < -pi) 6 | theta = theta + 2 * pi; 7 | end 8 | end -------------------------------------------------------------------------------- /GM_PHD_Filter_v110/GM_PHD_Filter/GM_PHD_Calculate_Performance_Metric.m: -------------------------------------------------------------------------------- 1 | %GM_PHD_Calculate_Performance_Metric 2 | %Matlab code by Bryan Clarke b.clarke@acfr.usyd.edu.au 3 | 4 | %This uses the Optimal Subpattern Assignment (OSPA) metric proposed by 5 | %Schuhmacher, D.; Ba-Tuong Vo; Ba-Ngu Vo, "A Consistent Metric for Performance Evaluation of Multi-Object Filters," Signal Processing, IEEE Transactions on , vol.56, no.8, pp.3447,3457, Aug. 2008 6 | %cutoff_c and order_p are tuning parameters set in GM_PHD_Initialisation 7 | if(CALCULATE_OSPA_METRIC == 1) 8 | X = X_k; 9 | Y = [simTarget1History(:,k), simTarget2History(:,k)]; 10 | if(k >= simTarget3SpawnTime) 11 | Y = [Y, simTarget3History(:,k-simTarget3SpawnTime+1)]; 12 | end 13 | 14 | metric = ospa_dist(X, Y, cutoff_c, order_p);%Use Ba-Ngu Vo's implementation 15 | metric_history = [metric_history, metric]; 16 | 17 | end -------------------------------------------------------------------------------- /GM_PHD_Filter_v110/GM_PHD_Filter/ospa_dist.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/GM_PHD_Filter_v110/GM_PHD_Filter/ospa_dist.m -------------------------------------------------------------------------------- /GM_PHD_Filter_v110/Older_Version_GM_PHD_Filter_Without_EKF/GM_PHD_Filter_v105b/ConvertPlusMinusPi.m: -------------------------------------------------------------------------------- 1 | function theta = ConvertPlusMinusPi(theta) 2 | while(theta > pi) 3 | theta = theta - 2 * pi; 4 | end 5 | while(theta < -pi) 6 | theta = theta + 2 * pi; 7 | end 8 | end -------------------------------------------------------------------------------- /GM_PHD_Filter_v110/Older_Version_GM_PHD_Filter_Without_EKF/GM_PHD_Filter_v105b/ospa_dist.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/GM_PHD_Filter_v110/Older_Version_GM_PHD_Filter_Without_EKF/GM_PHD_Filter_v105b/ospa_dist.m -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth.xlsx -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth1.mat -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth1.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth1.xlsx -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth2.mat -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth2.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth2.xlsx -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth3.mat -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth3.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth3.xlsx -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth_multi.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth_multi.mat -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth_multi.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth_multi.xlsx -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth_new.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth_new.mat -------------------------------------------------------------------------------- /JPDAF/Simulation Ground truth samples/ground_truth_new.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/Simulation Ground truth samples/ground_truth_new.xlsx -------------------------------------------------------------------------------- /JPDAF/getAncestors.m: -------------------------------------------------------------------------------- 1 | function Ancestors = getAncestors(ChildInd, Ancestors, EdgeList) 2 | ParentIndList = find(~cellfun(@isempty,EdgeList(:,ChildInd)))'; 3 | for i=1:size(ParentIndList,2) 4 | Ancestors = union(Ancestors,EdgeList{ParentIndList(i),ChildInd}); 5 | newChildInd = ParentIndList(i); 6 | %newParentIndList = find(~cellfun(@isempty,EdgeList{:,newChildInd}))'; 7 | Ancestors = getAncestors(newChildInd, Ancestors, EdgeList); 8 | end 9 | end -------------------------------------------------------------------------------- /JPDAF/getAssocProb.m: -------------------------------------------------------------------------------- 1 | function betta = getAssocProb(MeasInd, TrackInd, HypMap, HypProb) 2 | ValidHyps = []; 3 | keys(HypMap); 4 | size(keys(HypMap)); 5 | 6 | % get the hypotheses where the node [MeasInd, TrackInd] exists 7 | for i = 1:size(keys(HypMap),2)-1 8 | a = ismember([MeasInd, TrackInd], HypMap(i), 'rows'); 9 | if (a==1) 10 | ValidHyps = [ValidHyps, i]; % Add to Valid hypotheses 11 | end 12 | end 13 | 14 | betta = 0; 15 | % sum the Probabilities for all Valid hypothses 16 | for i = 1:size(ValidHyps,2) 17 | betta = betta + HypProb(ValidHyps(i)); 18 | end 19 | 20 | % Normalise betta 21 | betta = betta/(sum(HypProb(1:end-1))); 22 | 23 | end 24 | -------------------------------------------------------------------------------- /JPDAF/mahalDist.m: -------------------------------------------------------------------------------- 1 | function D = mahalDist(x, m, C, use_log) 2 | % p=gaussian_prob(x, m, C, use_log) 3 | % 4 | % Evaluate the multi-variate density with mean vector m and covariance 5 | % matrix C for the input vector x. 6 | % Vectorized version: Here X is a matrix of column vectors, and p is 7 | % a vector of probabilities for each vector. 8 | 9 | if nargin<4, use_log = 0; end 10 | 11 | d = length(m); 12 | 13 | if size(x,1)~=d 14 | x=x'; 15 | end 16 | N = size(x,2); 17 | 18 | m = m(:); 19 | M = m*ones(1,N); 20 | denom = (2*pi)^(d/2)*sqrt(abs(det(C))); 21 | invC = inv(C); 22 | mahal = sum(((x-M)'*invC).*(x-M)',2); % Chris Bregler's trick 23 | 24 | switch use_log, 25 | case 2, 26 | D = mahal; 27 | case 1, 28 | D = -0.5*mahal - log(denom); 29 | case 0, 30 | numer = exp(-0.5*mahal); 31 | D = numer/denom; 32 | otherwise 33 | error('Unsupported log type') 34 | end 35 | 36 | -------------------------------------------------------------------------------- /JPDAF/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF/maze.png -------------------------------------------------------------------------------- /JPDAF_Class/mahalDist.m: -------------------------------------------------------------------------------- 1 | function D = mahalDist(x, m, C, use_log) 2 | % p=gaussian_prob(x, m, C, use_log) 3 | % 4 | % Evaluate the multi-variate density with mean vector m and covariance 5 | % matrix C for the input vector x. 6 | % Vectorized version: Here X is a matrix of column vectors, and p is 7 | % a vector of probabilities for each vector. 8 | 9 | if nargin<4, use_log = 0; end 10 | 11 | d = length(m); 12 | 13 | if size(x,1)~=d 14 | x=x'; 15 | end 16 | N = size(x,2); 17 | 18 | m = m(:); 19 | M = m*ones(1,N); 20 | denom = (2*pi)^(d/2)*sqrt(abs(det(C))); 21 | invC = inv(C); 22 | mahal = sum(((x-M)'*invC).*(x-M)',2); % Chris Bregler's trick 23 | 24 | switch use_log, 25 | case 2, 26 | D = mahal; 27 | case 1, 28 | D = -0.5*mahal - log(denom); 29 | case 0, 30 | numer = exp(-0.5*mahal); 31 | D = numer/denom; 32 | otherwise 33 | error('Unsupported log type') 34 | end 35 | 36 | -------------------------------------------------------------------------------- /JPDAF_EHM.rar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM.rar -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth.xlsx -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth1.mat -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth1.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth1.xlsx -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth2.mat -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth2.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth2.xlsx -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth3.mat -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth3.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth3.xlsx -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth_multi.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth_multi.mat -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth_multi.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth_multi.xlsx -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth_new.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth_new.mat -------------------------------------------------------------------------------- /JPDAF_EHM/Example datasets/ground_truth_new.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/Example datasets/ground_truth_new.xlsx -------------------------------------------------------------------------------- /JPDAF_EHM/mahalDist.m: -------------------------------------------------------------------------------- 1 | function D = mahalDist(x, m, C, use_log) 2 | % p=gaussian_prob(x, m, C, use_log) 3 | % 4 | % Evaluate the multi-variate density with mean vector m and covariance 5 | % matrix C for the input vector x. 6 | % Vectorized version: Here X is a matrix of column vectors, and p is 7 | % a vector of probabilities for each vector. 8 | 9 | if nargin<4, use_log = 0; end 10 | 11 | d = length(m); 12 | 13 | if size(x,1)~=d 14 | x=x'; 15 | end 16 | N = size(x,2); 17 | 18 | m = m(:); 19 | M = m*ones(1,N); 20 | denom = (2*pi)^(d/2)*sqrt(abs(det(C))); 21 | invC = inv(C); 22 | mahal = sum(((x-M)'*invC).*(x-M)',2); % Chris Bregler's trick 23 | 24 | switch use_log, 25 | case 2, 26 | D = mahal; 27 | case 1, 28 | D = -0.5*mahal - log(denom); 29 | case 0, 30 | numer = exp(-0.5*mahal); 31 | D = numer/denom; 32 | otherwise 33 | error('Unsupported log type') 34 | end 35 | 36 | -------------------------------------------------------------------------------- /JPDAF_EHM/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/JPDAF_EHM/maze.png -------------------------------------------------------------------------------- /Li_SMC_PHD pack/CommonFiles/GENRANDN.m: -------------------------------------------------------------------------------- 1 | function x=GENRANDN(m,R,M) 2 | % this function draws M samples from N(m,R) 3 | % where m is the mean vector(row) and R is the covariance matrix which must 4 | % be positive definite 5 | n=length(m); % get the dimension 6 | C=chol(R); % perform cholesky decomp R = C'C 7 | %X=randn(M,n); % draw M samples from N(0,I) 8 | %x=(X*C+ones(M,1)*m')'; 9 | X=randn(n,M); 10 | x=C'*X+m*ones(1,M); -------------------------------------------------------------------------------- /Li_SMC_PHD pack/CommonFiles/cluster.m: -------------------------------------------------------------------------------- 1 | function x=cluster(data,N) 2 | if N>1 3 | % [centers, U, obj_fcn] = fcm(data', N); 4 | [index,centers]= kmeans(data',N,... 5 | 'maxiter',50,'replicates',1,'emptyaction','singleton'); 6 | x=centers'; 7 | elseif N==1 8 | x=mean(data,2); 9 | else 10 | x=[]; 11 | end 12 | 13 | -------------------------------------------------------------------------------- /Li_SMC_PHD pack/CommonFiles/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | % compute pD at a vector of values X 4 | % this function is problem dependent 5 | % here it is a Gaussian which depends only on position 6 | 7 | mid= [0; 0]; 8 | % cov= diag([4000,4000].^2); % the test used before. 9 | cov= diag([6000,6000].^2); 10 | 11 | M= size(X,2); 12 | P= model.pos*X(1:4,:); %coordinate associated with X 13 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 14 | %pD= exp(-e_sq/2)'; 15 | pD= model.Pd*exp(-e_sq/2)'; %use this to scale the peak of the function i.e. average value by P_D) 16 | % pD= model.Pd*ones(1,size(X,2))'; %use this for a constant P_D for comparison with EK and UK approximations 17 | -------------------------------------------------------------------------------- /Li_SMC_PHD pack/CommonFiles/ospa_dist.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Li_SMC_PHD pack/CommonFiles/ospa_dist.m -------------------------------------------------------------------------------- /Li_SMC_PHD pack/MEAP(test).mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Li_SMC_PHD pack/MEAP(test).mat -------------------------------------------------------------------------------- /Li_SMC_PHD pack/Trajectory_turn.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Li_SMC_PHD pack/Trajectory_turn.mat -------------------------------------------------------------------------------- /Li_i.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Li_i.m -------------------------------------------------------------------------------- /MEAP(test).mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/MEAP(test).mat -------------------------------------------------------------------------------- /NearestSymmetricPositiveDefinite/html/nearestSPD_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/NearestSymmetricPositiveDefinite/html/nearestSPD_demo.png -------------------------------------------------------------------------------- /NearestSymmetricPositiveDefinite/html/nearestSPD_demo_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/NearestSymmetricPositiveDefinite/html/nearestSPD_demo_01.png -------------------------------------------------------------------------------- /ProjectX/Filters/Kalman Filters/EKalmanFilterX/EKalmanFilterX_Functions/Jacobians/jaccsd.m: -------------------------------------------------------------------------------- 1 | function [z,J] = jaccsd(fun,x) 2 | % JACCSD Compute Jacobian through complex step differentiation 3 | % 4 | % INPUTS: x - A state vector 5 | % fun - A (non-linear) transition function 6 | % 7 | % OUTPUTS: z - The transformed vector 8 | % J - The computed Jacobian 9 | % 10 | % USAGE: [z,J] = jaccsd(fun,x) 11 | z = fun(x); 12 | n = numel(x); 13 | h = n*eps; 14 | J = imag(fun(repmat(x,1,n)+eye(n)*h*1i))./h; 15 | end -------------------------------------------------------------------------------- /ProjectX/Filters/Kalman Filters/EKalmanFilterX/EKalmanFilterX_Functions/Smoothing/EKalmanFilterX_SmoothRTS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Kalman Filters/EKalmanFilterX/EKalmanFilterX_Functions/Smoothing/EKalmanFilterX_SmoothRTS.m -------------------------------------------------------------------------------- /ProjectX/Filters/Kalman Filters/KalmanFilterX/KalmanFilterX_Class/KalmanFilterX2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Kalman Filters/KalmanFilterX/KalmanFilterX_Class/KalmanFilterX2.m -------------------------------------------------------------------------------- /ProjectX/Filters/Kalman Filters/KalmanFilterX/KalmanFilterX_Functions/Smoothing/KalmanFilterX_SmoothRTS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Kalman Filters/KalmanFilterX/KalmanFilterX_Functions/Smoothing/KalmanFilterX_SmoothRTS.m -------------------------------------------------------------------------------- /ProjectX/Filters/Kalman Filters/UKalmanFilterX/UKalmanFilterX_Class/UKalmanFilterX.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Kalman Filters/UKalmanFilterX/UKalmanFilterX_Class/UKalmanFilterX.m -------------------------------------------------------------------------------- /ProjectX/Filters/Kalman Filters/UKalmanFilterX/UKalmanFilterX_Functions/Smoothing/EKalmanFilterX_SmoothRTS.asv: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Kalman Filters/UKalmanFilterX/UKalmanFilterX_Functions/Smoothing/EKalmanFilterX_SmoothRTS.asv -------------------------------------------------------------------------------- /ProjectX/Filters/Kalman Filters/UKalmanFilterX/UKalmanFilterX_Functions/Smoothing/UKalmanFilterX_SmoothRTS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Kalman Filters/UKalmanFilterX/UKalmanFilterX_Functions/Smoothing/UKalmanFilterX_SmoothRTS.m -------------------------------------------------------------------------------- /ProjectX/Filters/Particle Filters/EParticleFilterX/EParticleFilterX_Functions/Update/ParticleFilterX_UpdateWeights.m: -------------------------------------------------------------------------------- 1 | function [newWeights] = ParticleFilterX_UpdateWeights(lik,y,parts,weights) 2 | % PARTICLEFILTERX_UPDATEWEIGHTS Perform the discrete-time PF weight update 3 | % step, under the assumption of additive process noise. 4 | % 5 | % INPUTS: lik(y,x) - A measurement likelihood function. 6 | % y - A (yDim x Nm) measurement matrix 7 | % parts - A (xDim x Np) particle matrix. 8 | % weights - A (1 x Np) weights matrix. 9 | % 10 | % OUTPUTS: newWeights - The (1 x Np) updated weights matrix matrix. 11 | % 12 | % October 2017 Lyudmil Vladimirov, University of Liverpool. 13 | 14 | % Update and normalise weights 15 | newWeights = weights.*lik(y,parts); 16 | newWeights = newWeights./sum(newWeights,2); 17 | end 18 | -------------------------------------------------------------------------------- /ProjectX/Filters/Particle Filters/ParticleFilterX/ParticleFilterX_Class/ParticleFilterX.asv: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Particle Filters/ParticleFilterX/ParticleFilterX_Class/ParticleFilterX.asv -------------------------------------------------------------------------------- /ProjectX/Filters/Particle Filters/ParticleFilterX/ParticleFilterX_Class/ParticleFilterX.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Particle Filters/ParticleFilterX/ParticleFilterX_Class/ParticleFilterX.m -------------------------------------------------------------------------------- /ProjectX/Filters/Particle Filters/ParticleFilterX/ParticleFilterX_Functions/Prediction/ParticleFilterX_Predict.m: -------------------------------------------------------------------------------- 1 | function [predictedParts] = ParticleFilterX_Predict(f,parts,wk) 2 | % PARTICLEFILTERX_PREDICT Perform the discrete-time PF state prediction step, 3 | % under the assumption of additive process noise. 4 | % 5 | % NOTES: 6 | % * Control inputs are NOT currently supported. 7 | % 8 | % INPUTS: parts - A (xDim x Np) particle matrix from the previous 9 | % time-step. 10 | % f - A (non-linear) state transition function. 11 | % wk - A (xDim x Np) process noise matrix. 12 | % 13 | % OUTPUTS: predictedParts - The (xDim x Np) predicted particle matrix. 14 | % 15 | % October 2017 Lyudmil Vladimirov, University of Liverpool. 16 | 17 | predictedParts = f(parts, wk); % Simply propagate all particles 18 | end -------------------------------------------------------------------------------- /ProjectX/Filters/Particle Filters/ParticleFilterX/ParticleFilterX_Functions/Update/ParticleFilterX_UpdateWeights.m: -------------------------------------------------------------------------------- 1 | function [newWeights] = ParticleFilterX_UpdateWeights(lik,y,parts,weights) 2 | % PARTICLEFILTERX_UPDATEWEIGHTS Perform the discrete-time PF weight update 3 | % step, under the assumption of additive process noise. 4 | % 5 | % INPUTS: lik(y,x) - A measurement likelihood function. 6 | % y - A (yDim x Nm) measurement matrix 7 | % parts - A (xDim x Np) particle matrix. 8 | % weights - A (1 x Np) weights matrix. 9 | % 10 | % OUTPUTS: newWeights - The (1 x Np) updated weights matrix matrix. 11 | % 12 | % October 2017 Lyudmil Vladimirov, University of Liverpool. 13 | 14 | % Update and normalise weights 15 | newWeights = weights.*lik(y,parts); 16 | newWeights = newWeights./sum(newWeights,2); 17 | end 18 | -------------------------------------------------------------------------------- /ProjectX/Filters/Probability Hypothesis Density Filters/SMC_PHDFilterX_Class/SMC_PHDFilterX.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Probability Hypothesis Density Filters/SMC_PHDFilterX_Class/SMC_PHDFilterX.m -------------------------------------------------------------------------------- /ProjectX/Filters/Probability Hypothesis Density Filters/SMC_PHDFilterX_Class/SMC_PHDX.asv: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Filters/Probability Hypothesis Density Filters/SMC_PHDFilterX_Class/SMC_PHDX.asv -------------------------------------------------------------------------------- /ProjectX/Models/Dynamic Models/ConstantHeadingModelX_Class/testCH.m: -------------------------------------------------------------------------------- 1 | config.q_vel = 0.01; 2 | config.q_head = 0.16; 3 | 4 | CHmodel = ConstantHeadingModelX(config); 5 | 6 | CHmodel2 = CHmodel; 7 | 8 | xkm1 = [0; 0; sqrt(2); pi/4]; 9 | pkm1 = [0 1; 0 1; sqrt(2) sqrt(2); pi/4 pi/4]; 10 | Pkm1 = [0 1 0 1; 0 1 0 1; 0 1 0 1; 1 1 0 1]; 11 | xk = CHmodel.sys(1, xkm1,2); 12 | pk = CHmodel.sys(1, pkm1,2); 13 | Pk = CHmodel.sys_cov(1, Pkm1,2); -------------------------------------------------------------------------------- /ProjectX/Models/Dynamic Models/ConstantVelocityModelX_Class/testCV.m: -------------------------------------------------------------------------------- 1 | config.q = 0.01; 2 | config.xDim = 4; 3 | 4 | CVmodel = ConstantVelocityModelX(config); 5 | 6 | xkm1 = [0; 0; 1; 1]; 7 | pkm1 = [zeros(2,1000);ones(2,1000)]; 8 | Pkm1 = [0 1 0 1; 0 1 0 1; 0 1 0 1; 1 1 0 1]; 9 | xk = CVmodel.sys(1, xkm1); 10 | pk = CVmodel.sys(1, pkm1, CVmodel.sys_noise(1, size(pkm1,2))); 11 | Pk = CVmodel.sys_cov(1); 12 | likelihoods = CVmodel.eval(0.1, pk, pkm1); -------------------------------------------------------------------------------- /ProjectX/Models/Dynamic Models/DynamicModelX_ClassTemplate/DynamicModelX.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Models/Dynamic Models/DynamicModelX_ClassTemplate/DynamicModelX.m -------------------------------------------------------------------------------- /ProjectX/Models/Dynamic Models/DynamicModelX_ClassTemplate/testDynModelTemp.m: -------------------------------------------------------------------------------- 1 | Params_dm.nDims = 2; 2 | DynModel = DynamicModelX(Params_dm); 3 | 4 | DynModel.sys('wk',[0.5 1; 0.2 12],'xkm1',ans,'k',1); -------------------------------------------------------------------------------- /ProjectX/Models/Observation Models/LinearGaussianObsModel_Class/testPOM.m: -------------------------------------------------------------------------------- 1 | config.xDim = 4; 2 | config.yDim = 2; 3 | config.r = 0.1; 4 | 5 | model = PositionalObsModelX(config); 6 | 7 | xkm1 = [0.5; 0.3; sqrt(2); pi/4]; 8 | pkm1 = [0 1; 0 1; sqrt(2) sqrt(2); pi/4 pi/4]; 9 | Pkm1 = [0 1 0 1; 0 1 0 1; 0 1 0 1; 1 1 0 1]; 10 | yk = model.obs(1,xkm1); 11 | pk = model.obs_cov(); 12 | Pk = model.eval_likelihood(1,yk,xkm1); -------------------------------------------------------------------------------- /ProjectX/Models/Observation Models/ObservationModelX_ClassTemplate/ObservationModelX.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/ProjectX/Models/Observation Models/ObservationModelX_ClassTemplate/ObservationModelX.m -------------------------------------------------------------------------------- /ProjectX/Models/Observation Models/ObservationModelX_ClassTemplate/testDynModelTemp.m: -------------------------------------------------------------------------------- 1 | Params_dm.nDims = 2; 2 | DynModel = DynamicModelX(Params_dm); 3 | 4 | DynModel.sys('wk',[0.5 1; 0.2 12],'xkm1',ans,'k',1); -------------------------------------------------------------------------------- /RMSE_σ=0.6.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/RMSE_σ=0.6.fig -------------------------------------------------------------------------------- /RMSE_σ=1.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/RMSE_σ=1.fig -------------------------------------------------------------------------------- /Radar Processing/extract_data.m: -------------------------------------------------------------------------------- 1 | DataList = {}; 2 | timestamps = []; 3 | i=1; 4 | j=1; 5 | while i<=size(radar,1) 6 | tempDataList = []; 7 | timestamp = radar(i,7); 8 | z = 1; 9 | if isnan(timestamp) 10 | i = i+1; 11 | continue; 12 | end 13 | 14 | while(i<=size(radar,1)&×tamp==radar(i,7)) 15 | tempDataList(z,:) = radar(i,3:4);%/1000; 16 | i=i+1 17 | z=z+1; 18 | end 19 | DataList{j} = tempDataList'; 20 | timestamps(j) =timestamp; 21 | j=j+1 22 | end 23 | 24 | Dt = timestamps(2:end) - timestamps(1:end-1); 25 | i=1; 26 | while i<=size(Dt,2) 27 | if(Dt(i)==0) 28 | DataList{i-1} = [DataList{i-1},DataList{i}]; 29 | DataList(i) = []; 30 | Dt(i)=[]; 31 | else 32 | i = i+1; 33 | end 34 | end 35 | 36 | 37 | % for i=1:size(DataList,2) 38 | % plot(DataList{i}(:,1), DataList{i}(:,2), 'r*'); 39 | % axis([-2000 -200 -3000 3000]); 40 | % pause(0.1); 41 | % end -------------------------------------------------------------------------------- /Radar Processing/radar.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/Radar Processing/radar.mat -------------------------------------------------------------------------------- /Radar Processing/restrict_data.m: -------------------------------------------------------------------------------- 1 | i=1; 2 | bounding_box = [-1500 500 -1000 500];%[-700 -400 -700 400]%[-2 -.800 2 3];% [-.700 -.400 -.700 .400]; 3 | while i<=size(DataList,2) 4 | j=1; 5 | while j<=size(DataList{i},2) 6 | if DataList{i}(1,j)>bounding_box(2) || DataList{i}(1,j)bounding_box(4) || DataList{i}(2,j) cartesian coordinates (linear units) 12 | % z / 13 | 14 | % Copyright (c) 2011, Michael R. Craymer 15 | % All rights reserved. 16 | % Email: mike@craymer.com 17 | 18 | if nargin ~= 3 19 | warning('Incorrect number of input arguments'); 20 | return 21 | end 22 | 23 | sinv=sin(va); 24 | cosv=cos(va); 25 | sina=sin(az); 26 | cosa=cos(az); 27 | x=d.*cosv.*cosa; 28 | y=d.*cosv.*sina; 29 | z=d.*sinv; 30 | -------------------------------------------------------------------------------- /geodetic/test_in.m: -------------------------------------------------------------------------------- 1 | lat=36.0762253*10; 2 | lon = 24.87468932*10; 3 | alt = 0; 4 | 5 | a = 6378137; 6 | b = 8.1819190842622e-2; 7 | 8 | % intermediate calculation 9 | % (prime vertical radius of curvature) 10 | N = a / sqrt(1 - b^2 * sin(lat)^2); 11 | 12 | % results: 13 | x = (N+alt) * cos(lat) * cos(lon) 14 | y = (N+alt) * cos(lat) * sin(lon) 15 | z = ((1-b^2) * N + alt) * sin(lat) 16 | -------------------------------------------------------------------------------- /geodetic/xyz2sph.m: -------------------------------------------------------------------------------- 1 | function [az,va,d]=xyz2sph(x,y,z) 2 | % XYZ2SPH Converts Cartesian coordinates in a left-handed 3 | % (local geodetic) system to spherical coordinates. 4 | % Vectorized. See also SPH2XYZ. 5 | % Version: 7 Jul 96 6 | % Useage: [az,va,d]=xyz2sph(x,y,z) 7 | % Input: x \ 8 | % y > vectors of cartesian coordinates (linear units) 9 | % z / 10 | % Output: az - vector of azimuths (radians) 11 | % va - vector of vertical angles (radians) 12 | % d - vector of distances (linear units) 13 | 14 | % Copyright (c) 2011, Michael R. Craymer 15 | % All rights reserved. 16 | % Email: mike@craymer.com 17 | 18 | if nargin ~= 3 19 | warning('Incorrect number of input arguments'); 20 | return 21 | end 22 | 23 | d=sqrt(x.*x+y.*y+z.*z); 24 | az=atan2(y,x); 25 | %az=atan(y./x); 26 | %az=az+(x<0)*pi; % 2nd & 3rd quad 27 | %az=az+(az<0)*2*pi; % 4th quad 28 | va=asin(z./d); 29 | -------------------------------------------------------------------------------- /geodetic/yr2jd.m: -------------------------------------------------------------------------------- 1 | function jd=yr2jd(yr); 2 | % YR2JD Converts year and decimal of year to Julian date. 3 | % . Non-vectorized version. See also CAL2JD, DOY2JD, 4 | % GPS2JD, JD2CAL, JD2DOW, JD2DOY, JD2GPS, YR2JD 5 | % Version: 24 Apr 99 6 | % Usage: jd=yr2jd(yr) 7 | % Input: yr - year and decimal of year 8 | % Output: jd - Julian date 9 | 10 | % Copyright (c) 2011, Michael R. Craymer 11 | % All rights reserved. 12 | % Email: mike@craymer.com 13 | 14 | if nargin ~= 1 15 | warning('Incorrect number of arguments'); 16 | return; 17 | end 18 | 19 | iyr = fix(yr); 20 | jd0 = cal2jd(iyr,1,1); 21 | days = cal2jd(iyr+1,1,1) - jd0; 22 | doy = (yr-iyr)*days + 1; 23 | jd = doy2jd(iyr,doy); 24 | -------------------------------------------------------------------------------- /geolife_preproc.py: -------------------------------------------------------------------------------- 1 | from openpyxl import load_workbook 2 | import ecef 3 | import pickle 4 | import json 5 | 6 | wb = load_workbook("C:\Users\sglvladi\Documents\WebSync\University of Liverpool\PhD\resources\geolife.xlsx") 7 | gps_data = wb['Sheet1'] 8 | in_file = open("/home/sglvladi/Mini_Challenge_2/cart_table.txt","r"); 9 | i=1; 10 | for line in in_file: 11 | tokens = line.split(",") 12 | taxi_id = tokens[0] 13 | timestamp = tokens[1] 14 | lat = tokens[2] 15 | long = tokens[3] 16 | gps_data["A%s"%i]=taxi_id 17 | gps_data["B%s"%i]=timestamp 18 | gps_data["C%s"%i]=lat 19 | gps_data["D%s"%i]=long 20 | 21 | -------------------------------------------------------------------------------- /geolife_preproc.txt: -------------------------------------------------------------------------------- 1 | from openpyxl import load_workbook 2 | import ecef 3 | import pickle 4 | import json 5 | 6 | tokens = data.split(" ") 7 | mac_addr = tokens[0] 8 | temp_1 = tokens[2] 9 | temp_2 = tokens[4] 10 | cur_time = datetime.now() -------------------------------------------------------------------------------- /jdbcConfig.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/jdbcConfig.mat -------------------------------------------------------------------------------- /kalmanf/maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/kalmanf/maze.png -------------------------------------------------------------------------------- /kalmanf/sigmas.m: -------------------------------------------------------------------------------- 1 | function X=sigmas(x,P,c) 2 | %Sigma points around reference point 3 | %Inputs: 4 | % x: reference point 5 | % P: covariance 6 | % c: coefficient 7 | %Output: 8 | % X: Sigma points 9 | 10 | A = c*sqrtm(P)'; 11 | Y = x(:,ones(1,numel(x))); 12 | X = [x Y+A Y-A]; -------------------------------------------------------------------------------- /matlab.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/matlab.mat -------------------------------------------------------------------------------- /matlabx2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/matlabx2.mat -------------------------------------------------------------------------------- /maxmin.m: -------------------------------------------------------------------------------- 1 | N = 1e5; 2 | n = 90; 3 | Z = zeros(N,1); 4 | tic 5 | for i = 1:N 6 | X = randn(n,1); 7 | Z(i) = max(X) - min(X); 8 | end 9 | V = var(Z) 10 | toc 11 | -------------------------------------------------------------------------------- /maze.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/maze.png -------------------------------------------------------------------------------- /needle/needle.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/needle/needle.pptx -------------------------------------------------------------------------------- /needle/needlesTandem.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | 4 | N = 1e2; 5 | n = 1e3; 6 | h1 = zeros(N,1); 7 | h2 = h1; 8 | 9 | tic 10 | for i = 1:N 11 | h1(i) = throwNeedles(n,0); 12 | h2(i) = throwNeedles2(n); 13 | end 14 | 15 | subplot(2,1,1) 16 | hist(h1); 17 | title(['Uniform Sampling, Var = ',num2str(var(h1))]) 18 | lims = get(gca,'xlim') 19 | subplot(2,1,2) 20 | hist(h2); 21 | title(['LH Sampling, Var = ',num2str(var(h2))]) 22 | set(gca,'xlim',lims) 23 | toc -------------------------------------------------------------------------------- /needle/throwNeedles.m: -------------------------------------------------------------------------------- 1 | function [m] = throwNeedles(numNeedles,flag) 2 | 3 | X = zeros(numNeedles,1); 4 | Y = X; 5 | Xno = X; 6 | Yno = X; 7 | 8 | inCircle = 0; 9 | for i = 1:numNeedles 10 | x = rand; 11 | y = rand; 12 | if sqrt(x^2+y^2) <= 1; 13 | X(i) = x; 14 | Y(i) = y; 15 | inCircle = inCircle + 1; 16 | else 17 | Xno(i) = x; 18 | Yno(i) = y; 19 | end 20 | end 21 | m = 4*(inCircle/numNeedles); 22 | 23 | if flag == 1 24 | hold on 25 | cx=0:0.025:1; 26 | cy = sqrt(1-(cx.^2)); 27 | figure(1); 28 | plot(cx, cy, 'k','linewidth',2); 29 | title(['Pi estimate = ', num2str(m)]) 30 | 31 | plot(X,Y,'.b') 32 | plot(Xno,Yno,'.r') 33 | 34 | axis square 35 | end 36 | 37 | 38 | -------------------------------------------------------------------------------- /needle/throwNeedles.m~: -------------------------------------------------------------------------------- 1 | function [m] = throwNeedles(numNeedles) 2 | 3 | inCircle = 0; 4 | X = rand(numNeedles,2); 5 | tic 6 | for i = 1:numNeedles 7 | if sqrt(X(i,1)^2+X(i,2)^2) <= 1; 8 | inCircle = inCircle + 1; 9 | end 10 | end 11 | m = 4*(inCircle/numNeedles); 12 | toc 13 | -------------------------------------------------------------------------------- /needle/throwNeedles2.m: -------------------------------------------------------------------------------- 1 | function [m] = throwNeedles2(numNeedles) 2 | 3 | inCircle = 0; 4 | P = lhsdesign(numNeedles,2); 5 | for i = 1:numNeedles 6 | if sqrt(P(i,1)^2+P(i,2)^2) <= 1; 7 | inCircle = inCircle + 1; 8 | end 9 | end 10 | m = 4*(inCircle/numNeedles); 11 | -------------------------------------------------------------------------------- /particlef/untitled.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/particlef/untitled.fig -------------------------------------------------------------------------------- /pollutant.m: -------------------------------------------------------------------------------- 1 | N = 1e2; 2 | C = zeros(N,1); 3 | tic 4 | for i = 1:N 5 | u = log(sqrt(0.1)*randn + 2); 6 | sy = log(sqrt(0.2)*randn + 10); 7 | sx = log(sqrt(0.05)*randn + 5); 8 | 9 | C(i) = 100/(2*pi*u*sqrt(sx*sy)) * exp(-0.5*((40^2/sx) + (100/sy))); 10 | end 11 | 12 | P = prctile(C,95) 13 | toc 14 | -------------------------------------------------------------------------------- /releasecodes/_common/assignmentoptimal.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/_common/assignmentoptimal.mexa64 -------------------------------------------------------------------------------- /releasecodes/_common/assignmentoptimal.mexw32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/_common/assignmentoptimal.mexw32 -------------------------------------------------------------------------------- /releasecodes/_common/assignmentoptimal.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/_common/assignmentoptimal.mexw64 -------------------------------------------------------------------------------- /releasecodes/_common/ekf_predict_multiple.m: -------------------------------------------------------------------------------- 1 | function [m_predict,P_predict] = ekf_predict_multiple(model,m,P) 2 | 3 | plength= size(m,2); 4 | 5 | m_predict = zeros(size(m)); 6 | P_predict = zeros(size(P)); 7 | 8 | for idxp=1:plength 9 | [m_temp,P_temp] = ekf_predict_single(model,m(:,idxp),P(:,:,idxp)); 10 | m_predict(:,idxp) = m_temp; 11 | P_predict(:,:,idxp) = P_temp; 12 | end 13 | 14 | function [m_predict,P_predict] = ekf_predict_single(model,m,P) 15 | 16 | m_predict = gen_newstate_fn(model,m,'noiseless'); 17 | [F_ekf,G_ekf]= ekf_predict_mat(model,m); % user specified function for application 18 | P_predict= G_ekf*model.Q*G_ekf' + F_ekf*P*F_ekf'; 19 | P_predict= (P_predict+P_predict')/2; % addition step to avoid numerical problem -------------------------------------------------------------------------------- /releasecodes/_common/esf.m: -------------------------------------------------------------------------------- 1 | function s = esf(Z) 2 | 3 | %calculate elementary symmetric function using Mahler's recursive formula 4 | 5 | if isempty(Z) 6 | s= 1; 7 | return; 8 | end 9 | 10 | n_z = length(Z); 11 | F = zeros(2,n_z); 12 | 13 | i_n = 1; 14 | i_nminus = 2; 15 | 16 | for n = 1:n_z 17 | F(i_n,1) = F(i_nminus,1) + Z(n); 18 | for k = 2:n 19 | if k==n 20 | F(i_n,k) = Z(n)*F(i_nminus,k-1); 21 | else 22 | F(i_n,k) = F(i_nminus,k) + Z(n)*F(i_nminus,k-1); 23 | end 24 | end 25 | tmp = i_n; 26 | i_n = i_nminus; 27 | i_nminus = tmp; 28 | end 29 | 30 | s= [1; F(i_nminus,:)']; -------------------------------------------------------------------------------- /releasecodes/_common/gate_meas_ekf.m: -------------------------------------------------------------------------------- 1 | function z_gate= gate_meas_ekf(z,gamma,model,m,P) 2 | 3 | valid_idx = []; 4 | zlength = size(z,2); 5 | plength = size(m,2); 6 | 7 | for j=1:plength 8 | [C_ekf,U_ekf]= ekf_update_mat(model,m(:,j)); 9 | Sj= U_ekf*model.R*U_ekf' + C_ekf*P(:,:,j)*C_ekf'; Sj= (Sj+ Sj')/2; 10 | Vs= chol(Sj); det_Sj= prod(diag(Vs))^2; inv_sqrt_Sj= inv(Vs); 11 | iSj= inv_sqrt_Sj*inv_sqrt_Sj'; 12 | nu= z- repmat(gen_observation_fn(model,m(:,j),zeros(size(model.D,2),1)),[1 zlength]); 13 | dist= sum((inv_sqrt_Sj'*nu).^2); 14 | valid_idx= union(valid_idx,find( dist < gamma )); 15 | end 16 | z_gate = z(:,valid_idx); -------------------------------------------------------------------------------- /releasecodes/_common/gate_meas_gms.m: -------------------------------------------------------------------------------- 1 | function z_gate= gate_meas_gms(z,gamma,model,m,P) 2 | 3 | valid_idx = []; 4 | zlength = size(z,2); 5 | plength = size(m,2); 6 | 7 | for j=1:plength 8 | Sj= model.R + model.H*P(:,:,j)*model.H'; 9 | Vs= chol(Sj); det_Sj= prod(diag(Vs))^2; inv_sqrt_Sj= inv(Vs); 10 | iSj= inv_sqrt_Sj*inv_sqrt_Sj'; 11 | nu= z- model.H*repmat(m(:,j),[1 zlength]); 12 | dist= sum((inv_sqrt_Sj'*nu).^2); 13 | valid_idx= union(valid_idx,find( dist < gamma )); 14 | end 15 | z_gate = z(:,valid_idx); -------------------------------------------------------------------------------- /releasecodes/_common/gate_meas_ukf.m: -------------------------------------------------------------------------------- 1 | function z_gate= gate_meas_ukf(z,gamma,model,m,P,alpha,kappa,beta) 2 | 3 | valid_idx = []; 4 | zlength = size(z,2); 5 | plength = size(m,2); 6 | 7 | for j=1:plength 8 | [X_ukf,u]= ut( [m(:,j); zeros(model.w_dim,1) ], blkdiag(P(:,:,j),model.R), alpha, kappa ); 9 | Z_pred= gen_observation_fn( model, X_ukf(1:model.x_dim,:), X_ukf((model.x_dim+1):(model.x_dim+model.w_dim),:) ); 10 | eta= Z_pred*u(:); Sj_temp= Z_pred- repmat(eta,[1 length(u)]); u(1)= u(1)+(1-alpha^2+beta); 11 | Sj= Sj_temp*diag(u)*Sj_temp'; 12 | Vs= chol(Sj); det_Sj= prod(diag(Vs))^2; inv_sqrt_Sj= inv(Vs); 13 | iSj= inv_sqrt_Sj*inv_sqrt_Sj'; 14 | nu= z- repmat(gen_observation_fn(model,m(:,j),zeros(size(model.D,2),1)),[1 zlength]); 15 | dist= sum((inv_sqrt_Sj'*nu).^2); 16 | valid_idx= union(valid_idx,find( dist < gamma )); 17 | end 18 | z_gate = z(:,valid_idx); -------------------------------------------------------------------------------- /releasecodes/_common/gaus_cap.m: -------------------------------------------------------------------------------- 1 | function [w_new,x_new,P_new]= gaus_cap(w,x,P,max_number) 2 | 3 | if length(w) > max_number 4 | [notused,idx]= sort(w,1,'descend'); 5 | w_new= w(idx(1:max_number)); w_new = w_new * (sum(w)/sum(w_new)); 6 | x_new= x(:,idx(1:max_number)); 7 | P_new= P(:,:,idx(1:max_number)); 8 | else 9 | x_new = x; 10 | P_new = P; 11 | w_new = w; 12 | end 13 | -------------------------------------------------------------------------------- /releasecodes/_common/gaus_cap_1.m: -------------------------------------------------------------------------------- 1 | function [w_new,x_new,P_new,u_new,v_new]= gaus_cap_1(w,x,P,u,v,max_number) 2 | 3 | if length(w) > max_number 4 | [notused,idx]= sort(w,1,'descend'); 5 | w_new= w(idx(1:max_number)); w_new = w_new * (sum(w)/sum(w_new)); 6 | x_new= x(:,idx(1:max_number)); 7 | P_new= P(:,:,idx(1:max_number)); 8 | u_new= u(idx(1:max_number)); 9 | v_new= v(idx(1:max_number)); 10 | else 11 | x_new = x; 12 | P_new = P; 13 | w_new = w; 14 | u_new = u; 15 | v_new = v; 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/_common/gaus_cap_3.m: -------------------------------------------------------------------------------- 1 | function [w_new,x_new,P_new,u_new,v_new,wc_new,uc_new,vc_new]= gaus_cap_3(w,x,P,u,v,wc,uc,vc,max_number) 2 | 3 | if length(w) > max_number 4 | [notused,idx1]= sort(w,1,'descend'); 5 | w_new= w(idx1(1:max_number)); w_new = w_new * (sum(w)/sum(w_new)); 6 | x_new= x(:,idx1(1:max_number)); 7 | P_new= P(:,:,idx1(1:max_number)); 8 | u_new= u(idx1(1:max_number)); 9 | v_new= v(idx1(1:max_number)); 10 | else 11 | x_new = x; 12 | P_new = P; 13 | w_new = w; 14 | u_new = u; 15 | v_new = v; 16 | end 17 | if length(wc) > max_number 18 | [notused,idx2]= sort(wc,1,'descend'); 19 | wc_new= wc(idx2(1:max_number)); wc_new = wc_new * (sum(wc)/sum(wc_new)); 20 | uc_new= uc(idx2(1:max_number)); 21 | vc_new= vc(idx2(1:max_number)); 22 | else 23 | wc_new= wc; 24 | uc_new = uc; 25 | vc_new = vc; 26 | end 27 | -------------------------------------------------------------------------------- /releasecodes/_common/gaus_prune.m: -------------------------------------------------------------------------------- 1 | function [w_new,x_new,P_new]= gaus_prune(w,x,P,elim_threshold) 2 | 3 | idx= find( w > elim_threshold ); 4 | w_new= w(idx); 5 | x_new= x(:,idx); 6 | P_new= P(:,:,idx); -------------------------------------------------------------------------------- /releasecodes/_common/gaus_prune_1.m: -------------------------------------------------------------------------------- 1 | function [w_new,x_new,P_new,u_new,v_new]= gaus_prune_1(w,x,P,u,v,elim_threshold) 2 | 3 | idx= find( w > elim_threshold ); 4 | w_new= w(idx); 5 | x_new= x(:,idx); 6 | P_new= P(:,:,idx); 7 | u_new= u(idx); 8 | v_new= v(idx); -------------------------------------------------------------------------------- /releasecodes/_common/gaus_prune_3.m: -------------------------------------------------------------------------------- 1 | function [w_new,x_new,P_new,u_new,v_new,wc_new,uc_new,vc_new]= gaus_prune_3(w,x,P,u,v,wc,uc,vc,elim_threshold) 2 | 3 | idx1= find( w > elim_threshold ); 4 | w_new= w(idx1); 5 | x_new= x(:,idx1); 6 | P_new= P(:,:,idx1); 7 | u_new= u(idx1); 8 | v_new= v(idx1); 9 | 10 | idx2= find(wc > elim_threshold); 11 | wc_new= wc(idx2); 12 | uc_new= uc(idx2); 13 | vc_new= vc(idx2); -------------------------------------------------------------------------------- /releasecodes/_common/gen_gms.m: -------------------------------------------------------------------------------- 1 | function X= gen_gms(w,m,P,num_par) 2 | % generate samples from Gaussian mixture intensity 3 | 4 | x_dim=size(m,1); 5 | X=zeros(x_dim,num_par); 6 | 7 | w= w/sum(w); 8 | w= sort(w,'descend'); 9 | 10 | nc= length(w); 11 | 12 | comps= randsample(1:nc,num_par,true,w); 13 | 14 | ns= zeros(nc,1); 15 | for c=1:nc 16 | ns(c)= nnz(comps==c); 17 | end 18 | 19 | startpt= 1; 20 | for i=1:nc 21 | endpt= startpt+ns(i)-1; 22 | X(:,startpt:endpt)= gen_mvs(m(:,i),P(:,:,i),ns(i)); 23 | startpt= endpt+1; 24 | end 25 | 26 | function X= gen_mvs(m,P,ns) 27 | 28 | U= chol(P); X= repmat(m,[1 ns]) + U*randn(length(m),ns); 29 | 30 | 31 | -------------------------------------------------------------------------------- /releasecodes/_common/hist2d.m: -------------------------------------------------------------------------------- 1 | function [Z,x,y]= hist2d(C,no_of_bins,limit) 2 | % compute histogram for 2D data 3 | % C- input data with dimension 2 x data_len 4 | % no_of_bins= [ no of bins for x-axis, no of bins for y-axis ] 5 | % limit= [ lower limit for x-axis, upper limit for x-axis 6 | % lower limit for y-axis, upper limit for y-axis 7 | 8 | data_len= size(C,2); 9 | Z= sparse(no_of_bins(1),no_of_bins(2)); 10 | 11 | [point_list,value_list,scan_points]= ... 12 | histnd(C,no_of_bins,limit); 13 | Z= sparse(point_list(1,:),point_list(2,:),value_list,no_of_bins(1),no_of_bins(2)); 14 | x= scan_points{1}; y= scan_points{2}; 15 | -------------------------------------------------------------------------------- /releasecodes/_common/kalman_predict_multiple.m: -------------------------------------------------------------------------------- 1 | function [m_predict,P_predict] = kalman_predict_multiple(model,m,P) 2 | 3 | plength= size(m,2); 4 | 5 | m_predict = zeros(size(m)); 6 | P_predict = zeros(size(P)); 7 | 8 | for idxp=1:plength 9 | [m_temp,P_temp] = kalman_predict_single(model.F,model.Q,m(:,idxp),P(:,:,idxp)); 10 | m_predict(:,idxp) = m_temp; 11 | P_predict(:,:,idxp) = P_temp; 12 | end 13 | 14 | function [m_predict,P_predict] = kalman_predict_single(F,Q,m,P) 15 | 16 | m_predict = F*m; 17 | P_predict = Q+F*P*F'; 18 | -------------------------------------------------------------------------------- /releasecodes/_common/logsumexp.m: -------------------------------------------------------------------------------- 1 | function logsum = logsumexp(w) 2 | 3 | %performs log-sum-exp trick to avoid numerical underflow 4 | %input: w weight vector assumed already log transformed 5 | %output: log(sum(exp(w))) 6 | 7 | if all(w==-inf) 8 | logsum= -inf; 9 | return; 10 | end 11 | 12 | [val,idx] = max(w); 13 | logsum = log(sum(exp(w-val))) + val; 14 | 15 | -------------------------------------------------------------------------------- /releasecodes/_common/mbestwrap_updt_custom.m: -------------------------------------------------------------------------------- 1 | function [assignments,costs]= murty_wrapper_update(P0,m) 2 | 3 | if m==0 4 | assignments= []; 5 | costs= []; 6 | return; 7 | end 8 | 9 | n1 = size(P0,1); 10 | n2 = size(P0,2); 11 | 12 | % Padding blocks for dummy variables 13 | blk1 = -log(ones(n1,n1)); 14 | 15 | P0 = [P0 blk1]; 16 | 17 | % Make costs non-negative (required by 'assignmentoptimal') 18 | x = min(min(P0)); 19 | P0 = P0 - x; 20 | 21 | % Murty 22 | [assignments, costs] = murty_custom(P0,m); 23 | 24 | % Restore correct costs to assignments 25 | costs = costs + (x.*sum(assignments>0,2))'; 26 | 27 | % Strip dummy variables 28 | assignments = assignments(:,1:n1); 29 | 30 | % Dummy assignments are clutter 31 | assignments(assignments>n2) = 0; 32 | 33 | 34 | end 35 | -------------------------------------------------------------------------------- /releasecodes/_common/ospa_dist.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/_common/ospa_dist.m -------------------------------------------------------------------------------- /releasecodes/_common/resample.m: -------------------------------------------------------------------------------- 1 | function resample_idx= resample(w,L) 2 | % particles resampling 3 | % w- weights with sum(w)= 1 4 | % L- no. of samples desired 5 | % resample_idx- indices for the resampled particles 6 | 7 | w=w/sum(w); 8 | resample_idx= []; 9 | [notused,sort_idx]= sort(-w); %sort in descending order 10 | rv= rand(L,1); 11 | i= 0; threshold= 0; 12 | while ~isempty(rv), 13 | i= i+1; threshold= threshold+ w(sort_idx(i)); 14 | rv_len= length(rv); 15 | idx= find(rv>threshold); 16 | resample_idx= [ resample_idx; sort_idx(i)*ones(rv_len-length(idx),1) ]; 17 | rv= rv(idx); 18 | end; -------------------------------------------------------------------------------- /releasecodes/_common/ukf_predict_multiple.m: -------------------------------------------------------------------------------- 1 | function [m_predict,P_predict] = ukf_predict_multiple(model,m,P,alpha,kappa,beta) 2 | 3 | plength= size(m,2); 4 | 5 | m_predict = zeros(size(m)); 6 | P_predict = zeros(size(P)); 7 | 8 | for idxp=1:plength 9 | [m_temp,P_temp] = ukf_predict_single(model,m(:,idxp),P(:,:,idxp),alpha,kappa,beta); 10 | m_predict(:,idxp) = m_temp; 11 | P_predict(:,:,idxp) = P_temp; 12 | end 13 | 14 | function [m_predict,P_predict] = ukf_predict_single(model,m,P,alpha,kappa,beta) 15 | 16 | [X_ukf,u]= ut( [m; zeros(model.v_dim,1) ], blkdiag(P,model.Q), alpha, kappa ); 17 | X_pred= gen_newstate_fn( model, X_ukf(1:model.x_dim,:), X_ukf((model.x_dim+1):(model.x_dim+model.v_dim),:) ); 18 | 19 | m_predict = X_pred*u(:); 20 | X_temp= X_pred- repmat(m_predict,[1 length(u)]); 21 | u(1)= u(1)+(1-alpha^2+beta); 22 | P_predict= X_temp*diag(u)*X_temp'; 23 | -------------------------------------------------------------------------------- /releasecodes/_common/ut.m: -------------------------------------------------------------------------------- 1 | function [X,w]= ut(m,P,alpha,kappa) 2 | 3 | % standard unscented transformation - note [choose alpha=1 => =lambda=kappa; giving offset of beta for first cov weight] 4 | 5 | n_x= length(m); 6 | lambda= alpha^2*(n_x+kappa) - n_x; 7 | Psqrtm= chol((n_x+lambda)*P)'; 8 | X= repmat(m,[1 2*n_x+1])+ [ zeros(n_x,1) -Psqrtm Psqrtm ]; 9 | w= [ lambda 0.5*ones(1,2*n_x) ]/(n_x+lambda); 10 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/bernoulli/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/bernoulli/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/bernoulli/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([1 3],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | max= 0.98; 7 | mid= [0; 0]; 8 | cov= diag([2000,2000].^2); 9 | 10 | M= size(X,2); 11 | P= X([1 3],:); 12 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | 14 | pD= max*exp(-e_sq/2); 15 | pD= pD(:); 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/smc/compute_pS.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | pS= pS(:); 8 | end 9 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k}) ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/bernoulli/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/bernoulli/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/cbmember/ekf/demo.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/cbmember/ekf/demo.m -------------------------------------------------------------------------------- /releasecodes/cbmember/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/cbmember/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/cbmember/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/cbmember/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/cbmember/ekf/run_filter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/cbmember/ekf/run_filter.m -------------------------------------------------------------------------------- /releasecodes/cbmember/gms/demo.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/cbmember/gms/demo.m -------------------------------------------------------------------------------- /releasecodes/cbmember/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/cbmember/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/cbmember/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/cbmember/gms/run_filter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/cbmember/gms/run_filter.m -------------------------------------------------------------------------------- /releasecodes/cbmember/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([1 3],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/cbmember/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | max= 0.98; 7 | mid= [0; 0]; 8 | cov= diag([2000,2000].^2); 9 | 10 | M= size(X,2); 11 | P= X([1 3],:); 12 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | 14 | pD= max*exp(-e_sq/2); 15 | pD= pD(:); 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/cbmember/smc/compute_pS.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | pS= pS(:); 8 | end 9 | -------------------------------------------------------------------------------- /releasecodes/cbmember/smc/demo.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/cbmember/smc/demo.m -------------------------------------------------------------------------------- /releasecodes/cbmember/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k}) ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/cbmember/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/cbmember/smc/run_filter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/cbmember/smc/run_filter.m -------------------------------------------------------------------------------- /releasecodes/cbmember/ukf/demo.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/cbmember/ukf/demo.m -------------------------------------------------------------------------------- /releasecodes/cbmember/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/cbmember/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/cbmember/ukf/run_filter.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/releasecodes/cbmember/ukf/run_filter.m -------------------------------------------------------------------------------- /releasecodes/cphd/ekf/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the GMCPHD filter proposed in 2 | % (assuming Poisson clutter) 3 | % B.-T. Vo, B.-N. Vo and A. Cantoni, "Analytic implementations of the Cardinalized Probability Hypothesis Density Filter," IEEE Trans Signal Processing, Vol. 55, No. 7, part 2, pp. 3553-3567, 2007. 4 | % http://ba-ngu.vo-au.com/vo/VVC_CPHD_SP07.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{GMCPHD, 7 | % author={B.-T. Vo and B.-N. Vo and A. Cantoni}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={Analytic Implementations of the Cardinalized Probability Hypothesis Density Filter}, 10 | % year={2007}, 11 | % month={July}, 12 | % volume={55}, 13 | % number={7}, 14 | % pages={3553-3567}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/cphd/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/cphd/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/cphd/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/cphd/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/cphd/gms/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the GMCPHD filter proposed in 2 | % (assuming Poisson clutter) 3 | % B.-T. Vo, B.-N. Vo and A. Cantoni, "Analytic implementations of the Cardinalized Probability Hypothesis Density Filter," IEEE Trans Signal Processing, Vol. 55, No. 7, part 2, pp. 3553-3567, 2007. 4 | % http://ba-ngu.vo-au.com/vo/VVC_CPHD_SP07.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{GMCPHD, 7 | % author={B.-T. Vo and B.-N. Vo and A. Cantoni}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={Analytic Implementations of the Cardinalized Probability Hypothesis Density Filter}, 10 | % year={2007}, 11 | % month={July}, 12 | % volume={55}, 13 | % number={7}, 14 | % pages={3553-3567}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/cphd/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/cphd/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/cphd/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/cphd/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([1 3],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/cphd/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | max= 0.98; 7 | mid= [0; 0]; 8 | cov= diag([2000,2000].^2); 9 | 10 | M= size(X,2); 11 | P= X([1 3],:); 12 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | 14 | pD= max*exp(-e_sq/2); 15 | end 16 | -------------------------------------------------------------------------------- /releasecodes/cphd/smc/compute_pS.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | end 8 | -------------------------------------------------------------------------------- /releasecodes/cphd/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k})'); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/cphd/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/cphd/ukf/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the GMCPHD filter proposed in 2 | % (assuming Poisson clutter) 3 | % B.-T. Vo, B.-N. Vo and A. Cantoni, "Analytic implementations of the Cardinalized Probability Hypothesis Density Filter," IEEE Trans Signal Processing, Vol. 55, No. 7, part 2, pp. 3553-3567, 2007. 4 | % http://ba-ngu.vo-au.com/vo/VVC_CPHD_SP07.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{GMCPHD, 7 | % author={B.-T. Vo and B.-N. Vo and A. Cantoni}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={Analytic Implementations of the Cardinalized Probability Hypothesis Density Filter}, 10 | % year={2007}, 11 | % month={July}, 12 | % volume={55}, 13 | % number={7}, 14 | % pages={3553-3567}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/cphd/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/cphd/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/glmb/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/glmb/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/glmb/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/glmb/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/glmb/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/glmb/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/glmb/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/glmb/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([1 3],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/glmb/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | max= 0.98; 7 | mid= [0; 0]; 8 | cov= diag([2000,2000].^2); 9 | 10 | M= size(X,2); 11 | P= X([1 3],:); 12 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | 14 | pD= max*exp(-e_sq/2); 15 | pD= pD(:); 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/glmb/smc/compute_pS.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | pS= pS(:); 8 | end 9 | -------------------------------------------------------------------------------- /releasecodes/glmb/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k}) ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/glmb/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/glmb/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/glmb/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/lmb/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/lmb/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/lmb/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/lmb/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/lmb/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/lmb/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/lmb/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/lmb/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([1 3],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/lmb/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | max= 0.98; 7 | mid= [0; 0]; 8 | cov= diag([2000,2000].^2); 9 | 10 | M= size(X,2); 11 | P= X([1 3],:); 12 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | 14 | pD= max*exp(-e_sq/2); 15 | pD= pD(:); 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/lmb/smc/compute_pS.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | pS= pS(:); 8 | end 9 | -------------------------------------------------------------------------------- /releasecodes/lmb/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k}) ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/lmb/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/lmb/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/lmb/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/phd/ekf/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the GM-PHD filter proposed in 2 | % (assuming no target spawning) 3 | % B.-N. Vo, and W. K. Ma, "The Gaussian mixture Probability Hypothesis Density Filter," IEEE Trans Signal Processing, Vol. 54, No. 11, pp. 4091-4104, 2006. 4 | % http://ba-ngu.vo-au.com/vo/VM_GMPHD_SP06.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{GMPHD, 7 | % author={B.-N. Vo and W.-K. Ma}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={The Gaussian Mixture Probability Hypothesis Density Filter}, 10 | % year={2006}, 11 | % month={Nov}, 12 | % volume={54}, 13 | % number={11}, 14 | % pages={4091-4104}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/phd/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/phd/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/phd/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/phd/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/phd/gms/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the GM-PHD filter proposed in 2 | % (assuming no target spawning) 3 | % B.-N. Vo, and W. K. Ma, "The Gaussian mixture Probability Hypothesis Density Filter," IEEE Trans Signal Processing, Vol. 54, No. 11, pp. 4091-4104, 2006. 4 | % http://ba-ngu.vo-au.com/vo/VM_GMPHD_SP06.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{GMPHD, 7 | % author={B.-N. Vo and W.-K. Ma}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={The Gaussian Mixture Probability Hypothesis Density Filter}, 10 | % year={2006}, 11 | % month={Nov}, 12 | % volume={54}, 13 | % number={11}, 14 | % pages={4091-4104}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/phd/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/phd/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/phd/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/phd/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([1 3],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/phd/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | max= 0.98; 7 | mid= [0; 0]; 8 | cov= diag([2000,2000].^2); 9 | 10 | M= size(X,2); 11 | P= X([1 3],:); 12 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | 14 | pD= max*exp(-e_sq/2); 15 | pD= pD(:); 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/phd/smc/compute_pS.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | pS= pS(:); 8 | end 9 | -------------------------------------------------------------------------------- /releasecodes/phd/smc/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the SMC-PHD filter proposed in 2 | % (assuming no target spawning) 3 | % B.-N. Vo, S. Singh and A. Doucet, "Sequential Monte Carlo methods for Bayesian Multi-target filtering with Random Finite Sets," IEEE Trans. Aerospace and Electronic Systems, Vol. 41, No. 4, pp. 1224-1245, 2005. 4 | % http://ba-ngu.vo-au.com/vo/VSD_SMCRFS_AES05.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{SMCRFS, 7 | % author={B.-N. Vo and S. Singh and A. Doucet}, 8 | % journal={IEEE Transactions on Aerospace and Electronic Systems}, 9 | % title={Sequential Monte Carlo methods for multitarget filtering with random finite sets}, 10 | % year={2005}, 11 | % month={Oct}, 12 | % volume={41}, 13 | % number={4}, 14 | % pages={1224-1245}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/phd/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k}) ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/phd/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/phd/ukf/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the GM-PHD filter proposed in 2 | % (assuming no target spawning) 3 | % B.-N. Vo, and W. K. Ma, "The Gaussian mixture Probability Hypothesis Density Filter," IEEE Trans Signal Processing, Vol. 54, No. 11, pp. 4091-4104, 2006. 4 | % http://ba-ngu.vo-au.com/vo/VM_GMPHD_SP06.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{GMPHD, 7 | % author={B.-N. Vo and W.-K. Ma}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={The Gaussian Mixture Probability Hypothesis Density Filter}, 10 | % year={2006}, 11 | % month={Nov}, 12 | % volume={54}, 13 | % number={11}, 14 | % pages={4091-4104}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/phd/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/phd/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood_tg(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([2 4],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/compute_likelihood_clt.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood_clt(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([2 4],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D_clt))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D_clt)))); 13 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/compute_likelihood_tg.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood_tg(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([2 4],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | max= 0.98; 7 | mid= [0; 0]; 8 | cov= diag([2000,2000].^2); 9 | 10 | M= size(X,2); 11 | P= X([1 3],:); 12 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | 14 | pD= max*exp(-e_sq/2); 15 | pD= pD(:); 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/compute_pS.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | pS= pS(:); 8 | end 9 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/compute_pS_clt.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS_clt(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.90*ones(size(X,2),1); 7 | pS= pS(:); 8 | end 9 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/compute_pS_tg.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS_tg(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | pS= pS(:); 8 | end 9 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/demo.m: -------------------------------------------------------------------------------- 1 | % This is the demo script for the Robust CBMeMBer filter proposed in 2 | % (without track labelling) 3 | % B.-T. Vo, B.-N. Vo, R. Hoseinnezhad, and R. Mahler "Robust Multi-Bernoulli Filtering," IEEE Journal on Selected Topics in Signal Processing, Vol. 7, No. 3, pp. 399-409, 2013. 4 | % http://ba-ngu.vo-au.com/vo/VVHM_JSSP13.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{RobustCBMEMBER, 7 | % author={B.-T. Vo and B.-N. Vo and R. Hoseinnezhad and R. Mahler}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={Robust Multi-Bernoulli Filtering}, 10 | % year={2013}, 11 | % month={Jun}, 12 | % volume={7}, 13 | % number={3}, 14 | % pages={399-409}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k}) ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= binornd(model.clutter_N_T,model.clutter_P_D,1,1); 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/gen_newstate_clt.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_clt(model,X_old) 2 | %--- this new state generation function follows the linear state spc eqn. 3 | % x= Ax_old + Bv 4 | 5 | if isempty(X_old), 6 | X= []; 7 | else 8 | mu= X_old(1,:); mu=min(mu,0.999); mu=max(mu,0.001); X_old(1,:)= mu; 9 | sig= min(0.9*mu.*(1-mu),model.pdvarfac_clt); 10 | X= gen_newstate_fn_clt(model,X_old,betarnd((mu.*(1-mu)/sig -1).*mu,(mu.*(1-mu)/sig -1).*(1-mu))-mu, mvnrnd(zeros(size(X_old,1)-1,1)',model.Q_clt,size(X_old,2))'); 11 | end; -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/gen_newstate_fn_clt.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn_clt(model,X_old,S,V) 2 | %--- this new state generation function is the random walk model 3 | %but sets the velocity and turn rate components to zero. 4 | 5 | if isempty(X_old), 6 | X= []; return; 7 | end; 8 | 9 | X= X_old+ [S;V]; 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/gen_newstate_tg.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_tg(model,X_old) 2 | %--- this new state generation function follows the linear state spc eqn. 3 | % x= Ax_old + Bv 4 | 5 | if isempty(X_old), 6 | X= []; 7 | else 8 | mu= X_old(1,:); mu=min(mu,0.999); mu=max(mu,0.001); X_old(1,:)= mu; 9 | sig= min(0.9*mu.*(1-mu),model.pdvarfac_tg); 10 | X= gen_newstate_fn_tg(model,X_old,betarnd((mu.*(1-mu)./sig -1).*mu,(mu.*(1-mu)./sig -1).*(1-mu))-mu, model.B*randn(size(model.B,2),size(X_old,2))); 11 | end; -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/gen_observation_clt.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X) 2 | % this observation generation function is for coordinate 3 | % measurements 4 | 5 | if isempty(X), 6 | Z= []; 7 | else 8 | Z= gen_observation_fn_clt( model, X, model.D_clt*randn(size(model.D_clt,2),size(X,2)) ); %coordinate extraction 9 | end; -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([2 4],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/jointcbmember/smc/gen_observation_tgt.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X) 2 | % this observation generation function is for coordinate 3 | % measurements 4 | 5 | if isempty(X), 6 | Z= []; 7 | else 8 | Z= gen_observation_fn_tgt( model, X, model.D_tgt*randn(size(model.D_tgt,2),size(X,2)) ); %coordinate extraction 9 | end; -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= binornd(model.clutter_Nt,model.clutter_P_D,1,1); 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= binornd(model.clutter_Nt,model.clutter_P_D,1,1); 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/compute_likelihood_clt.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood_clt(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([2 4],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D_c))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D_c)))); 13 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/compute_likelihood_tg.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood_tg(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([2 4],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | % max= 0.98; 7 | % mid= [0; 0]; 8 | % cov= diag([2000,2000].^2); 9 | % 10 | % M= size(X,2); 11 | % P= X([1 3],:); 12 | % e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | % 14 | % pD= max*exp(-e_sq/2); 15 | pD= 0.98*ones(1,size(X,2)); 16 | end 17 | 18 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/compute_pS_clt.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS_clt(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.5*ones(size(X,2),1); 7 | end 8 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/compute_pS_tg.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS_tg(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | end 8 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k})'); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= binornd(model.clutter_Nt,model.clutter_P_D,1,1); 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/gen_newstate_clt.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_clt(model,X_old) 2 | %--- this new state generation function follows the linear state spc eqn. 3 | % x= Ax_old + Bv 4 | 5 | if isempty(X_old), 6 | X= []; 7 | else 8 | mu= X_old(1,:); mu=min(mu,0.999); mu=max(mu,0.001); X_old(1,:)= mu; 9 | sig= min(0.9*mu.*(1-mu),model.pdvarfac_c); 10 | X= gen_newstate_fn_clt(model,X_old,betarnd((mu.*(1-mu)/sig -1).*mu,(mu.*(1-mu)/sig -1).*(1-mu))-mu, mvnrnd(zeros(size(X_old,1)-1,1)',model.Q_c,size(X_old,2))'); 11 | end; -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/gen_newstate_fn_clt.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn_clt(model,X_old,S,V) 2 | %--- this new state generation function is the random walk model 3 | %but sets the velocity and turn rate components to zero. 4 | 5 | if isempty(X_old), 6 | X= []; return; 7 | end; 8 | 9 | X= X_old+ [S;V]; 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/gen_newstate_tg.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_tg(model,X_old) 2 | %--- this new state generation function follows the linear state spc eqn. 3 | % x= Ax_old + Bv 4 | 5 | if isempty(X_old), 6 | X= []; 7 | else 8 | mu= X_old(1,:); mu=min(mu,0.999); mu=max(mu,0.001); X_old(1,:)= mu; 9 | sig= min(0.9*mu.*(1-mu),model.pdvarfac_tg); 10 | X= gen_newstate_fn_tg(model,X_old,betarnd((mu.*(1-mu)./sig -1).*mu,(mu.*(1-mu)./sig -1).*(1-mu))-mu, model.B*randn(size(model.B,2),size(X_old,2))); 11 | end; -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/jointcphd/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= binornd(model.clutter_Nt,model.clutter_P_D,1,1); 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= binornd(model.clutter_Nt,model.clutter_P_D,1,1); 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([1 3],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | % max= 0.98; 7 | % mid= [0; 0]; 8 | % cov= diag([2000,2000].^2); 9 | % 10 | % M= size(X,2); 11 | % P= X([1 3],:); 12 | % e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | % 14 | % pD= max*exp(-e_sq/2); 15 | pD= 0.98*ones(1,size(X,2)); 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/smc/compute_pS.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | end 8 | -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k})'); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= binornd(model.clutter_Nt,model.clutter_P_D,1,1); 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= binornd(model.clutter_Nt,model.clutter_P_D,1,1); 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/lcphd/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([2 4],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/smc/compute_likelihood_tg.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood_tg(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([2 4],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | max= 0.98; 7 | mid= [0; 0]; 8 | cov= diag([6000,6000].^2); 9 | 10 | M= size(X,2); 11 | P= X([1 3],:); 12 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | 14 | pD= max*exp(-e_sq/2); 15 | %pD= 0.98*ones(1,size(X,2)); 16 | end 17 | 18 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/smc/compute_pS.m: -------------------------------------------------------------------------------- 1 | function pS = compute_pS(model,X) 2 | 3 | if isempty(X) 4 | pS= []; 5 | else 6 | pS= 0.99*ones(size(X,2),1); 7 | end 8 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k})'); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/smc/gen_newstate_tg.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_tg(model,X_old) 2 | %--- this new state generation function follows the linear state spc eqn. 3 | % x= Ax_old + Bv 4 | 5 | if isempty(X_old), 6 | X= []; 7 | else 8 | mu= X_old(1,:); mu=min(mu,0.999); mu=max(mu,0.001); X_old(1,:)= mu; 9 | sig= min(0.9*mu.*(1-mu),model.pdvarfac_tg); 10 | X= gen_newstate_fn_tg(model,X_old,betarnd((mu.*(1-mu)./sig -1).*mu,(mu.*(1-mu)./sig -1).*(1-mu))-mu, model.B*randn(size(model.B,2),size(X_old,2))); 11 | end; -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/robust/pdcphd/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/singletarget/ekf/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the single target filter with RFS observations proposed in 2 | % (assuming Poisson clutter and no extraneous measurements) 3 | % B.-T. Vo, B.-N. Vo, and A. Cantoni, "Bayesian filtering with random finite set observations," IEEE Trans. Signal Processing, Vol. 56, No. 4, pp. 1313-1326, 2008. 4 | % http://ba-ngu.vo-au.com/vo/VVCsingletargetSP08.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{STF, 7 | % author={B.-T.Vo and B.-N. Vo and A. Cantoni}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={Bayesian Filtering With Random Finite Set Observations}, 10 | % year={2008}, 11 | % month={April}, 12 | % volume={56}, 13 | % number={4}, 14 | % pages={1313-1326}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/singletarget/ekf/ekf_predict_mat.m: -------------------------------------------------------------------------------- 1 | function [F,G]= ekf_predict_mat(model,mu_old) 2 | 3 | tol= 1e-6; 4 | omega= mu_old(5); 5 | 6 | T= model.T; 7 | sin_omega_T= sin(omega*T); 8 | cos_omega_T= cos(omega*T); 9 | 10 | a= T; b= 0; 11 | if abs(omega) > tol, 12 | a= sin_omega_T/omega; 13 | b= (1-cos_omega_T)/omega; 14 | end; 15 | A= [ 1 a 0 -b; ... 16 | 0 cos_omega_T 0 -sin_omega_T; ... 17 | 0 b 1 a; ... 18 | 0 sin_omega_T 0 cos_omega_T ]; 19 | 20 | c= 0; d= T^2/2; 21 | if abs(omega) > tol, 22 | c= (omega*T*cos_omega_T - sin_omega_T )/omega^2; 23 | d= ( omega*T*sin_omega_T - 1 + cos_omega_T )/omega^2; 24 | end; 25 | dA= [ 0 c 0 -d; ... 26 | 0 -T*sin_omega_T 0 -T*cos_omega_T; ... 27 | 0 d 0 c; ... 28 | 0 T*cos_omega_T 0 -T*sin_omega_T ]; 29 | 30 | F= [ A dA*mu_old(1:4); ... 31 | zeros(1,4) 1 ]; 32 | 33 | G= model.B2; 34 | 35 | 36 | -------------------------------------------------------------------------------- /releasecodes/singletarget/ekf/ekf_update_mat.m: -------------------------------------------------------------------------------- 1 | function [H,U]= ekf_update_mat(model,mu) 2 | 3 | p= mu([1 3],:); 4 | mag= p(1)^2 + p(2)^2; 5 | sqrt_mag= sqrt(mag); 6 | H= [ p(2)/mag 0 -p(1)/mag 0 0; ... 7 | p(1)/sqrt_mag 0 p(2)/sqrt_mag 0 0 ]; 8 | U= eye(2); 9 | -------------------------------------------------------------------------------- /releasecodes/singletarget/ekf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/singletarget/ekf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/singletarget/gms/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the single target filter with RFS observations proposed in 2 | % (assuming Poisson clutter and no extraneous measurements) 3 | % B.-T. Vo, B.-N. Vo, and A. Cantoni, "Bayesian filtering with random finite set observations," IEEE Trans. Signal Processing, Vol. 56, No. 4, pp. 1313-1326, 2008. 4 | % http://ba-ngu.vo-au.com/vo/VVCsingletargetSP08.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{STF, 7 | % author={B.-T.Vo and B.-N. Vo and A. Cantoni}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={Bayesian Filtering With Random Finite Set Observations}, 10 | % year={2008}, 11 | % month={April}, 12 | % volume={56}, 13 | % number={4}, 14 | % pages={1313-1326}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/singletarget/gms/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/singletarget/gms/gen_newstate_fn.m: -------------------------------------------------------------------------------- 1 | function X= gen_newstate_fn(model,Xd,V) 2 | 3 | %linear state space equation (CV model) 4 | 5 | if ~isnumeric(V) 6 | if strcmp(V,'noise') 7 | V= model.sigma_V*model.B*randn(size(model.B,2),size(Xd,2)); 8 | elseif strcmp(V,'noiseless') 9 | V= zeros(size(model.B,1),size(Xd,2)); 10 | end 11 | end 12 | 13 | if isempty(Xd) 14 | X= []; 15 | else 16 | X= model.F*Xd+ V; 17 | end -------------------------------------------------------------------------------- /releasecodes/singletarget/gms/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation(model,X,W) 2 | 3 | %linear observation equation (position components only) 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else 16 | Z= model.H*X+ W; 17 | end -------------------------------------------------------------------------------- /releasecodes/singletarget/smc/compute_likelihood.m: -------------------------------------------------------------------------------- 1 | function gz_vals= compute_likelihood(model,z,X) 2 | 3 | % compute likelihood vector g= [ log_g(z|x_1), ... , log_g(z|x_M) ] - 4 | % this is for bearings and range case with additive Gaussian noise 5 | 6 | M= size(X,2); 7 | P= X([1 3],:); 8 | Phi= zeros(2,M); 9 | Phi(1,:)= atan2(P(1,:),P(2,:)); 10 | Phi(2,:)= sqrt(sum(P.^2)); 11 | e_sq= sum( (diag(1./diag(model.D))*(repmat(z,[1 M])- Phi)).^2 ); 12 | gz_vals= exp(-e_sq/2 - log(2*pi*prod(diag(model.D)))); 13 | -------------------------------------------------------------------------------- /releasecodes/singletarget/smc/compute_pD.m: -------------------------------------------------------------------------------- 1 | function pD = compute_pD(model,X) 2 | 3 | if isempty(X) 4 | pD= []; 5 | else 6 | max= 0.98; 7 | mid= [0; 0]; 8 | cov= diag([2000,2000].^2); 9 | 10 | M= size(X,2); 11 | P= X([1 3],:); 12 | e_sq= sum( (diag(1./diag(sqrt(cov)))*(P-repmat(mid,[1 M]))).^2 ); 13 | 14 | pD= max*exp(-e_sq/2); 15 | pD= pD(:); 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/singletarget/smc/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the single target filter with RFS observations proposed in 2 | % (assuming Poisson clutter and no extraneous measurements) 3 | % B.-T. Vo, B.-N. Vo, and A. Cantoni, "Bayesian filtering with random finite set observations," IEEE Trans. Signal Processing, Vol. 56, No. 4, pp. 1313-1326, 2008. 4 | % http://ba-ngu.vo-au.com/vo/VVCsingletargetSP08.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{STF, 7 | % author={B.-T.Vo and B.-N. Vo and A. Cantoni}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={Bayesian Filtering With Random Finite Set Observations}, 10 | % year={2008}, 11 | % month={April}, 12 | % volume={56}, 13 | % number={4}, 14 | % pages={1313-1326}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/singletarget/smc/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= compute_pD(model,truth.X{k}) ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/singletarget/smc/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /releasecodes/singletarget/ukf/demo.m: -------------------------------------------------------------------------------- 1 | % This is a demo script for the single target filter with RFS observations proposed in 2 | % (assuming Poisson clutter and no extraneous measurements) 3 | % B.-T. Vo, B.-N. Vo, and A. Cantoni, "Bayesian filtering with random finite set observations," IEEE Trans. Signal Processing, Vol. 56, No. 4, pp. 1313-1326, 2008. 4 | % http://ba-ngu.vo-au.com/vo/VVCsingletargetSP08.pdf 5 | % ---BibTeX entry 6 | % @ARTICLE{STF, 7 | % author={B.-T.Vo and B.-N. Vo and A. Cantoni}, 8 | % journal={IEEE Transactions on Signal Processing}, 9 | % title={Bayesian Filtering With Random Finite Set Observations}, 10 | % year={2008}, 11 | % month={April}, 12 | % volume={56}, 13 | % number={4}, 14 | % pages={1313-1326}} 15 | %--- 16 | 17 | model= gen_model; 18 | truth= gen_truth(model); 19 | meas= gen_meas(model,truth); 20 | est= run_filter(model,meas); 21 | handles= plot_results(model,truth,meas,est); -------------------------------------------------------------------------------- /releasecodes/singletarget/ukf/gen_meas.m: -------------------------------------------------------------------------------- 1 | function meas= gen_meas(model,truth) 2 | 3 | %variables 4 | meas.K= truth.K; 5 | meas.Z= cell(truth.K,1); 6 | 7 | %generate measurements 8 | for k=1:truth.K 9 | if truth.N(k) > 0 10 | idx= find( rand(truth.N(k),1) <= model.P_D ); %detected target indices 11 | meas.Z{k}= gen_observation_fn(model,truth.X{k}(:,idx),'noise'); %single target observations if detected 12 | end 13 | N_c= poissrnd(model.lambda_c); %number of clutter points 14 | C= repmat(model.range_c(:,1),[1 N_c])+ diag(model.range_c*[ -1; 1 ])*rand(model.z_dim,N_c); %clutter generation 15 | meas.Z{k}= [ meas.Z{k} C ]; %measurement is union of detections and clutter 16 | end 17 | -------------------------------------------------------------------------------- /releasecodes/singletarget/ukf/gen_observation_fn.m: -------------------------------------------------------------------------------- 1 | function Z= gen_observation_fn(model,X,W) 2 | 3 | %r/t observation equation 4 | 5 | if ~isnumeric(W) 6 | if strcmp(W,'noise') 7 | W= model.D*randn(size(model.D,2),size(X,2)); 8 | elseif strcmp(W,'noiseless') 9 | W= zeros(size(model.D,1),size(X,2)); 10 | end 11 | end 12 | 13 | if isempty(X) 14 | Z= []; 15 | else %modify below here for user specified measurement model 16 | P= X([1 3],:); 17 | Z(1,:)= atan2(P(1,:),P(2,:)); 18 | Z(2,:)= sqrt(sum(P.^2)); 19 | Z= Z+ W; 20 | end -------------------------------------------------------------------------------- /results1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/results1.mat -------------------------------------------------------------------------------- /results1x2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/results1x2.mat -------------------------------------------------------------------------------- /results2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/results2.mat -------------------------------------------------------------------------------- /results3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/results3.mat -------------------------------------------------------------------------------- /sample_mvgm/configuremingw.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/sample_mvgm/configuremingw.p -------------------------------------------------------------------------------- /sample_mvgm/mexme_sample_mvgm.m: -------------------------------------------------------------------------------- 1 | echo on 2 | 3 | mex -DranSHR3 sample_mvgm.c 4 | 5 | echo off -------------------------------------------------------------------------------- /sample_mvgm/sample_mvgm.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/sample_mvgm/sample_mvgm.c -------------------------------------------------------------------------------- /sample_mvgm/sample_mvgm.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/sample_mvgm/sample_mvgm.mexw64 -------------------------------------------------------------------------------- /sim_σ_0,4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/sim_σ_0,4.mat -------------------------------------------------------------------------------- /sim_σ_0,8.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/sim_σ_0,8.mat -------------------------------------------------------------------------------- /task3.m: -------------------------------------------------------------------------------- 1 | function task4(N1,N2,N3,N4) 2 | %Determine the input of the function 3 | N=[N1,N2,N3,N4]; 4 | %An array that store the 4 variables in the experiment 5 | P=[0,0,0,0]; 6 | %Defualt value of P 7 | %A FOR loop to call function in task 2 with 4 variables 8 | for i=1:4 9 | P(i)=task2(N(i),5); 10 | hold on 11 | end 12 | %Change the variables in log form for better display 13 | plot(log10(N),P,'*-'); 14 | %Labels's names 15 | xlabel('log10(N)'); 16 | ylabel('P'); 17 | %Figure title 18 | title('Probabilities P against N in Task4') -------------------------------------------------------------------------------- /traj_gen/createTraj3.m: -------------------------------------------------------------------------------- 1 | function [a3,a2,a1,a0] = createTraj3(theta0,thetaf,thetad0,thetadf,tstart,tfinal) 2 | % inputs : initial position, velocity + final position, velocity + initial and final times 3 | % output : a vector specifying the polynomial and can be used with poly functions such as : polyder, polyval, etc. 4 | % create a 3rd order trajectory 5 | % example: 6 | % createTraj3(10,30,0,0,0,1) 7 | % 8 | % 9 | % By: Reza Ahmadzadeh - Matlab/Octave - 2013 10 | T = tfinal - tstart; 11 | a0 = theta0; 12 | a1 = thetad0; 13 | a2 = (-3 * (theta0 - thetaf) - (2 * thetad0+thetadf )*T)/ T ^ 2; 14 | a3 = (2 * (theta0 - thetaf) + (thetad0+thetadf )*T)/ T ^ 3; 15 | 16 | end 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /untitled.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sglvladi/MATLAB/5fbc4086362c1c9c2a7fa13c97aa9a0d34e2069b/untitled.fig -------------------------------------------------------------------------------- /wind.m: -------------------------------------------------------------------------------- 1 | clear all 2 | 3 | tic 4 | N = 1e5; 5 | 6 | Y = zeros(100,1); 7 | X = zeros(N,1); 8 | E = zeros(N,1); 9 | 10 | Y(1) = 1.5; 11 | Y(2) = Y(1); 12 | 13 | for i = 1:N 14 | for t = 3:100 15 | e = sqrt(0.02)*randn; 16 | Y(t) = 0.7*Y(t-1) + 0.45*Y(t-2) + e; 17 | end 18 | X(i) = sum((Y 10 20 | E(i) = 1; 21 | else 22 | E(i) = 0; 23 | end 24 | end 25 | p = sum(E)/N 26 | toc 27 | --------------------------------------------------------------------------------