├── HA02 ├── GNN.png ├── JPDA.png ├── TOMHT.png ├── MATLABGraderTestPackage │ ├── .DS_Store │ ├── +reference │ │ ├── KeyVal.p │ │ ├── MurtyData.p │ │ ├── assign2D.p │ │ ├── measmodel.p │ │ ├── modelgen.p │ │ ├── BinaryHeap.p │ │ ├── Simulation.p │ │ ├── log_mvnpdf.p │ │ ├── measdatagen.p │ │ ├── motionmodel.p │ │ ├── RMSE_n_objects.p │ │ ├── assign2DByCol.p │ │ ├── kBest2DAssign.p │ │ ├── n_objectracker.p │ │ ├── objectdatagen.p │ │ ├── GaussianDensity.p │ │ ├── assign2DByGibbs.p │ │ ├── hypothesisReduction.p │ │ ├── normalizeLogWeights.p │ │ └── readme.rtf │ ├── readme.rtf │ ├── SymbolicExpressionComparator.m │ ├── AssessmentDiagnostic.m │ ├── HA2_test │ │ ├── SA3Ex1Test1.m │ │ ├── SA3Ex1Test2.m │ │ ├── SA3Ex3Test1.m │ │ ├── SA3Ex3Test2.m │ │ ├── SA3Ex2Test1.m │ │ └── SA3Ex2Test2.m │ └── assessVariableEqual.m ├── normalizeLogWeights.m ├── RMSE_n_objects.m ├── objectdatagen.m ├── measdatagen.m ├── assign2DByGibbs.m ├── modelgen.m ├── main.m ├── motionmodel.m ├── KeyVal.m ├── hypothesisReduction.m └── Animate_2D_tracking.m ├── HA05 ├── readme.txt ├── Simulation.mlx ├── normalizeLogWeights.m ├── objectdatagen.m ├── license.txt ├── measdatagen.m ├── modelgen.m ├── assign2DByGibbs.m ├── motionmodel.m ├── hypothesisReduction.m ├── auctionAlgortihm.m ├── KeyVal.m ├── Simulation.m └── multiobjectracker.m ├── HA03 ├── PHDfilter.png ├── simulation3.mlx ├── PHDfilter_flow.png ├── RFS_simulation.mlx ├── MATLABGraderTestPackage │ ├── +reference │ │ ├── GOSPA.p │ │ ├── PHDfilter.p │ │ ├── assign2D.p │ │ ├── measmodel.p │ │ ├── modelgen.p │ │ ├── trackgen.p │ │ ├── log_mvnpdf.p │ │ ├── measdatagen.p │ │ ├── motionmodel.p │ │ ├── simulation.p │ │ ├── objectdatagen.p │ │ ├── GaussianDensity.p │ │ ├── auctionAlgortihm.p │ │ ├── multiobjectracker.p │ │ ├── hypothesisReduction.p │ │ ├── multiobjectProcess.p │ │ ├── normalizeLogWeights.p │ │ └── readme.rtf │ ├── readme.rtf │ ├── SymbolicExpressionComparator.m │ ├── AssessmentDiagnostic.m │ ├── HA3_test │ │ └── SA4Ex1Test3.m │ └── assessVariableEqual.m ├── main.m ├── trackgen.m ├── auctionAlgortihm.m └── multiobjectracker_HA3.m ├── HA04 ├── simulation4.mlx ├── PMBM_post_processing.png ├── PMBM filtering recursion_1.png ├── PMBM filtering recursion_2.png ├── Challenges - labels to form trajectories.png └── MATLABGraderTestPackage │ ├── +reference │ ├── GOSPA.p │ ├── KeyVal.p │ ├── MurtyData.p │ ├── PHDfilter.p │ ├── assign2D.p │ ├── mb2mbm01.p │ ├── measmodel.p │ ├── modelgen.p │ ├── BinaryHeap.p │ ├── PMBMfilter.p │ ├── Simulation.p │ ├── log_mvnpdf.p │ ├── measdatagen.p │ ├── motionmodel.p │ ├── assign2DByCol.p │ ├── kBest2DAssign.p │ ├── objectdatagen.p │ ├── GaussianDensity.p │ ├── assign2DByGibbs.p │ ├── auctionAlgortihm.p │ ├── multiobjectracker.p │ ├── hypothesisReduction.p │ ├── normalizeLogWeights.p │ └── readme.rtf │ ├── readme.rtf │ ├── HA4_test │ ├── SA5Ex1Test7.m │ ├── SA5Ex1Test2.m │ ├── SA5Ex1Test10.m │ ├── SA5Ex1Test1.m │ ├── SA5Ex1Test3.m │ ├── SA5Ex1Test4.m │ ├── SA5Ex1Test5.m │ ├── SA5Ex1Test6.m │ ├── SA5Ex1Test8.m │ └── SA5Ex1Test9.m │ ├── SymbolicExpressionComparator.m │ ├── AssessmentDiagnostic.m │ └── assessVariableEqual.m ├── .gitignore ├── HA01 ├── MATLABGraderTestPackage │ ├── readme.rtf │ ├── +reference │ │ └── readme.rtf │ ├── HA1_test │ │ ├── SA2Ex5Test1.m │ │ ├── SA2Ex3Test2.m │ │ ├── SA2Ex3Test1.m │ │ ├── SA2Ex2Test3.m │ │ ├── SA2Ex1Test1.m │ │ ├── SA2Ex2Test2.m │ │ ├── SA2Ex2Test1.m │ │ ├── SA2Ex4Test3.m │ │ ├── SA2Ex4Test2.m │ │ └── SA2Ex4Test1.m │ ├── SymbolicExpressionComparator.m │ ├── AssessmentDiagnostic.m │ └── assessVariableEqual.m ├── normalizeLogWeights.m ├── RMSE.m ├── objectdatagen.m ├── measdatagen.m ├── main.m ├── modelgen.m ├── motionmodel.m ├── hypothesisReduction.m ├── Animate_2D_tracking.m └── simulation.m └── README.md /HA02/GNN.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/GNN.png -------------------------------------------------------------------------------- /HA02/JPDA.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/JPDA.png -------------------------------------------------------------------------------- /HA02/TOMHT.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/TOMHT.png -------------------------------------------------------------------------------- /HA05/readme.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA05/readme.txt -------------------------------------------------------------------------------- /HA03/PHDfilter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/PHDfilter.png -------------------------------------------------------------------------------- /HA05/Simulation.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA05/Simulation.mlx -------------------------------------------------------------------------------- /HA03/simulation3.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/simulation3.mlx -------------------------------------------------------------------------------- /HA04/simulation4.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/simulation4.mlx -------------------------------------------------------------------------------- /HA03/PHDfilter_flow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/PHDfilter_flow.png -------------------------------------------------------------------------------- /HA03/RFS_simulation.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/RFS_simulation.mlx -------------------------------------------------------------------------------- /HA04/PMBM_post_processing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/PMBM_post_processing.png -------------------------------------------------------------------------------- /HA04/PMBM filtering recursion_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/PMBM filtering recursion_1.png -------------------------------------------------------------------------------- /HA04/PMBM filtering recursion_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/PMBM filtering recursion_2.png -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/.DS_Store -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/KeyVal.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/KeyVal.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/GOSPA.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/GOSPA.p -------------------------------------------------------------------------------- /HA04/Challenges - labels to form trajectories.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/Challenges - labels to form trajectories.png -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/GOSPA.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/GOSPA.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/KeyVal.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/KeyVal.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/MurtyData.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/MurtyData.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/assign2D.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/assign2D.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/measmodel.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/measmodel.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/modelgen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/modelgen.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/PHDfilter.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/PHDfilter.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/assign2D.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/assign2D.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/measmodel.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/measmodel.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/modelgen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/modelgen.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/trackgen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/trackgen.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/MurtyData.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/MurtyData.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/PHDfilter.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/PHDfilter.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/assign2D.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/assign2D.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/mb2mbm01.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/mb2mbm01.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/measmodel.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/measmodel.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/modelgen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/modelgen.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/BinaryHeap.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/BinaryHeap.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/Simulation.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/Simulation.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/log_mvnpdf.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/log_mvnpdf.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/measdatagen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/measdatagen.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/motionmodel.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/motionmodel.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/log_mvnpdf.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/log_mvnpdf.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/measdatagen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/measdatagen.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/motionmodel.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/motionmodel.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/simulation.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/simulation.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/BinaryHeap.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/BinaryHeap.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/PMBMfilter.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/PMBMfilter.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/Simulation.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/Simulation.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/log_mvnpdf.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/log_mvnpdf.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/measdatagen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/measdatagen.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/motionmodel.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/motionmodel.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/RMSE_n_objects.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/RMSE_n_objects.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/assign2DByCol.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/assign2DByCol.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/kBest2DAssign.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/kBest2DAssign.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/n_objectracker.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/n_objectracker.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/objectdatagen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/objectdatagen.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/objectdatagen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/objectdatagen.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/assign2DByCol.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/assign2DByCol.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/kBest2DAssign.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/kBest2DAssign.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/objectdatagen.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/objectdatagen.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/GaussianDensity.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/GaussianDensity.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/assign2DByGibbs.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/assign2DByGibbs.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/GaussianDensity.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/GaussianDensity.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/auctionAlgortihm.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/auctionAlgortihm.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/multiobjectracker.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/multiobjectracker.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/GaussianDensity.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/GaussianDensity.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/assign2DByGibbs.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/assign2DByGibbs.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/auctionAlgortihm.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/auctionAlgortihm.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/multiobjectracker.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/multiobjectracker.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/hypothesisReduction.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/hypothesisReduction.p -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/normalizeLogWeights.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA02/MATLABGraderTestPackage/+reference/normalizeLogWeights.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/hypothesisReduction.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/hypothesisReduction.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/multiobjectProcess.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/multiobjectProcess.p -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/normalizeLogWeights.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA03/MATLABGraderTestPackage/+reference/normalizeLogWeights.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/hypothesisReduction.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/hypothesisReduction.p -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/normalizeLogWeights.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ankurjainjob/Multi-Object-Tracking/HEAD/HA04/MATLABGraderTestPackage/+reference/normalizeLogWeights.p -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | *.m~ 3 | 4 | *.zip 5 | 6 | *.mp4 7 | 8 | *.DS_Store 9 | 10 | HA02/MATLABGraderTestPackage/\+reference/HA2/__MACOSX/ 11 | 12 | HA01/MATLABGraderTestPackage/__MACOSX/ 13 | 14 | HA01/MATLABGraderTestPackage/ 15 | -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/readme.rtf: -------------------------------------------------------------------------------- 1 | {\rtf1\ansi\ansicpg1252\cocoartf1671\cocoasubrtf400 2 | {\fonttbl\f0\fswiss\fcharset0 Helvetica;} 3 | {\colortbl;\red255\green255\blue255;} 4 | {\*\expandedcolortbl;;} 5 | \paperw11900\paperh16840\margl1440\margr1440\vieww10800\viewh8400\viewkind0 6 | \pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\pardirnatural\partightenfactor0 7 | 8 | \f0\fs24 \cf0 Copy the test scripts and your own scripts here.} -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/readme.rtf: -------------------------------------------------------------------------------- 1 | {\rtf1\ansi\ansicpg1252\cocoartf1671\cocoasubrtf400 2 | {\fonttbl\f0\fswiss\fcharset0 Helvetica;} 3 | {\colortbl;\red255\green255\blue255;} 4 | {\*\expandedcolortbl;;} 5 | \paperw11900\paperh16840\margl1440\margr1440\vieww10800\viewh8400\viewkind0 6 | \pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\pardirnatural\partightenfactor0 7 | 8 | \f0\fs24 \cf0 Copy the test scripts and your own scripts here.} -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/readme.rtf: -------------------------------------------------------------------------------- 1 | {\rtf1\ansi\ansicpg1252\cocoartf1671\cocoasubrtf400 2 | {\fonttbl\f0\fswiss\fcharset0 Helvetica;} 3 | {\colortbl;\red255\green255\blue255;} 4 | {\*\expandedcolortbl;;} 5 | \paperw11900\paperh16840\margl1440\margr1440\vieww10800\viewh8400\viewkind0 6 | \pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\pardirnatural\partightenfactor0 7 | 8 | \f0\fs24 \cf0 Copy the test scripts and your own scripts here.} -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/readme.rtf: -------------------------------------------------------------------------------- 1 | {\rtf1\ansi\ansicpg1252\cocoartf1671\cocoasubrtf400 2 | {\fonttbl\f0\fswiss\fcharset0 Helvetica;} 3 | {\colortbl;\red255\green255\blue255;} 4 | {\*\expandedcolortbl;;} 5 | \paperw11900\paperh16840\margl1440\margr1440\vieww10800\viewh8400\viewkind0 6 | \pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\pardirnatural\partightenfactor0 7 | 8 | \f0\fs24 \cf0 Copy the test scripts and your own scripts here.} -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/+reference/readme.rtf: -------------------------------------------------------------------------------- 1 | {\rtf1\ansi\ansicpg1252\cocoartf1671\cocoasubrtf400 2 | {\fonttbl\f0\fswiss\fcharset0 Helvetica;} 3 | {\colortbl;\red255\green255\blue255;} 4 | {\*\expandedcolortbl;;} 5 | \paperw11900\paperh16840\margl1440\margr1440\vieww10800\viewh8400\viewkind0 6 | \pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\pardirnatural\partightenfactor0 7 | 8 | \f0\fs24 \cf0 Copy the reference scripts, i.e., p files here.} -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/+reference/readme.rtf: -------------------------------------------------------------------------------- 1 | {\rtf1\ansi\ansicpg1252\cocoartf1671\cocoasubrtf400 2 | {\fonttbl\f0\fswiss\fcharset0 Helvetica;} 3 | {\colortbl;\red255\green255\blue255;} 4 | {\*\expandedcolortbl;;} 5 | \paperw11900\paperh16840\margl1440\margr1440\vieww10800\viewh8400\viewkind0 6 | \pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\pardirnatural\partightenfactor0 7 | 8 | \f0\fs24 \cf0 Copy the reference scripts, i.e., p files here.} -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/+reference/readme.rtf: -------------------------------------------------------------------------------- 1 | {\rtf1\ansi\ansicpg1252\cocoartf1671\cocoasubrtf400 2 | {\fonttbl\f0\fswiss\fcharset0 Helvetica;} 3 | {\colortbl;\red255\green255\blue255;} 4 | {\*\expandedcolortbl;;} 5 | \paperw11900\paperh16840\margl1440\margr1440\vieww10800\viewh8400\viewkind0 6 | \pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\pardirnatural\partightenfactor0 7 | 8 | \f0\fs24 \cf0 Copy the reference scripts, i.e., p files here.} -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/+reference/readme.rtf: -------------------------------------------------------------------------------- 1 | {\rtf1\ansi\ansicpg1252\cocoartf1671\cocoasubrtf400 2 | {\fonttbl\f0\fswiss\fcharset0 Helvetica;} 3 | {\colortbl;\red255\green255\blue255;} 4 | {\*\expandedcolortbl;;} 5 | \paperw11900\paperh16840\margl1440\margr1440\vieww10800\viewh8400\viewkind0 6 | \pard\tx566\tx1133\tx1700\tx2267\tx2834\tx3401\tx3968\tx4535\tx5102\tx5669\tx6236\tx6803\pardirnatural\partightenfactor0 7 | 8 | \f0\fs24 \cf0 Copy the reference scripts, i.e., p files here.} -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test7.m: -------------------------------------------------------------------------------- 1 | P_D = rand; 2 | PPP.w = log(rand(3,1)); 3 | 4 | % Run reference solution. 5 | PMBM_ref = reference.PMBMfilter(); 6 | PMBM_ref.paras.PPP.w = PPP.w; 7 | PMBM_ref = PPP_undetected_update(PMBM_ref,P_D); 8 | 9 | PPP_w_ref = PMBM_ref.paras.PPP.w; 10 | 11 | % Run learner solution. 12 | PMBM = PMBMfilter(); 13 | PMBM.paras.PPP.w = PPP.w; 14 | PMBM = PPP_undetected_update(PMBM,P_D); 15 | 16 | PPP_w = PMBM.paras.PPP.w; 17 | 18 | % Compare. 19 | assessVariableEqual('PPP_w', PPP_w_ref,'Feedback','Parameter PPP.w is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/normalizeLogWeights.m: -------------------------------------------------------------------------------- 1 | function [log_w,log_sum_w] = normalizeLogWeights(log_w) 2 | 3 | % 4 | % Input: 5 | % log_w - log weights, e.g., log likelihoods 6 | % 7 | % Output: 8 | % log_w - log of the normalized weights 9 | % log_sum_w - log of the sum of the non-normalized weights 10 | % 11 | 12 | if length(log_w)<=1 13 | % Log of sum of weights times prior probabilities 14 | log_sum_w = log_w; 15 | % Normalize 16 | log_w = log_w-log_sum_w; 17 | elseif length(log_w)>1 18 | % Log of sum of weights times prior probabilities 19 | [log_w_aux,I] = sort(log_w,'descend'); 20 | log_sum_w = max(log_w_aux)+log(1+sum(exp(log_w(I(2:end))-max(log_w_aux)))); 21 | % Normalize 22 | log_w = log_w-log_sum_w; 23 | end -------------------------------------------------------------------------------- /HA02/normalizeLogWeights.m: -------------------------------------------------------------------------------- 1 | function [log_w,log_sum_w] = normalizeLogWeights(log_w) 2 | 3 | % 4 | % Input: 5 | % log_w - log weights, e.g., log likelihoods 6 | % 7 | % Output: 8 | % log_w - log of the normalized weights 9 | % log_sum_w - log of the sum of the non-normalized weights 10 | % 11 | 12 | if length(log_w)<=1 13 | % Log of sum of weights times prior probabilities 14 | log_sum_w = log_w; 15 | % Normalize 16 | log_w = log_w-log_sum_w; 17 | elseif length(log_w)>1 18 | % Log of sum of weights times prior probabilities 19 | [log_w_aux,I] = sort(log_w,'descend'); 20 | log_sum_w = max(log_w_aux)+log(1+sum(exp(log_w(I(2:end))-max(log_w_aux)))); 21 | % Normalize 22 | log_w = log_w-log_sum_w; 23 | end -------------------------------------------------------------------------------- /HA05/normalizeLogWeights.m: -------------------------------------------------------------------------------- 1 | function [log_w,log_sum_w] = normalizeLogWeights(log_w) 2 | 3 | % 4 | % Input: 5 | % log_w - log weights, e.g., log likelihoods 6 | % 7 | % Output: 8 | % log_w - log of the normalized weights 9 | % log_sum_w - log of the sum of the non-normalized weights 10 | % 11 | 12 | if length(log_w)<=1 13 | % Log of sum of weights times prior probabilities 14 | log_sum_w = log_w; 15 | % Normalize 16 | log_w = log_w-log_sum_w; 17 | elseif length(log_w)>1 18 | % Log of sum of weights times prior probabilities 19 | [log_w_aux,I] = sort(log_w,'descend'); 20 | log_sum_w = max(log_w_aux)+log(1+sum(exp(log_w(I(2:end))-max(log_w_aux)))); 21 | % Normalize 22 | log_w = log_w-log_sum_w; 23 | end -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex5Test1.m: -------------------------------------------------------------------------------- 1 | N = 20; 2 | x = cell(N,1); 3 | y = cell(N,1); 4 | 5 | for i = 1:N 6 | x{i} = randn(4,1); 7 | y{i} = randn(4,1); 8 | end 9 | 10 | 11 | % Run learner solution. 12 | root_mean_square_error = RMSE(x,y); 13 | 14 | % Run reference solution. 15 | root_mean_square_error_ref = reference.RMSE(x,y); 16 | 17 | % Compare. 18 | assert(abs(root_mean_square_error-root_mean_square_error_ref) < 1e-3, 'Expected %f, got %f. Your implementation is incorrect, please modify your code and submit again.', root_mean_square_error_ref, root_mean_square_error); 19 | % assessVariableEqual('root_mean_square_error', root_mean_square_error_ref,'RelativeTolerance',0.03,'Feedback','Your implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test2.m: -------------------------------------------------------------------------------- 1 | %Set detection probability 2 | P_D = rand; 3 | %Create hypothesis tree 4 | Bern.r = rand; 5 | tt_entry = [1,1]; 6 | 7 | % Run reference solution. 8 | PMBM_ref = reference.PMBMfilter(); 9 | PMBM_ref.paras.MBM.tt{1} = Bern; 10 | [Bern_ref, lik_undetected_ref] = Bern_undetected_update(PMBM_ref,tt_entry,P_D); 11 | r_ref = Bern_ref.r; 12 | 13 | % Run learner solution. 14 | PMBM = PMBMfilter(); 15 | PMBM.paras.MBM.tt{1} = Bern; 16 | [Bern, lik_undetected] = Bern_undetected_update(PMBM,tt_entry,P_D); 17 | r = Bern.r; 18 | 19 | % Compare. 20 | assessVariableEqual('r', r_ref,'Feedback','Parameter Bern.r is incorrect, please modify your code and submit again.'); 21 | assessVariableEqual('lik_undetected', lik_undetected_ref,'Feedback','Parameter lik_undetected is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex3Test2.m: -------------------------------------------------------------------------------- 1 | threshold = 50; 2 | 3 | hypothesis.x = 0; 4 | multiHypotheses = repmat(hypothesis,100,1); 5 | hypothesesWeight = rand(100,1); 6 | hypothesesWeight = log(hypothesesWeight/sum(hypothesesWeight)); 7 | 8 | % Run learner solution. 9 | [hypothesesWeight_hat, multiHypotheses_hat] = hypothesisReduction.cap(hypothesesWeight, multiHypotheses, threshold); 10 | 11 | hypothesesWeight_hat = sort(hypothesesWeight_hat); 12 | 13 | % Run reference solution. 14 | [hypothesesWeight_hat_ref, multiHypotheses_hat_ref] = reference.hypothesisReduction.cap(hypothesesWeight, multiHypotheses, threshold); 15 | 16 | hypothesesWeight_hat_ref = sort(hypothesesWeight_hat_ref); 17 | 18 | % Compare. 19 | assessVariableEqual('hypothesesWeight_hat', hypothesesWeight_hat_ref,'RelativeTolerance',0.03,'Feedback','Your implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex3Test1.m: -------------------------------------------------------------------------------- 1 | threshold = log(1e-2); 2 | 3 | hypothesis.x = 0; 4 | multiHypotheses = repmat(hypothesis,100,1); 5 | hypothesesWeight = rand(100,1); 6 | hypothesesWeight = log(hypothesesWeight/sum(hypothesesWeight)); 7 | 8 | % Run learner solution. 9 | [hypothesesWeight_hat, multiHypotheses_hat] = hypothesisReduction.prune(hypothesesWeight, multiHypotheses, threshold); 10 | 11 | hypothesesWeight_hat = sort(hypothesesWeight_hat); 12 | 13 | % Run reference solution. 14 | [hypothesesWeight_hat_ref, multiHypotheses_hat_ref] = reference.hypothesisReduction.prune(hypothesesWeight, multiHypotheses, threshold); 15 | 16 | hypothesesWeight_hat_ref = sort(hypothesesWeight_hat_ref); 17 | 18 | % Compare. 19 | assessVariableEqual('hypothesesWeight_hat', hypothesesWeight_hat_ref,'RelativeTolerance',0.03,'Feedback','Your implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA02/RMSE_n_objects.m: -------------------------------------------------------------------------------- 1 | function rmse = RMSE_n_objects(X,Y) 2 | 3 | % Compute RMSE distance between X and Y estimates 4 | % Inputs: X,Y - cell array, each cell contains matrices of column vectors 5 | % Output: scalar distance between X and Y 6 | % Note: the Euclidean 2-norm is used as the "base" distance on the region 7 | 8 | K = length(X); 9 | dist = zeros(K,1); 10 | for k = 1:K 11 | %Calculate sizes of the input point patterns 12 | n = size(X{k},2); 13 | 14 | %Calculate cost/weight matrix for pairings - fast method with vectorization 15 | XX= repmat(X{k},[1 n]); 16 | YY= reshape(repmat(Y{k},[n 1]),[size(Y{k},1) n*n]); 17 | D = reshape(sqrt(sum((XX-YY).^2)),[n n]); 18 | 19 | %Compute optimal assignment and cost 20 | [~,~,cost]= assign2D(D); 21 | 22 | %Calculate final distance 23 | dist(k)= cost/n; 24 | end 25 | rmse = mean(dist); 26 | 27 | end -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test10.m: -------------------------------------------------------------------------------- 1 | w = sort(rand(3,1)); 2 | ht = [1 0 1; 3 | 1 1 1; 4 | 0 1 1]; 5 | r = rand(3,1); 6 | threshold = mean(r); 7 | tt = cell(3,1); 8 | tt{1}.r = r(1); 9 | tt{1}.state.x = rand(4,1); 10 | tt{2}.r = r(2); 11 | tt{2}.state.x = rand(4,1); 12 | tt{3}.r = r(3); 13 | tt{3}.state.x = rand(4,1); 14 | 15 | % Run reference solution. 16 | PMBM_ref = reference.PMBMfilter(); 17 | PMBM_ref.density = feval(@GaussianDensity); 18 | PMBM_ref.paras.MBM.w = w; 19 | PMBM_ref.paras.MBM.ht = ht; 20 | PMBM_ref.paras.MBM.tt = tt; 21 | estimates_ref = PMBM_estimator(PMBM_ref,threshold); 22 | 23 | % Run learner solution. 24 | PMBM = PMBMfilter(); 25 | PMBM.density = feval(@GaussianDensity); 26 | PMBM.paras.MBM.w = w; 27 | PMBM.paras.MBM.ht = ht; 28 | PMBM.paras.MBM.tt = tt; 29 | estimates = PMBM_estimator(PMBM,threshold); 30 | 31 | % Compare. 32 | assessVariableEqual('estimates', estimates_ref,'Feedback','Output estimates is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/RMSE.m: -------------------------------------------------------------------------------- 1 | function root_mean_square_error = RMSE(state_sequence1,state_sequence2) 2 | %RMSE calculates the root mean square error between state_sequence1 and 3 | %state_sequence2 4 | %INPUT: state_sequence1: a cell array of size (total tracking time x 1), 5 | % each cell contains an object state mean vector of size 6 | % (state dimension x 1) 7 | % state_sequence2: a cell array of size (total tracking time x 1), 8 | % each cell contains an object state mean vector of size 9 | % (state dimension x 1) 10 | %OUTPUT:root_mean_square_error: root mean square error --- scalar 11 | 12 | root_mean_square_error = mean( (cell2mat(state_sequence1)-cell2mat(state_sequence2)).^2 ) ^0.5 13 | end 14 | 15 | 16 | 17 | %% test function 18 | 19 | % N = 20; 20 | % x = cell(N,1); 21 | % y = cell(N,1); 22 | % 23 | % for i = 1:N 24 | % x{i} = randn(4,1); 25 | % y{i} = randn(4,1); 26 | % end 27 | % 28 | % mse(cell2mat(x) - cell2mat(y)) 29 | % 30 | % root_mean_square_error = RMSE(x,y); -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/SymbolicExpressionComparator.m: -------------------------------------------------------------------------------- 1 | classdef SymbolicExpressionComparator < matlab.unittest.constraints.Comparator 2 | 3 | methods(Access=protected) 4 | function bool = supportsContainer(~, value) 5 | bool = isa(value, 'sym'); 6 | end 7 | 8 | function bool = containerSatisfiedBy(comparator, actual, expected) 9 | bool = comparator.isAlwaysEqual(actual, expected) || ... 10 | comparator.isSimplifiedEqual(actual, expected); 11 | end 12 | end 13 | 14 | methods(Access=private) 15 | function bool = isAlwaysEqual(~, actual, expected) 16 | result = isAlways(actual == expected, 'Unknown','false'); 17 | bool = all(result(:)); 18 | end 19 | 20 | function bool = isSimplifiedEqual(constraint, actual, expected) 21 | simplifiedResult = simplify(actual - expected, 'Steps', 1000); 22 | bool = constraint.isAlwaysEqual(simplifiedResult, 0); 23 | end 24 | end 25 | end -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/SymbolicExpressionComparator.m: -------------------------------------------------------------------------------- 1 | classdef SymbolicExpressionComparator < matlab.unittest.constraints.Comparator 2 | 3 | methods(Access=protected) 4 | function bool = supportsContainer(~, value) 5 | bool = isa(value, 'sym'); 6 | end 7 | 8 | function bool = containerSatisfiedBy(comparator, actual, expected) 9 | bool = comparator.isAlwaysEqual(actual, expected) || ... 10 | comparator.isSimplifiedEqual(actual, expected); 11 | end 12 | end 13 | 14 | methods(Access=private) 15 | function bool = isAlwaysEqual(~, actual, expected) 16 | result = isAlways(actual == expected, 'Unknown','false'); 17 | bool = all(result(:)); 18 | end 19 | 20 | function bool = isSimplifiedEqual(constraint, actual, expected) 21 | simplifiedResult = simplify(actual - expected, 'Steps', 1000); 22 | bool = constraint.isAlwaysEqual(simplifiedResult, 0); 23 | end 24 | end 25 | end -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/SymbolicExpressionComparator.m: -------------------------------------------------------------------------------- 1 | classdef SymbolicExpressionComparator < matlab.unittest.constraints.Comparator 2 | 3 | methods(Access=protected) 4 | function bool = supportsContainer(~, value) 5 | bool = isa(value, 'sym'); 6 | end 7 | 8 | function bool = containerSatisfiedBy(comparator, actual, expected) 9 | bool = comparator.isAlwaysEqual(actual, expected) || ... 10 | comparator.isSimplifiedEqual(actual, expected); 11 | end 12 | end 13 | 14 | methods(Access=private) 15 | function bool = isAlwaysEqual(~, actual, expected) 16 | result = isAlways(actual == expected, 'Unknown','false'); 17 | bool = all(result(:)); 18 | end 19 | 20 | function bool = isSimplifiedEqual(constraint, actual, expected) 21 | simplifiedResult = simplify(actual - expected, 'Steps', 1000); 22 | bool = constraint.isAlwaysEqual(simplifiedResult, 0); 23 | end 24 | end 25 | end -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/SymbolicExpressionComparator.m: -------------------------------------------------------------------------------- 1 | classdef SymbolicExpressionComparator < matlab.unittest.constraints.Comparator 2 | 3 | methods(Access=protected) 4 | function bool = supportsContainer(~, value) 5 | bool = isa(value, 'sym'); 6 | end 7 | 8 | function bool = containerSatisfiedBy(comparator, actual, expected) 9 | bool = comparator.isAlwaysEqual(actual, expected) || ... 10 | comparator.isSimplifiedEqual(actual, expected); 11 | end 12 | end 13 | 14 | methods(Access=private) 15 | function bool = isAlwaysEqual(~, actual, expected) 16 | result = isAlways(actual == expected, 'Unknown','false'); 17 | bool = all(result(:)); 18 | end 19 | 20 | function bool = isSimplifiedEqual(constraint, actual, expected) 21 | simplifiedResult = simplify(actual - expected, 'Steps', 1000); 22 | bool = constraint.isAlwaysEqual(simplifiedResult, 0); 23 | end 24 | end 25 | end -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test1.m: -------------------------------------------------------------------------------- 1 | %Create nonlinear motion model (coordinate turn) 2 | T = 1; 3 | sigmaV = 1; 4 | sigmaOmega = pi/180; 5 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 6 | %Set probability of existence 7 | P_S = rand; 8 | %Set Bernoulli RFS 9 | Bern.r = rand; 10 | Bern.state.x = rand(5,1); 11 | Bern.state.P = eye(5); 12 | 13 | % Run reference solution. 14 | PMBM_ref = reference.PMBMfilter(); 15 | PMBM_ref.density = feval(@GaussianDensity); 16 | Bern_ref = Bern_predict(PMBM_ref,Bern,motion_model,P_S); 17 | r_ref = Bern_ref.r; 18 | state_ref = Bern_ref.state; 19 | 20 | % Run learner solution. 21 | PMBM = PMBMfilter(); 22 | PMBM.density = feval(@GaussianDensity); 23 | Bern = Bern_predict(PMBM,Bern,motion_model,P_S); 24 | r = Bern.r; 25 | state = Bern.state; 26 | 27 | % Compare. 28 | assessVariableEqual('r', r_ref,'Feedback','Parameter Bern.r is incorrect, please modify your code and submit again.'); 29 | assessVariableEqual('state', state_ref,'Feedback','Parameter Bern.state is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test3.m: -------------------------------------------------------------------------------- 1 | %Set detection probability 2 | P_D = rand; 3 | %Create hypothesis tree 4 | Bern.r = rand; 5 | Bern.state.x = rand(5,1); 6 | Bern.state.P = eye(5); 7 | tt_entry = [1,1]; 8 | z = rand(2,3); 9 | %Create nonlinear measurement model (range/bearing) 10 | sigma_r = 5; 11 | sigma_b = pi/180; 12 | s = [300;400]; 13 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 14 | 15 | %Run learner solution. 16 | PMBM_ref = reference.PMBMfilter(); 17 | PMBM_ref.density = feval(@GaussianDensity); 18 | PMBM_ref.paras.MBM.tt{1} = Bern; 19 | lik_detected_ref = Bern_detected_update_lik(PMBM_ref,tt_entry,z,meas_model,P_D); 20 | 21 | % Run reference solution. 22 | PMBM = PMBMfilter(); 23 | PMBM.density = feval(@GaussianDensity); 24 | PMBM.paras.MBM.tt{1} = Bern; 25 | lik_detected = Bern_detected_update_lik(PMBM,tt_entry,z,meas_model,P_D); 26 | 27 | % Compare. 28 | assessVariableEqual('lik_detected', lik_detected_ref,'RelativeTolerance',0.0001,'Feedback','Parameter lik_detected is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/AssessmentDiagnostic.m: -------------------------------------------------------------------------------- 1 | classdef AssessmentDiagnostic < matlab.unittest.diagnostics.Diagnostic 2 | properties(SetAccess=private, GetAccess=public) 3 | DiagnosticIdentifier 4 | end 5 | 6 | methods 7 | function diag = AssessmentDiagnostic(identifier, value) 8 | % AssessmentDiagnostic - Class constructor 9 | % 10 | % AssessmentDiagnostic(IDENTIFIER, VALUE) creates a new AssessmentDiagnostic instance using 11 | % the VALUE provided. 12 | % 13 | % Examples: 14 | % 15 | % AssessmentDiagnostic('identifier', 'Diagnostic text'); 16 | % AssessmentDiagnostic('identifier', sprintf('This text is first created using %s', 'sprintf')); 17 | % 18 | validateattributes(identifier, {'char'}, {'2d'}, '', 'identifier'); 19 | validateattributes(value, {'char'}, {'2d'}, '', 'value'); 20 | diag.DiagnosticResult = strjoin(cellstr(value),'\n'); 21 | diag.DiagnosticIdentifier = identifier; 22 | end 23 | function diagnose(~) 24 | end 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/AssessmentDiagnostic.m: -------------------------------------------------------------------------------- 1 | classdef AssessmentDiagnostic < matlab.unittest.diagnostics.Diagnostic 2 | properties(SetAccess=private, GetAccess=public) 3 | DiagnosticIdentifier 4 | end 5 | 6 | methods 7 | function diag = AssessmentDiagnostic(identifier, value) 8 | % AssessmentDiagnostic - Class constructor 9 | % 10 | % AssessmentDiagnostic(IDENTIFIER, VALUE) creates a new AssessmentDiagnostic instance using 11 | % the VALUE provided. 12 | % 13 | % Examples: 14 | % 15 | % AssessmentDiagnostic('identifier', 'Diagnostic text'); 16 | % AssessmentDiagnostic('identifier', sprintf('This text is first created using %s', 'sprintf')); 17 | % 18 | validateattributes(identifier, {'char'}, {'2d'}, '', 'identifier'); 19 | validateattributes(value, {'char'}, {'2d'}, '', 'value'); 20 | diag.DiagnosticResult = strjoin(cellstr(value),'\n'); 21 | diag.DiagnosticIdentifier = identifier; 22 | end 23 | function diagnose(~) 24 | end 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/AssessmentDiagnostic.m: -------------------------------------------------------------------------------- 1 | classdef AssessmentDiagnostic < matlab.unittest.diagnostics.Diagnostic 2 | properties(SetAccess=private, GetAccess=public) 3 | DiagnosticIdentifier 4 | end 5 | 6 | methods 7 | function diag = AssessmentDiagnostic(identifier, value) 8 | % AssessmentDiagnostic - Class constructor 9 | % 10 | % AssessmentDiagnostic(IDENTIFIER, VALUE) creates a new AssessmentDiagnostic instance using 11 | % the VALUE provided. 12 | % 13 | % Examples: 14 | % 15 | % AssessmentDiagnostic('identifier', 'Diagnostic text'); 16 | % AssessmentDiagnostic('identifier', sprintf('This text is first created using %s', 'sprintf')); 17 | % 18 | validateattributes(identifier, {'char'}, {'2d'}, '', 'identifier'); 19 | validateattributes(value, {'char'}, {'2d'}, '', 'value'); 20 | diag.DiagnosticResult = strjoin(cellstr(value),'\n'); 21 | diag.DiagnosticIdentifier = identifier; 22 | end 23 | function diagnose(~) 24 | end 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/AssessmentDiagnostic.m: -------------------------------------------------------------------------------- 1 | classdef AssessmentDiagnostic < matlab.unittest.diagnostics.Diagnostic 2 | properties(SetAccess=private, GetAccess=public) 3 | DiagnosticIdentifier 4 | end 5 | 6 | methods 7 | function diag = AssessmentDiagnostic(identifier, value) 8 | % AssessmentDiagnostic - Class constructor 9 | % 10 | % AssessmentDiagnostic(IDENTIFIER, VALUE) creates a new AssessmentDiagnostic instance using 11 | % the VALUE provided. 12 | % 13 | % Examples: 14 | % 15 | % AssessmentDiagnostic('identifier', 'Diagnostic text'); 16 | % AssessmentDiagnostic('identifier', sprintf('This text is first created using %s', 'sprintf')); 17 | % 18 | validateattributes(identifier, {'char'}, {'2d'}, '', 'identifier'); 19 | validateattributes(value, {'char'}, {'2d'}, '', 'value'); 20 | diag.DiagnosticResult = strjoin(cellstr(value),'\n'); 21 | diag.DiagnosticIdentifier = identifier; 22 | end 23 | function diagnose(~) 24 | end 25 | end 26 | 27 | end -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex2Test3.m: -------------------------------------------------------------------------------- 1 | numGaussians = 10; 2 | 3 | %Moment matching 4 | numGaussians = 5; 5 | nDim = 4; 6 | states = []; 7 | for i = 1:numGaussians 8 | states(i,1).x = 5*randn(nDim,1); 9 | S = randn(nDim); 10 | states(i,1).P = S*S'; 11 | end 12 | 13 | w = abs(randn(numGaussians,1)); 14 | w = log(w/sum(w)); 15 | 16 | % Run learner solution. 17 | state = GaussianDensity.momentMatching(w, states); 18 | 19 | % Run reference solution. 20 | state_ref = reference.GaussianDensity.momentMatching(w, states); 21 | 22 | % Compare. 23 | tol = 1e-4; 24 | assert(norm(state.x-state_ref.x) < tol, 'The merged mean is not correctly computed. The Euclidean norm between your solution and the reference solution is %f. Your implementation is incorrect, please modify your code and submit again.', norm(state.x-state_ref.x)); 25 | assert(norm(state.P-state_ref.P) < tol, 'The merged covariance is not correctly computed. The Euclidean norm between your solution and the reference solution is %f. Your implementation is incorrect, please modify your code and submit again.', norm(state.P-state_ref.P)); 26 | % assessVariableEqual('state', state_ref,'RelativeTolerance',0.03,'Feedback','Your implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/HA3_test/SA4Ex1Test3.m: -------------------------------------------------------------------------------- 1 | numGaussian = 20; 2 | 3 | % Get generator settings. 4 | s = rng; 5 | 6 | % Run learner solution 7 | PPP = PHDfilter(); 8 | PPP.density = feval(@GaussianDensity); 9 | PPP.paras.w = log(rand(numGaussian,1)); 10 | for i = 1:numGaussian 11 | PPP.paras.states(i,1).x = rand(2,1); 12 | end 13 | 14 | estimates = PHD_estimator(PPP); 15 | 16 | [~,I] = sort(estimates(1,:)); 17 | estimates = estimates(:,I); 18 | num_objects = size(estimates,2); 19 | 20 | % Restore previous generator settings. 21 | rng(s); 22 | 23 | % Run reference solution. 24 | PPP_ref = reference.PHDfilter(); 25 | PPP_ref.density = feval(@GaussianDensity); 26 | PPP_ref.paras.w = log(rand(numGaussian,1)); 27 | for i = 1:numGaussian 28 | PPP_ref.paras.states(i,1).x = rand(2,1); 29 | end 30 | 31 | estimates_ref = PHD_estimator(PPP_ref); 32 | 33 | 34 | [~,I] = sort(estimates_ref(1,:)); 35 | estimates_ref = estimates_ref(:,I); 36 | num_objects_ref = size(estimates_ref,2); 37 | 38 | % Compare. 39 | assert(num_objects == num_objects_ref,'The number of estimated objects: Expected %d, got %d.', num_objects_ref, num_objects); 40 | assessVariableEqual('estimates', estimates_ref,'RelativeTolerance',0.001,'Feedback','Your implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/objectdatagen.m: -------------------------------------------------------------------------------- 1 | function objectdata = objectdatagen(groundtruth,motionmodel,ifnoisy) 2 | %OBJECTDATA generates groundtruth object data 3 | %INPUT: groundtruth specifies the parameters used to generate groundtruth 4 | % motionmodel: a structure specifies the motion model parameters 5 | % ifnoisy: boolean value indicating whether to generate noisy object 6 | % state sequence or not 7 | %OUTPUT: objectdata.X: (K x 1) cell array, each cell stores object states 8 | % of size (object state dimension) x (number of objects at 9 | % corresponding time step) 10 | % objectdata.N: (K x 1) cell array, each cell stores the number of 11 | % objects at corresponding time step 12 | 13 | %Preallocate memory 14 | K = groundtruth.K; 15 | objectdata.X = cell(K,1); 16 | objectdata.N = zeros(K,1); 17 | 18 | for i = 1:groundtruth.nbirths 19 | objectstate = groundtruth.xstart(:,i); 20 | for k = groundtruth.tbirth(i):min(groundtruth.tdeath(i),K) 21 | if ifnoisy 22 | objectstate = mvnrnd(motionmodel.f(objectstate), motionmodel.Q)'; 23 | else 24 | objectstate = motionmodel.f(objectstate); 25 | end 26 | objectdata.X{k} = [objectdata.X{k} objectstate]; 27 | objectdata.N(k) = objectdata.N(k) + 1; 28 | end 29 | end 30 | 31 | end -------------------------------------------------------------------------------- /HA02/objectdatagen.m: -------------------------------------------------------------------------------- 1 | function objectdata = objectdatagen(groundtruth,motionmodel,ifnoisy) 2 | %OBJECTDATA generates groundtruth object data 3 | %INPUT: groundtruth specifies the parameters used to generate groundtruth 4 | % motionmodel: a structure specifies the motion model parameters 5 | % ifnoisy: boolean value indicating whether to generate noisy object 6 | % state sequence or not 7 | %OUTPUT: objectdata.X: (K x 1) cell array, each cell stores object states 8 | % of size (object state dimension) x (number of objects at 9 | % corresponding time step) 10 | % objectdata.N: (K x 1) cell array, each cell stores the number of 11 | % objects at corresponding time step 12 | 13 | %Preallocate memory 14 | K = groundtruth.K; 15 | objectdata.X = cell(K,1); 16 | objectdata.N = zeros(K,1); 17 | 18 | for i = 1:groundtruth.nbirths 19 | objectstate = groundtruth.xstart(:,i); 20 | for k = groundtruth.tbirth(i):min(groundtruth.tdeath(i),K) 21 | if ifnoisy 22 | objectstate = mvnrnd(motionmodel.f(objectstate), motionmodel.Q)'; 23 | else 24 | objectstate = motionmodel.f(objectstate); 25 | end 26 | objectdata.X{k} = [objectdata.X{k} objectstate]; 27 | objectdata.N(k) = objectdata.N(k) + 1; 28 | end 29 | end 30 | 31 | end -------------------------------------------------------------------------------- /HA05/objectdatagen.m: -------------------------------------------------------------------------------- 1 | function objectdata = objectdatagen(groundtruth,motionmodel,ifnoisy) 2 | %TARGETDATA generates groundtruth object data 3 | %INPUT: groundtruth specifies the parameters used to generate groundtruth 4 | % motionmodel: a structure specifies the motion model parameters 5 | % ifnoisy: boolean value indicating whether to generate noisy object 6 | % state sequence or not 7 | %OUTPUT: objectdata.X: (K x 1) cell array, each cell stores object states 8 | % of size (object state dimension) x (number of objects at 9 | % corresponding time step) 10 | % objectdata.N: (K x 1) cell array, each cell stores the number of 11 | % objects at corresponding time step 12 | 13 | %Preallocate memory 14 | K = groundtruth.K; 15 | objectdata.X = cell(K,1); 16 | objectdata.N = zeros(K,1); 17 | 18 | for i = 1:groundtruth.nbirths 19 | objectstate = groundtruth.xstart(:,i); 20 | for k = groundtruth.tbirth(i):min(groundtruth.tdeath(i),K) 21 | if ifnoisy 22 | objectstate = mvnrnd(motionmodel.f(objectstate), motionmodel.Q)'; 23 | else 24 | objectstate = motionmodel.f(objectstate); 25 | end 26 | objectdata.X{k} = [objectdata.X{k} objectstate]; 27 | objectdata.N(k) = objectdata.N(k) + 1; 28 | end 29 | end 30 | 31 | end -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test4.m: -------------------------------------------------------------------------------- 1 | %Create hypothesis tree 2 | Bern.r = rand; 3 | Bern.state.x = rand(5,1); 4 | Bern.state.P = eye(5); 5 | tt_entry = [1,1]; 6 | z = rand(2,1); 7 | %Create nonlinear measurement model (range/bearing) 8 | sigma_r = 5; 9 | sigma_b = pi/180; 10 | s = [300;400]; 11 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 12 | 13 | %Run learner solution. 14 | PMBM_ref = reference.PMBMfilter(); 15 | PMBM_ref.density = feval(@GaussianDensity); 16 | PMBM_ref.paras.MBM.tt{1} = Bern; 17 | Bern_ref = Bern_detected_update_state(PMBM_ref,tt_entry,z,meas_model); 18 | r_ref = Bern_ref.r; 19 | state_ref = Bern_ref.state; 20 | 21 | % Run reference solution. 22 | PMBM = PMBMfilter(); 23 | PMBM.density = feval(@GaussianDensity); 24 | PMBM.paras.MBM.tt{1} = Bern; 25 | Bern = Bern_detected_update_state(PMBM,tt_entry,z,meas_model); 26 | r = Bern.r; 27 | state = Bern.state; 28 | 29 | % Compare. 30 | assessVariableEqual('r', r_ref,'Feedback','Parameter Bern.r is incorrect, please modify your code and submit again.'); 31 | assessVariableEqual('state', state_ref,'Feedback','Parameter Bern.state is incorrect, please modify your code and submit again.'); 32 | assessVariableEqual('Bern', Bern_ref,'RelativeTolerance',0.01,'Feedback','Parameter Bern is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA05/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2019, Yuxuan Xia E-mail: yuxuan.xia@chalmers.se 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex1Test1.m: -------------------------------------------------------------------------------- 1 | P_D = rand; 2 | lambda_c = rand; 3 | range_c = [-rand rand;-rand rand]; 4 | 5 | % Run learner solution. 6 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 7 | 8 | % Run reference solution. 9 | sensor_model_ref = reference.modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | % Compare. 12 | % assessVariableEqual('sensor_model', sensor_model_ref); 13 | tol = 1e-4; 14 | assert(abs(sensor_model.pdf_c-sensor_model_ref.pdf_c) < tol,'The value of clutter pdf 2D case: Expected %f, got %f.', sensor_model_ref.pdf_c, sensor_model.pdf_c); 15 | assert(abs(sensor_model.intensity_c-sensor_model_ref.intensity_c) < tol,'Clutter intensity 2D case: Expected %f, got %f.', sensor_model_ref.intensity_c, sensor_model.intensity_c); 16 | 17 | range_c = [-rand rand]; 18 | 19 | % Run learner solution. 20 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 21 | 22 | % Run reference solution. 23 | sensor_model_ref = reference.modelgen.sensormodel(P_D,lambda_c,range_c); 24 | 25 | % Compare. 26 | % assessVariableEqual('sensor_model', sensor_model_ref); 27 | tol = 1e-4; 28 | assert(abs(sensor_model.pdf_c-sensor_model_ref.pdf_c) < tol,'The value of clutter pdf 1D case: Expected %f, got %f.', sensor_model_ref.pdf_c, sensor_model.pdf_c); 29 | assert(abs(sensor_model.intensity_c-sensor_model_ref.intensity_c) < tol,'Clutter intensity 1D case: Expected %f, got %f.', sensor_model_ref.intensity_c, sensor_model.intensity_c); 30 | 31 | -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex2Test2.m: -------------------------------------------------------------------------------- 1 | nbirths = 1; 2 | K = 10; 3 | initial_state.x = [0; 0; 5; 5]; 4 | ground_truth = modelgen.groundtruth(nbirths,initial_state.x,1,K+1,K); 5 | 6 | T = 1; 7 | sigma_q = 2; 8 | motion_model = motionmodel.cvmodel(T,sigma_q); 9 | 10 | P_D = 1; 11 | lambda_c = 10; 12 | range_c = [-50 50;-50 50]; 13 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 14 | 15 | sigma_r = 5; 16 | meas_model = measmodel.cvmeasmodel(sigma_r); 17 | 18 | objectdata = objectdatagen(ground_truth,motion_model,0); 19 | 20 | measdata = measdatagen(objectdata,sensor_model,meas_model); 21 | 22 | 23 | for i = 1:K 24 | state.x = objectdata.X{i}; 25 | state.P = eye(motion_model.d); 26 | % Run learner solution. 27 | predict_likelihood = GaussianDensity.predictedLikelihood(state,measdata{i},meas_model); 28 | % Run reference solution. 29 | predict_likelihood_ref = reference.GaussianDensity.predictedLikelihood(state,measdata{i},meas_model); 30 | % Compare. 31 | assert(norm(predict_likelihood-predict_likelihood_ref) < 1e-3, 'The Euclidean norm between your solution and the reference solution is %f. Your implementation is incorrect, please modify your code and submit again.', norm(predict_likelihood-predict_likelihood_ref)); 32 | % assessVariableEqual('predict_likelihood', predict_likelihood_ref,'RelativeTolerance',0.01,'Feedback','Your implementation is incorrect, please modify your code and submit again.'); 33 | end 34 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi-Object-Tracking 2 | Repository for the course "Multi-Object Tracking for Automotive Systems" at EDX Chalmers University of Technology 3 | 4 | ## Home-Assignment 01 (HA01) - Single-Object Tracking in Clutter 5 | Implementation of the following algorithms: 6 | - [x] Nearest Neighbors Filter (NN) 7 | - [x] Probabilistic Data Association Filter (PDA) 8 | - [x] Gaussian Sum Filter (GSF) 9 | 10 | The main class is located at [HA01/singleobjectracker.m](./HA01/singleobjectracker.m) 11 | 12 | Simulations can be done using either [HA01/main.m](./HA01/main.m) or [HA01/simulation.m](./HA01/simulation.m) 13 | 14 | ## Home-Assignment 02 (HA02) - Tracking n Objects in Clutter 15 | Implementation of the following algorithms: 16 | - [x] Global Nearest Neighbors Filter (GNN) 17 | - [x] Joint Probabilistic Data Association Filter (JPDA) 18 | - [x] Track-oriented Multiple Hypothesis Tracker (TO-MHT) 19 | 20 | The main class is located at [HA02/n_objectracker.m](./HA02/n_objectracker.m) 21 | 22 | Simulations can be done using either [HA02/main.m](./HA02/main.m) or [HA02/simulation.m](./HA02/simulation.m) 23 | 24 | ## Home-Assignment 03 (HA03) - Random Finite Sets 25 | - [ ] Probability Hypothesis Density Filter (PHD) 26 | - [ ] Gaussian Mixture Probability Hypothesis Density Filter (GM-PHD) 27 | 28 | ## Home-Assignment 04 (HA04) - MOT Using Conjugate Priors 29 | - [ ] Multi-Bernoulli Mixture filter (MBM) 30 | - [ ] Poisson Multi-Bernoulli Mixture filter (PMBM) 31 | 32 | ## Home-Assigment 05 (HA05) - Extended Object Tracking 33 | -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test5.m: -------------------------------------------------------------------------------- 1 | %Create nonlinear motion model (coordinate turn) 2 | T = 1; 3 | sigmaV = 1; 4 | sigmaOmega = pi/180; 5 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 6 | %Set birth model 7 | birth_model = repmat(struct('w',log(rand),'x',rand(5,1),'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,4]); 8 | %Set probability of existence 9 | P_S = rand; 10 | %Set Poisson RFS 11 | PPP.w = log(rand(3,1)); 12 | PPP.states = repmat(struct('x',rand(5,1),'P',eye(5)),[3,1]); 13 | 14 | PPP.states(1).x = [0; 0; 5; 0; pi/180]; 15 | PPP.states(2).x = [20; 20; -20; 0; pi/90]; 16 | PPP.states(3).x = [-20; 10; -10; 0; pi/360]; 17 | 18 | % Run reference solution. 19 | PMBM_ref = reference.PMBMfilter(); 20 | PMBM_ref.density = feval(@GaussianDensity); 21 | PMBM_ref.paras.PPP.w = PPP.w; 22 | PMBM_ref.paras.PPP.states = PPP.states; 23 | PMBM_ref = PPP_predict(PMBM_ref,motion_model,birth_model,P_S); 24 | 25 | [PPP_w_ref,I] = sort(PMBM_ref.paras.PPP.w); 26 | PPP_states_ref = PMBM_ref.paras.PPP.states(I); 27 | 28 | % Run learner solution. 29 | PMBM = PMBMfilter(); 30 | %Create density handle 31 | PMBM.density = feval(@GaussianDensity); 32 | PMBM.paras.PPP.w = PPP.w; 33 | PMBM.paras.PPP.states = PPP.states; 34 | PMBM = PPP_predict(PMBM,motion_model,birth_model,P_S); 35 | 36 | [PPP_w,I] = sort(PMBM.paras.PPP.w); 37 | PPP_states = PMBM.paras.PPP.states(I); 38 | 39 | % Compare. 40 | assessVariableEqual('PPP_w', PPP_w_ref,'Feedback','Parameter PPP.w is incorrect, please modify your code and submit again.'); 41 | assessVariableEqual('PPP_states', PPP_states_ref,'Feedback','Parameter PPP.states is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/measdatagen.m: -------------------------------------------------------------------------------- 1 | function measdata = measdatagen(objectdata, sensormodel, measmodel) 2 | %MEASDATAGEN generates object-generated measurements and clutter 3 | %INPUT: objectdata: a structure contains object data 4 | % sensormodel: a structure specifies sensor model parameters 5 | % measmodel: a structure specifies the measurement model 6 | % parameters 7 | %OUTPUT: measdata: cell array of size (total tracking time, 1), each 8 | % cell stores measurements of size (measurement dimension) x 9 | % (number of measurements at corresponding time step) 10 | 11 | %Initialize memory 12 | measdata = cell(length(objectdata.X),1); 13 | 14 | %Generate measurements 15 | for k = 1:length(objectdata.X) 16 | if objectdata.N(k) > 0 17 | idx = rand(objectdata.N(k),1) <= sensormodel.P_D; 18 | %Only generate object-originated observations for detected objects 19 | if ~isempty(objectdata.X{k}(:,idx)) 20 | objectstates = objectdata.X{k}(:,idx); 21 | for i = 1:size(objectstates,2) 22 | meas = mvnrnd(measmodel.h(objectstates(:,i))', measmodel.R)'; 23 | measdata{k} = [measdata{k} meas]; 24 | end 25 | end 26 | end 27 | %Number of clutter measurements 28 | N_c = poissrnd(sensormodel.lambda_c); 29 | %Generate clutter 30 | C = repmat(sensormodel.range_c(:,1),[1 N_c])+ diag(sensormodel.range_c*[-1; 1])*rand(measmodel.d,N_c); 31 | %Total measurements are the union of object detections and clutter 32 | measdata{k}= [measdata{k} C]; 33 | end 34 | 35 | end -------------------------------------------------------------------------------- /HA02/measdatagen.m: -------------------------------------------------------------------------------- 1 | function measdata = measdatagen(objectdata, sensormodel, measmodel) 2 | %MEASDATAGEN generates object-generated measurements and clutter 3 | %INPUT: objectdata: a structure contains object data 4 | % sensormodel: a structure specifies sensor model parameters 5 | % measmodel: a structure specifies the measurement model 6 | % parameters 7 | %OUTPUT: measdata: cell array of size (total tracking time, 1), each 8 | % cell stores measurements of size (measurement dimension) x 9 | % (number of measurements at corresponding time step) 10 | 11 | %Initialize memory 12 | measdata = cell(length(objectdata.X),1); 13 | 14 | %Generate measurements 15 | for k = 1:length(objectdata.X) 16 | if objectdata.N(k) > 0 17 | idx = rand(objectdata.N(k),1) <= sensormodel.P_D; 18 | %Only generate object-originated observations for detected objects 19 | if ~isempty(objectdata.X{k}(:,idx)) 20 | objectstates = objectdata.X{k}(:,idx); 21 | for i = 1:size(objectstates,2) 22 | meas = mvnrnd(measmodel.h(objectstates(:,i))', measmodel.R)'; 23 | measdata{k} = [measdata{k} meas]; 24 | end 25 | end 26 | end 27 | %Number of clutter measurements 28 | N_c = poissrnd(sensormodel.lambda_c); 29 | %Generate clutter 30 | C = repmat(sensormodel.range_c(:,1),[1 N_c])+ diag(sensormodel.range_c*[-1; 1])*rand(measmodel.d,N_c); 31 | %Total measurements are the union of object detections and clutter 32 | measdata{k}= [measdata{k} C]; 33 | end 34 | 35 | end -------------------------------------------------------------------------------- /HA05/measdatagen.m: -------------------------------------------------------------------------------- 1 | function measdata = measdatagen(objectdata, sensormodel, measmodel) 2 | %MEASDATAGEN generates object-generated measurements and clutter 3 | %INPUT: objectdata: a structure contains object data 4 | % sensormodel: a structure specifies sensor model parameters 5 | % measmodel: a structure specifies the measurement model 6 | % parameters 7 | %OUTPUT: measdata: cell array of size (total tracking time, 1), each 8 | % cell stores measurements of size (measurement dimension) x 9 | % (number of measurements at corresponding time step) 10 | 11 | %Initialize memory 12 | measdata = cell(length(objectdata.X),1); 13 | 14 | %Generate measurements 15 | for k = 1:length(objectdata.X) 16 | if objectdata.N(k) > 0 17 | idx = rand(objectdata.N(k),1) <= sensormodel.P_D; 18 | %Only generate object-originated observations for detected objects 19 | if ~isempty(objectdata.X{k}(:,idx)) 20 | objectstates = objectdata.X{k}(:,idx); 21 | for i = 1:size(objectstates,2) 22 | meas = mvnrnd(measmodel.h(objectstates(:,i))', measmodel.R)'; 23 | measdata{k} = [measdata{k} meas]; 24 | end 25 | end 26 | end 27 | %Number of clutter measurements 28 | N_c = poissrnd(sensormodel.lambda_c); 29 | %Generate clutter 30 | if measmodel.d == 2 31 | C = repmat(sensormodel.range_c(:,1),[1 N_c])+ diag(sensormodel.range_c*[-1; 1])*rand(measmodel.d,N_c); 32 | elseif measmodel.d == 1 33 | C = (sensormodel.range_c(1,2)-sensormodel.range_c(1,1))*rand(measmodel.d,N_c)-sensormodel.range_c(1,2); 34 | end 35 | %Total measurements are the union of object detections and clutter 36 | measdata{k}= [measdata{k} C]; 37 | end 38 | 39 | end -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test6.m: -------------------------------------------------------------------------------- 1 | P_D = rand; 2 | clutter_intensity = rand/100; 3 | %Create nonlinear measurement model (range/bearing) 4 | sigma_r = 5; 5 | sigma_b = pi/180; 6 | s = [300;400]; 7 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 8 | %Set Poisson RFS 9 | PPP.w = log(rand(3,1)); 10 | PPP.states = repmat(struct('x',rand(5,1),'P',eye(5)),[3,1]); 11 | 12 | PPP.states(1).x = [0; 0; 5; 0; pi/180]; 13 | PPP.states(2).x = [20; 20; -20; 0; pi/90]; 14 | PPP.states(3).x = [-20; 10; -10; 0; pi/360]; 15 | 16 | indices = [true;false;true]; 17 | z = rand(2,1); 18 | 19 | % Run reference solution. 20 | PMBM_ref = reference.PMBMfilter(); 21 | PMBM_ref.density = feval(@GaussianDensity); 22 | PMBM_ref.paras.PPP.w = PPP.w; 23 | PMBM_ref.paras.PPP.states = PPP.states; 24 | [Bern_ref, lik_new_ref] = PPP_detected_update(PMBM_ref,indices,z,meas_model,P_D,clutter_intensity); 25 | r_ref = Bern_ref.r; 26 | state_ref = Bern_ref.state; 27 | x_ref = state_ref.x; 28 | P_ref = state_ref.P; 29 | 30 | % Run learner solution. 31 | PMBM = PMBMfilter(); 32 | %Create density handle 33 | PMBM.density = feval(@GaussianDensity); 34 | PMBM.paras.PPP.w = PPP.w; 35 | PMBM.paras.PPP.states = PPP.states; 36 | [Bern, lik_new] = PPP_detected_update(PMBM,indices,z,meas_model,P_D,clutter_intensity); 37 | r = Bern.r; 38 | state = Bern.state; 39 | x = state.x; 40 | P = state.P; 41 | 42 | % Compare. 43 | assessVariableEqual('r', r_ref,'RelativeTolerance',0.0001,'Feedback','Parameter Bern.r is incorrect, please modify your code and submit again.'); 44 | assessVariableEqual('x', x_ref,'AbsoluteTolerance',0.001,'Feedback','Parameter Bern.state.x is incorrect, please modify your code and submit again.'); 45 | assessVariableEqual('P', P_ref,'AbsoluteTolerance',0.001,'Feedback','Parameter Bern.state.P is incorrect, please modify your code and submit again.'); 46 | assessVariableEqual('lik_new', lik_new_ref,'RelativeTolerance',0.0001,'Feedback','Parameter lik_new is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex2Test1.m: -------------------------------------------------------------------------------- 1 | nbirths = 1; 2 | K = 10; 3 | initial_state.x = [0; 0; 5; 5]; 4 | ground_truth = modelgen.groundtruth(nbirths,initial_state.x,1,K+1,K); 5 | 6 | T = 1; 7 | sigma_q = 2; 8 | motion_model = motionmodel.cvmodel(T,sigma_q); 9 | 10 | P_D = 1; 11 | lambda_c = 60; 12 | range_c = [-200 200;-200 200]; 13 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 14 | 15 | sigma_r = 10; 16 | meas_model = measmodel.cvmeasmodel(sigma_r); 17 | 18 | objectdata = objectdatagen(ground_truth,motion_model,0); 19 | 20 | measdata = measdatagen(objectdata,sensor_model,meas_model); 21 | 22 | gating_size = chi2inv(0.9999,meas_model.d); 23 | 24 | for i = 1:K-1 25 | state.x = objectdata.X{i}; 26 | state.P = eye(motion_model.d); 27 | state_pred = GaussianDensity.predict(state, motion_model); 28 | % Run learner solution. 29 | [z_ingate, meas_in_gate] = GaussianDensity.ellipsoidalGating(state_pred, measdata{i+1}, meas_model, gating_size); 30 | m_ingate = size(z_ingate,2); 31 | if m_ingate > 1 32 | z_ingate = sort(z_ingate,2); 33 | end 34 | 35 | % Run reference solution. 36 | [z_ingate_ref, meas_in_gate_ref] = reference.GaussianDensity.ellipsoidalGating(state_pred, measdata{i+1}, meas_model, gating_size); 37 | m_ingate_ref = size(z_ingate_ref,2); 38 | if m_ingate_ref > 1 39 | z_ingate_ref = sort(z_ingate_ref,2); 40 | end 41 | 42 | % Compare. 43 | assessVariableEqual('meas_in_gate', meas_in_gate_ref,'Feedback','The output boolean vector does not match the reference solution. Your implementation is incorrect, please modify your code and submit again.'); 44 | assert(abs(m_ingate-m_ingate_ref) == 0, 'Number of measurements inside the gate: Expected %f, got %f. Your implementation is incorrect, please modify your code and submit again.', m_ingate_ref, m_ingate); 45 | assessVariableEqual('z_ingate', z_ingate_ref,'Feedback','The output measurements do not match the reference solution. Your implementation is incorrect, please modify your code and submit again.'); 46 | end 47 | -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test8.m: -------------------------------------------------------------------------------- 1 | %Create nonlinear motion model (coordinate turn) 2 | T = 1; 3 | sigmaV = 1; 4 | sigmaOmega = pi/180; 5 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 6 | %Set birth model 7 | birth_model = repmat(struct('w',log(rand),'x',rand(5,1),'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,4]); 8 | %Set probability of existence 9 | P_S = rand; 10 | %Set Poisson RFS 11 | PPP.w = log(rand(3,1)); 12 | PPP.states = repmat(struct('x',rand(5,1),'P',eye(5)),[3,1]); 13 | PPP.states(1).x = [0; 0; 5; 0; pi/180]; 14 | PPP.states(2).x = [20; 20; -20; 0; pi/90]; 15 | PPP.states(3).x = [-20; 10; -10; 0; pi/360]; 16 | %Set Bernoulli RFS 17 | tt = cell(2,1); 18 | tt{1} = repmat(struct('r',rand,'state',struct('x',rand(5,1),'P',eye(5))),[3,1]); 19 | tt{2} = repmat(struct('r',rand,'state',struct('x',rand(5,1),'P',eye(5))),[2,1]); 20 | 21 | % Run reference solution. 22 | PMBM_ref = reference.PMBMfilter(); 23 | PMBM_ref.density = feval(@GaussianDensity); 24 | PMBM_ref.paras.PPP.w = PPP.w; 25 | PMBM_ref.paras.PPP.states = PPP.states; 26 | PMBM_ref.paras.MBM.tt = tt; 27 | PMBM_ref = PMBM_predict(PMBM_ref,P_S,motion_model,birth_model); 28 | 29 | tt_ref = PMBM_ref.paras.MBM.tt; 30 | [PPP_w_ref,I] = sort(PMBM_ref.paras.PPP.w); 31 | PPP_states_ref = PMBM_ref.paras.PPP.states(I); 32 | 33 | % Run learner solution. 34 | PMBM = PMBMfilter(); 35 | %Create density handle 36 | PMBM.density = feval(@GaussianDensity); 37 | PMBM.paras.PPP.w = PPP.w; 38 | PMBM.paras.PPP.states = PPP.states; 39 | PMBM.paras.MBM.tt = tt; 40 | PMBM = PMBM_predict(PMBM,P_S,motion_model,birth_model); 41 | 42 | tt = PMBM.paras.MBM.tt; 43 | [PPP_w,I] = sort(PMBM.paras.PPP.w); 44 | PPP_states = PMBM.paras.PPP.states(I); 45 | 46 | % Compare. 47 | assessVariableEqual('PPP_w', PPP_w_ref,'RelativeTolerance',0.0001,'Feedback','Parameter obj.paras.PPP.w is incorrect, please modify your code and submit again.'); 48 | assessVariableEqual('PPP_states', PPP_states_ref,'RelativeTolerance',0.001,'Feedback','Parameter obj.paras.PPP.states is incorrect, please modify your code and submit again.'); 49 | assessVariableEqual('tt', tt_ref,'Feedback','Parameter obj.paras.MBM.tt is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA05/modelgen.m: -------------------------------------------------------------------------------- 1 | classdef modelgen 2 | %MODELGEN is a class used to create the tracking model 3 | 4 | methods (Static) 5 | function obj = sensormodel(P_D,lambda_c,range_c) 6 | %SENSORMODEL creates the sensor model 7 | %INPUT: P_D: object detection probability --- scalar 8 | % lambda_c: average number of clutter measurements per 9 | % time scan, Poisson distributed --- scalar 10 | % range_c: range of surveillance area --- if 2D model: 2 11 | % x 2 matrix of the form [xmin xmax;ymin ymax]; if 1D 12 | % model: 1 x 2 vector of the form [xmin xmax] 13 | %OUTPUT: obj.pdf_c: uniform clutter density --- scalar 14 | % obj.P_D: same as P_D 15 | % obj.lambda_c: same as lambda_c 16 | % obj.range_c: same as range_c 17 | % obj.intensity_c: uniform clutter intensity --- scalar 18 | obj.P_D = P_D; 19 | obj.lambda_c = lambda_c; 20 | obj.range_c = range_c; 21 | obj.pdf_c = 1/prod(range_c(:,2)-range_c(:,1)); 22 | obj.intensity_c = obj.lambda_c*obj.pdf_c; 23 | end 24 | 25 | function obj = groundtruth(nbirths,xstart,tbirth,tdeath,K) 26 | %GROUNDTRUTH specifies the parameters to generate groundtruth 27 | %INPUT: nbirths: number of objects hypothesised to exist from 28 | % time step 1 to time step K --- scalar 29 | % xstart: object initial state --- (object state 30 | % dimension) x nbirths matrix 31 | % tbirth: object birth (appearing) time --- (object 32 | % state dimension) x 1 vector 33 | % tdeath: object death (disappearing) time --- (object 34 | % state dimension) x 1 vector 35 | % K: total tracking time --- scalar 36 | obj.nbirths = nbirths; 37 | obj.xstart = xstart; 38 | obj.tbirth = tbirth; 39 | obj.tdeath = tdeath; 40 | obj.K = K; 41 | end 42 | 43 | end 44 | 45 | end -------------------------------------------------------------------------------- /HA02/assign2DByGibbs.m: -------------------------------------------------------------------------------- 1 | function [assignments,costs]= assign2DByGibbs(C,numIteration,k) 2 | %%KBEST2DASSIGN Find the k lowest cost 2D assignments for the 3 | % two-dimensional assignment problem with a rectangular cost 4 | % matrix C. 5 | % 6 | %INPUTS: C A numRowXnumCol cost matrix. numRow = number of objects. numCol 7 | % = number of objects + number of measurements 8 | % numIteration: number of iterations used in Gibbs sampling 9 | % k The number >=1 of hypotheses to generate. If k is less than the 10 | % total number of unique hypotheses, then all possible hypotheses 11 | % will be returned. 12 | % 13 | %OUTPUTS: col4rowBest A numRowXk matrix where the entry in each element 14 | % is an assignment of the element in that row to a 15 | % column. 0 entries signify unassigned rows. 16 | % costs A kX1 vector containing the sum of the values of the 17 | % assigned elements in C for all of the hypotheses. 18 | 19 | n = size(C,1); 20 | m = size(C,2) - n; 21 | 22 | assignments= zeros(n,numIteration); 23 | costs= zeros(numIteration,1); 24 | 25 | currsoln= m+1:m+n; %use all missed detections as initial solution 26 | assignments(:,1)= currsoln; 27 | costs(1)=sum(C(sub2ind(size(C),1:n,currsoln))); 28 | for sol= 2:numIteration 29 | for var= 1:n 30 | tempsamp= exp(-C(var,:)); %grab row of costs for current association variable 31 | tempsamp(currsoln([1:var-1,var+1:end]))= 0; %lock out current and previous iteration step assignments except for the one in question 32 | idxold= find(tempsamp>0); tempsamp= tempsamp(idxold); 33 | [~,currsoln(var)]= histc(rand(1,1),[0;cumsum(tempsamp(:))/sum(tempsamp)]); 34 | currsoln(var)= idxold(currsoln(var)); 35 | end 36 | assignments(:,sol)= currsoln; 37 | costs(sol)= sum(C(sub2ind(size(C),1:n,currsoln))); 38 | end 39 | [unique_assignments,I,~]= unique(assignments','rows'); 40 | assignments= unique_assignments'; 41 | costs= costs(I); 42 | 43 | if length(costs) > k 44 | [costs, sorted_idx] = sort(costs); 45 | costs = costs(1:k); 46 | assignments = assignments(:,sorted_idx(1:k)); 47 | end 48 | 49 | end -------------------------------------------------------------------------------- /HA05/assign2DByGibbs.m: -------------------------------------------------------------------------------- 1 | function [assignments,costs]= assign2DByGibbs(C,numIteration,k) 2 | %%KBEST2DASSIGN Find the k lowest cost 2D assignments for the 3 | % two-dimensional assignment problem with a rectangular cost 4 | % matrix C. 5 | % 6 | %INPUTS: C A numRowXnumCol cost matrix. numRow = number of objects. numCol 7 | % = number of objects + number of measurements 8 | % numIteration: number of iterations used in Gibbs sampling 9 | % k The number >=1 of hypotheses to generate. If k is less than the 10 | % total number of unique hypotheses, then all possible hypotheses 11 | % will be returned. 12 | % 13 | %OUTPUTS: col4rowBest A numRowXk matrix where the entry in each element 14 | % is an assignment of the element in that row to a 15 | % column. 0 entries signify unassigned rows. 16 | % costs A kX1 vector containing the sum of the values of the 17 | % assigned elements in C for all of the hypotheses. 18 | 19 | n = size(C,1); 20 | m = size(C,2) - n; 21 | 22 | assignments= zeros(n,numIteration); 23 | costs= zeros(numIteration,1); 24 | 25 | currsoln= m+1:m+n; %use all missed detections as initial solution 26 | assignments(:,1)= currsoln; 27 | costs(1)=sum(C(sub2ind(size(C),1:n,currsoln))); 28 | for sol= 2:numIteration 29 | for var= 1:n 30 | tempsamp= exp(-C(var,:)); %grab row of costs for current association variable 31 | tempsamp(currsoln([1:var-1,var+1:end]))= 0; %lock out current and previous iteration step assignments except for the one in question 32 | idxold= find(tempsamp>0); tempsamp= tempsamp(idxold); 33 | [~,currsoln(var)]= histc(rand(1,1),[0;cumsum(tempsamp(:))/sum(tempsamp)]); 34 | currsoln(var)= idxold(currsoln(var)); 35 | end 36 | assignments(:,sol)= currsoln; 37 | costs(sol)= sum(C(sub2ind(size(C),1:n,currsoln))); 38 | end 39 | [unique_assignments,I,~]= unique(assignments','rows'); 40 | assignments= unique_assignments'; 41 | costs= costs(I); 42 | 43 | if length(costs) > k 44 | [costs, sorted_idx] = sort(costs); 45 | costs = costs(1:k); 46 | assignments = assignments(:,sorted_idx(1:k)); 47 | end 48 | 49 | end -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex4Test3.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.7; 3 | %Choose clutter rate 4 | lambda_c = 60; 5 | 6 | %Create sensor model 7 | %Range/bearing measurement range 8 | range_c = [-1000 1000;-pi pi]; 9 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | %Creat ground truth model 12 | nbirths = 1; 13 | K = 50; 14 | initial_state.x = [0; 0; 10; 0; pi/180]; 15 | initial_state.P = diag([1 1 1 1*pi/180 1*pi/180].^2); 16 | ground_truth = modelgen.groundtruth(nbirths,initial_state.x,1,K+1,K); 17 | 18 | %Create nonlinear motion model (coordinate turn) 19 | T = 1; 20 | sigmaV = 1; 21 | sigmaOmega = pi/180; 22 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 23 | 24 | %Create nonlinear measurement model (range/bearing) 25 | sigma_r = 5; 26 | sigma_b = pi/180; 27 | s = [300;400]; 28 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 29 | 30 | %% Generate true object data (noisy or noiseless) and measurement data 31 | ifnoisy = 0; 32 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 33 | measdata = measdatagen(objectdata,sensor_model,meas_model); 34 | 35 | %% Single object tracker parameter setting 36 | P_G = 0.999; %gating size in percentage 37 | wmin = 1e-3; %hypothesis pruning threshold 38 | merging_threshold = 4; %hypothesis merging threshold 39 | M = 50; %maximum number of hypotheses kept in Gaussian sum filter 40 | density_class_handle = feval(@GaussianDensity); %density class handle 41 | 42 | 43 | % Run learner solution. 44 | tracker = singleobjectracker(); 45 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,wmin,merging_threshold,M); 46 | 47 | GaussianSumEstimates = GaussianSumFilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 48 | GaussianSumRMSE = RMSE(GaussianSumEstimates,objectdata.X); 49 | 50 | % Run reference solution. 51 | tracker_ref = reference.singleobjectracker(); 52 | tracker_ref = tracker_ref.initialize(density_class_handle,P_G,meas_model.d,wmin,merging_threshold,M); 53 | 54 | GaussianSumEstimates_ref = GaussianSumFilter(tracker_ref, initial_state, measdata, sensor_model, motion_model, meas_model); 55 | GaussianSumRMSE_ref = RMSE(GaussianSumEstimates_ref,objectdata.X); 56 | 57 | % Compare. 58 | assert(abs(GaussianSumRMSE-GaussianSumRMSE_ref) < 1e-3, 'Root Mean Square Error: Expected %f, got %f.', GaussianSumRMSE_ref, GaussianSumRMSE); 59 | assessVariableEqual('GaussianSumEstimates', GaussianSumEstimates_ref,'RelativeTolerance',0.03,'Feedback','Your Gaussian Sum Filter implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex4Test2.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.7; 3 | %Choose clutter rate 4 | lambda_c = 60; 5 | 6 | %Create sensor model 7 | %Range/bearing measurement range 8 | range_c = [-1000 1000;-pi pi]; 9 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | %Creat ground truth model 12 | nbirths = 1; 13 | K = 50; 14 | initial_state.x = [0; 0; 10; 0; pi/180]; 15 | initial_state.P = diag([1 1 1 1*pi/180 1*pi/180].^2); 16 | ground_truth = modelgen.groundtruth(nbirths,initial_state.x,1,K+1,K); 17 | 18 | %Create nonlinear motion model (coordinate turn) 19 | T = 1; 20 | sigmaV = 1; 21 | sigmaOmega = pi/180; 22 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 23 | 24 | %Create nonlinear measurement model (range/bearing) 25 | sigma_r = 5; 26 | sigma_b = pi/180; 27 | s = [300;400]; 28 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 29 | 30 | %% Generate true object data (noisy or noiseless) and measurement data 31 | ifnoisy = 0; 32 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 33 | measdata = measdatagen(objectdata,sensor_model,meas_model); 34 | 35 | %% Single object tracker parameter setting 36 | P_G = 0.999; %gating size in percentage 37 | wmin = 1e-3; %hypothesis pruning threshold 38 | merging_threshold = 4; %hypothesis merging threshold 39 | M = 50; %maximum number of hypotheses kept in Gaussian sum filter 40 | density_class_handle = feval(@GaussianDensity); %density class handle 41 | 42 | 43 | % Run learner solution. 44 | tracker = singleobjectracker(); 45 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,wmin,merging_threshold,M); 46 | 47 | probDataAssocEstimates = probDataAssocFilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 48 | probDataAssocRMSE = RMSE(probDataAssocEstimates,objectdata.X); 49 | 50 | % Run reference solution. 51 | tracker_ref = reference.singleobjectracker(); 52 | tracker_ref = tracker_ref.initialize(density_class_handle,P_G,meas_model.d,wmin,merging_threshold,M); 53 | 54 | probDataAssocEstimates_ref = probDataAssocFilter(tracker_ref, initial_state, measdata, sensor_model, motion_model, meas_model); 55 | probDataAssocRMSE_ref = RMSE(probDataAssocEstimates_ref,objectdata.X); 56 | 57 | % Compare. 58 | assert(abs(probDataAssocRMSE-probDataAssocRMSE_ref) < 1e-3, 'Root Mean Square Error: Expected %f, got %f.', probDataAssocRMSE_ref, probDataAssocRMSE); 59 | assessVariableEqual('probDataAssocEstimates', probDataAssocEstimates_ref,'RelativeTolerance',0.03,'Feedback','Your Probabilistic Data Association Filter implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/HA1_test/SA2Ex4Test1.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.7; 3 | %Choose clutter rate 4 | lambda_c = 60; 5 | 6 | %Create sensor model 7 | %Range/bearing measurement range 8 | range_c = [-1000 1000;-pi pi]; 9 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | %Creat ground truth model 12 | nbirths = 1; 13 | K = 50; 14 | initial_state.x = [0; 0; 10; 0; pi/180]; 15 | initial_state.P = diag([1 1 1 1*pi/180 1*pi/180].^2); 16 | ground_truth = modelgen.groundtruth(nbirths,initial_state.x,1,K+1,K); 17 | 18 | %Create nonlinear motion model (coordinate turn) 19 | T = 1; 20 | sigmaV = 1; 21 | sigmaOmega = pi/180; 22 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 23 | 24 | %Create nonlinear measurement model (range/bearing) 25 | sigma_r = 5; 26 | sigma_b = pi/180; 27 | s = [300;400]; 28 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 29 | 30 | %% Generate true object data (noisy or noiseless) and measurement data 31 | ifnoisy = 0; 32 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 33 | measdata = measdatagen(objectdata,sensor_model,meas_model); 34 | 35 | %% Single object tracker parameter setting 36 | P_G = 0.999; %gating size in percentage 37 | wmin = 1e-3; %hypothesis pruning threshold 38 | merging_threshold = 4; %hypothesis merging threshold 39 | M = 50; %maximum number of hypotheses kept in Gaussian sum filter 40 | density_class_handle = feval(@GaussianDensity); %density class handle 41 | 42 | 43 | % Run learner solution. 44 | tracker = singleobjectracker(); 45 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,wmin,merging_threshold,M); 46 | 47 | nearestNeighborEstimates = nearestNeighbourFilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 48 | nearestNeighborRMSE = RMSE(nearestNeighborEstimates,objectdata.X); 49 | 50 | % Run reference solution. 51 | tracker_ref = reference.singleobjectracker(); 52 | tracker_ref = tracker_ref.initialize(density_class_handle,P_G,meas_model.d,wmin,merging_threshold,M); 53 | 54 | nearestNeighborEstimates_ref = nearestNeighbourFilter(tracker_ref, initial_state, measdata, sensor_model, motion_model, meas_model); 55 | nearestNeighborRMSE_ref = RMSE(nearestNeighborEstimates_ref,objectdata.X); 56 | 57 | % Compare. 58 | assert(abs(nearestNeighborRMSE-nearestNeighborRMSE_ref) < 1e-3, 'Root Mean Square Error: Expected %f, got %f.', nearestNeighborRMSE_ref, nearestNeighborRMSE); 59 | assessVariableEqual('nearestNeighborEstimates', nearestNeighborEstimates_ref,'RelativeTolerance',0.03,'Feedback','Your Nearest Neighbour Filter implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/main.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.6; 3 | %Choose clutter rate 4 | lambda_c = 10 *5; 5 | %Create sensor model 6 | range_c = [-1000 1000;-1000 1000]; 7 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 8 | 9 | %Create ground truth model 10 | nbirths = 1; 11 | K = 100; 12 | initial_state.x = [0; 0; 10; 10]; 13 | ground_truth = modelgen.groundtruth(nbirths,initial_state.x,1,K+1,K); 14 | 15 | % initial guess 16 | initial_state.x = [10; 10; 0; 0]; 17 | initial_state.P = eye(4) * 300^2; 18 | 19 | 20 | %Create linear motion model 21 | T = 1; 22 | sigma_q = 5; 23 | motion_model = motionmodel.cvmodel(T,sigma_q); 24 | 25 | %Create linear measurement model 26 | sigma_r = 10; 27 | meas_model = measmodel.cvmeasmodel(sigma_r); 28 | 29 | %Generate true object data (noisy or noiseless) and measurement data 30 | ifnoisy = 0; 31 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 32 | measdata = measdatagen(objectdata,sensor_model,meas_model); 33 | 34 | %Single object tracker parameter setting 35 | P_G = 0.999; %gating size in percentage 36 | w_min = 1e-3; %hypothesis pruning threshold 37 | merging_threshold = 2; %hypothesis merging threshold 38 | M = 100; %maximum number of hypotheses kept in Gaussian sum filter 39 | density_class_handle = feval(@GaussianDensity); %density class handle 40 | tracker = singleobjectracker(); 41 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 42 | 43 | %Nearest neighbour filter 44 | [x_NN, P_NN] = nearestNeighbourFilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 45 | 46 | %Probabilistic data association filter 47 | [x_PDA, P_PDA] = probDataAssocFilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 48 | 49 | %Gaussian sum filter 50 | [x_GSF, P_GSF] = GaussianSumFilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 51 | 52 | 53 | est = struct('x',x_GSF, 'P', P_GSF) 54 | animate = Animate_2D_tracking(); 55 | animate.animate(est, initial_state, measdata, meas_model, range_c); 56 | 57 | 58 | 59 | true_state = cell2mat(objectdata.X'); 60 | NN_estimated_state = cell2mat(x_NN'); 61 | PDA_estimated_state = cell2mat(x_PDA'); 62 | GS_estimated_state = cell2mat(x_GSF'); 63 | 64 | figure 65 | hold on 66 | grid on 67 | 68 | plot(true_state(1,:), true_state(2,:), 'g','Linewidth', 2) 69 | plot(NN_estimated_state(1,:), NN_estimated_state(2,:), 'r-s' , 'Linewidth', 1) 70 | plot(PDA_estimated_state(1,:), PDA_estimated_state(2,:), 'm-o' , 'Linewidth', 1) 71 | plot(GS_estimated_state(1,:), GS_estimated_state(2,:), 'b-d' , 'Linewidth', 1) 72 | 73 | xlabel('x (m)') 74 | ylabel('y (m)') 75 | legend('Ground Truth','Nearest Neighbour', 'Probalistic Data Association', 'Gaussian Sum', 'Location', 'best') 76 | 77 | set(gca,'FontSize',12) -------------------------------------------------------------------------------- /HA01/modelgen.m: -------------------------------------------------------------------------------- 1 | classdef modelgen 2 | %MODELGEN is a class used to create the tracking model 3 | 4 | methods (Static) 5 | function obj = sensormodel(P_D,lambda_c,range_c) 6 | %SENSORMODEL creates the sensor model 7 | %INPUT: P_D: object detection probability --- scalar 8 | % lambda_c: average number of clutter measurements per 9 | % time scan, Poisson distributed --- scalar 10 | % range_c: range of surveillance area --- if 2D model: 2 11 | % x 2 matrix of the form [xmin xmax;ymin ymax]; if 1D 12 | % model: 1 x 2 vector of the form [xmin xmax] 13 | %OUTPUT: obj.pdf_c: uniform clutter density --- scalar 14 | % obj.P_D: same as P_D 15 | % obj.lambda_c: same as lambda_c 16 | % obj.range_c: same as range_c 17 | % obj.intensity_c: uniform clutter intensity --- scalar 18 | if size(range_c,1)>1 19 | obj.V = (range_c(1,2)-range_c(1,1))*(range_c(2,2)-range_c(2,1)); 20 | else 21 | obj.V = range_c(2)-range_c(1); 22 | end 23 | obj.pdf_c = 1 / obj.V; 24 | obj.P_D = P_D; 25 | obj.lambda_c = lambda_c; 26 | obj.range_c = range_c; 27 | obj.intensity_c = lambda_c * obj.pdf_c; 28 | end 29 | 30 | function obj = groundtruth(nbirths,xstart,tbirth,tdeath,K) 31 | %GROUNDTRUTH specifies the parameters to generate groundtruth 32 | %INPUT: nbirths: number of objects hypothesised to exist from 33 | % time step 1 to time step K --- scalar 34 | % xstart: object initial state --- (object state 35 | % dimension) x nbirths matrix 36 | % tbirth: object birth (appearing) time --- (total number 37 | % of objects existed in the scene) x 1 vector 38 | % tdeath: the last time the object exists --- (total number 39 | % of objects existed in the scene) x 1 vector 40 | % K: total tracking time --- scalar 41 | 42 | obj.nbirths = nbirths; 43 | obj.xstart = xstart; 44 | obj.tbirth = tbirth; 45 | obj.tdeath = tdeath; 46 | obj.K = K; 47 | end 48 | 49 | end 50 | 51 | end 52 | 53 | %% test 54 | 55 | % P_D = 0.9; 56 | % lambda_c = 30; 57 | % %2D Cartisian coordinate 58 | % range_c = [-100 100;-100 100]; 59 | % 60 | % obj = modelgen.sensormodel(P_D,lambda_c,range_c); 61 | % 62 | % disp(obj.pdf_c) 63 | % disp(obj.intensity_c) 64 | % 65 | % %1D Cartisian coordinate 66 | % range_c = [-100 100]; 67 | % 68 | % obj = modelgen.sensormodel(P_D,lambda_c,range_c); 69 | % 70 | % disp(obj.pdf_c) 71 | % disp(obj.intensity_c) 72 | 73 | -------------------------------------------------------------------------------- /HA02/modelgen.m: -------------------------------------------------------------------------------- 1 | classdef modelgen 2 | %MODELGEN is a class used to create the tracking model 3 | 4 | methods (Static) 5 | function obj = sensormodel(P_D,lambda_c,range_c) 6 | %SENSORMODEL creates the sensor model 7 | %INPUT: P_D: object detection probability --- scalar 8 | % lambda_c: average number of clutter measurements per 9 | % time scan, Poisson distributed --- scalar 10 | % range_c: range of surveillance area --- if 2D model: 2 11 | % x 2 matrix of the form [xmin xmax;ymin ymax]; if 1D 12 | % model: 1 x 2 vector of the form [xmin xmax] 13 | %OUTPUT: obj.pdf_c: uniform clutter density --- scalar 14 | % obj.P_D: same as P_D 15 | % obj.lambda_c: same as lambda_c 16 | % obj.range_c: same as range_c 17 | % obj.intensity_c: uniform clutter intensity --- scalar 18 | if size(range_c,1)>1 19 | obj.V = (range_c(1,2)-range_c(1,1))*(range_c(2,2)-range_c(2,1)); 20 | else 21 | obj.V = range_c(2)-range_c(1); 22 | end 23 | obj.pdf_c = 1 / obj.V; 24 | obj.P_D = P_D; 25 | obj.lambda_c = lambda_c; 26 | obj.range_c = range_c; 27 | obj.intensity_c = lambda_c * obj.pdf_c; 28 | end 29 | 30 | function obj = groundtruth(nbirths,xstart,tbirth,tdeath,K) 31 | %GROUNDTRUTH specifies the parameters to generate groundtruth 32 | %INPUT: nbirths: number of objects hypothesised to exist from 33 | % time step 1 to time step K --- scalar 34 | % xstart: object initial state --- (object state 35 | % dimension) x nbirths matrix 36 | % tbirth: object birth (appearing) time --- (total number 37 | % of objects existed in the scene) x 1 vector 38 | % tdeath: the last time the object exists --- (total number 39 | % of objects existed in the scene) x 1 vector 40 | % K: total tracking time --- scalar 41 | 42 | obj.nbirths = nbirths; 43 | obj.xstart = xstart; 44 | obj.tbirth = tbirth; 45 | obj.tdeath = tdeath; 46 | obj.K = K; 47 | end 48 | 49 | end 50 | 51 | end 52 | 53 | %% test 54 | 55 | % P_D = 0.9; 56 | % lambda_c = 30; 57 | % %2D Cartisian coordinate 58 | % range_c = [-100 100;-100 100]; 59 | % 60 | % obj = modelgen.sensormodel(P_D,lambda_c,range_c); 61 | % 62 | % disp(obj.pdf_c) 63 | % disp(obj.intensity_c) 64 | % 65 | % %1D Cartisian coordinate 66 | % range_c = [-100 100]; 67 | % 68 | % obj = modelgen.sensormodel(P_D,lambda_c,range_c); 69 | % 70 | % disp(obj.pdf_c) 71 | % disp(obj.intensity_c) 72 | 73 | -------------------------------------------------------------------------------- /HA02/main.m: -------------------------------------------------------------------------------- 1 | % rng(1); 2 | 3 | %Choose object detection probability 4 | P_D = 0.9; 5 | %Choose clutter rate 6 | lambda_c = 10; 7 | 8 | %Choose linear or nonlinear scenario 9 | scenario_type = 'linear'; 10 | 11 | %Creat sensor model 12 | range_c = [-1000 1000;-1000 1000]; 13 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 14 | 15 | %Creat linear motion model 16 | T = 1; 17 | sigma_q = 5; 18 | motion_model = motionmodel.cvmodel(T,sigma_q); 19 | 20 | %Create linear measurement model 21 | sigma_r = 10; 22 | meas_model = measmodel.cvmeasmodel(sigma_r); 23 | 24 | %Creat ground truth model 25 | nbirths = 5; 26 | K = 100; 27 | tbirth = zeros(nbirths,1); 28 | tdeath = zeros(nbirths,1); 29 | 30 | initial_state = repmat(struct('x',[],'P',eye(motion_model.d)*1e0),[1,nbirths]); 31 | 32 | initial_state(1).x = [0; 0; 0; -10]; tbirth(1) = 1; tdeath(1) = K; 33 | initial_state(2).x = [400; -600; -10; 5]; tbirth(2) = 1; tdeath(2) = K; 34 | initial_state(3).x = [-800; -200; 20; -5]; tbirth(3) = 1; tdeath(3) = K; 35 | initial_state(4).x = [0; 0; 7.5; -5]; tbirth(4) = 1; tdeath(4) = K; 36 | initial_state(5).x = [-200; 800; -3; -15]; tbirth(5) = 1; tdeath(5) = K; 37 | 38 | %% Generate true object data (noisy or noiseless) and measurement data 39 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 40 | ifnoisy = 0; 41 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 42 | measdata = measdatagen(objectdata,sensor_model,meas_model); 43 | 44 | %% N-object tracker parameter setting 45 | P_G = 0.999; %gating size in percentage 46 | w_min = 1e-3; %hypothesis pruning threshold 47 | merging_threshold = 2; %hypothesis merging threshold 48 | M = 100; %maximum number of hypotheses kept in TOMHT 49 | density_class_handle = feval(@GaussianDensity); %density class handle 50 | tracker = n_objectracker(); 51 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 52 | 53 | % [x_est, P_est] = GNNfilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 54 | % [x_est, P_est] = JPDAfilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 55 | [x_est, P_est] = TOMHT(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 56 | 57 | 58 | % animate = Animate_2D_tracking(); 59 | % animate.animate(struct('x',x_est, 'P', P_est), initial_state, measdata, meas_model, range_c); 60 | 61 | 62 | figure 63 | hold on 64 | grid on 65 | 66 | for i = 1:nbirths 67 | h1 = plot(cell2mat(cellfun(@(x) x(1,i), objectdata.X, 'UniformOutput', false)), ... 68 | cell2mat(cellfun(@(x) x(2,i), objectdata.X, 'UniformOutput', false)), 'g', 'Linewidth', 2); 69 | h2 = plot(cell2mat(cellfun(@(x) x(1,i), x_est, 'UniformOutput', false)), ... 70 | cell2mat(cellfun(@(x) x(2,i), x_est, 'UniformOutput', false)), '-s', 'Linewidth', 1); 71 | % pause(1); 72 | % drawnow(); 73 | end 74 | 75 | xlabel('x'); ylabel('y') 76 | 77 | xlim([-1000 1000]) 78 | ylim([-1000 1000]) 79 | 80 | legend([h1 h2],'Ground Truth','GNN Estimates', 'Location', 'best') 81 | 82 | set(gca,'FontSize',12) -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/HA2_test/SA3Ex1Test1.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.9; 3 | %Choose clutter rate 4 | lambda_c = 10; 5 | 6 | %Create sensor model 7 | %Range/bearing measurement range 8 | range_c = [-1000 1000;-pi pi]; 9 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | %Create nonlinear motion model (coordinate turn) 12 | T = 1; 13 | sigmaV = 1; 14 | sigmaOmega = pi/180; 15 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 16 | 17 | %Create nonlinear measurement model (range/bearing) 18 | sigma_r = 5; 19 | sigma_b = pi/180; 20 | s = [300;400]; 21 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 22 | 23 | %Creat ground truth model 24 | nbirths = 4; 25 | K = 20; 26 | 27 | initial_state = repmat(struct('x',[],'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,nbirths]); 28 | initial_state(1).x = [0; 0; 5; 0; pi/180]; tbirth(1) = 1; tdeath(1) = K; 29 | initial_state(2).x = [20; 20; -20; 0; pi/90]; tbirth(2) = 1; tdeath(2) = K; 30 | initial_state(3).x = [-20; 10; -10; 0; pi/360]; tbirth(3) = 1; tdeath(3) = K; 31 | initial_state(4).x = [-10; -10; 8; 0; pi/270]; tbirth(4) = 1; tdeath(4) = K; 32 | 33 | %% Generate true object data (noisy or noiseless) and measurement data 34 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 35 | ifnoisy = 0; 36 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 37 | measdata = measdatagen(objectdata,sensor_model,meas_model); 38 | 39 | %% N-object tracker parameter setting 40 | P_G = 0.999; %gating size in percentage 41 | w_min = 1e-3; %hypothesis pruning threshold 42 | merging_threshold = 2; %hypothesis merging threshold 43 | M = 100; %maximum number of hypotheses kept in MHT 44 | density_class_handle = feval(@GaussianDensity); %density class handle 45 | tracker = n_objectracker(); 46 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 47 | 48 | % Run learner solution. 49 | GNNestimates = GNNfilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 50 | GNN_RMSE = RMSE_n_objects(objectdata.X,GNNestimates); 51 | 52 | for k = 1:K 53 | [~,I] = sort(GNNestimates{k}(1,:)); 54 | GNNestimates{k} = GNNestimates{k}(:,I); 55 | end 56 | 57 | tracker_ref = reference.n_objectracker(); 58 | tracker_ref = tracker_ref.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 59 | 60 | % Run reference solution. 61 | GNNestimates_ref = GNNfilter(tracker_ref, initial_state, measdata, sensor_model, motion_model, meas_model); 62 | GNN_RMSE_ref = RMSE_n_objects(objectdata.X,GNNestimates_ref); 63 | 64 | 65 | for k = 1:K 66 | [~,I] = sort(GNNestimates_ref{k}(1,:)); 67 | GNNestimates_ref{k} = GNNestimates_ref{k}(:,I); 68 | end 69 | 70 | % Compare. 71 | assert(abs(GNN_RMSE-GNN_RMSE_ref) < 1e-3, 'Root Mean Square Error: Expected %f, got %f. Your GNN implementation is incorrect, please modify your code and submit again.', GNN_RMSE_ref, GNN_RMSE); 72 | assessVariableEqual('GNNestimates', GNNestimates_ref,'RelativeTolerance',0.03,'Feedback','Your GNN implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/HA2_test/SA3Ex1Test2.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.5; 3 | %Choose clutter rate 4 | lambda_c = 30; 5 | 6 | %Create sensor model 7 | %Range/bearing measurement range 8 | range_c = [-1000 1000;-pi pi]; 9 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | %Create nonlinear motion model (coordinate turn) 12 | T = 1; 13 | sigmaV = 1; 14 | sigmaOmega = pi/180; 15 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 16 | 17 | %Create nonlinear measurement model (range/bearing) 18 | sigma_r = 5; 19 | sigma_b = pi/180; 20 | s = [300;400]; 21 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 22 | 23 | %Creat ground truth model 24 | nbirths = 4; 25 | K = 20; 26 | 27 | initial_state = repmat(struct('x',[],'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,nbirths]); 28 | initial_state(1).x = [0; 0; 5; 0; pi/180]; tbirth(1) = 1; tdeath(1) = K; 29 | initial_state(2).x = [20; 20; -20; 0; pi/90]; tbirth(2) = 1; tdeath(2) = K; 30 | initial_state(3).x = [-20; 10; -10; 0; pi/360]; tbirth(3) = 1; tdeath(3) = K; 31 | initial_state(4).x = [-10; -10; 8; 0; pi/270]; tbirth(4) = 1; tdeath(4) = K; 32 | 33 | %% Generate true object data (noisy or noiseless) and measurement data 34 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 35 | ifnoisy = 0; 36 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 37 | measdata = measdatagen(objectdata,sensor_model,meas_model); 38 | 39 | %% N-object tracker parameter setting 40 | P_G = 0.999; %gating size in percentage 41 | w_min = 1e-3; %hypothesis pruning threshold 42 | merging_threshold = 2; %hypothesis merging threshold 43 | M = 100; %maximum number of hypotheses kept in MHT 44 | density_class_handle = feval(@GaussianDensity); %density class handle 45 | tracker = n_objectracker(); 46 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 47 | 48 | % Run learner solution. 49 | GNNestimates = GNNfilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 50 | GNN_RMSE = RMSE_n_objects(objectdata.X,GNNestimates); 51 | 52 | for k = 1:K 53 | [~,I] = sort(GNNestimates{k}(1,:)); 54 | GNNestimates{k} = GNNestimates{k}(:,I); 55 | end 56 | 57 | tracker_ref = reference.n_objectracker(); 58 | tracker_ref = tracker_ref.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 59 | 60 | % Run reference solution. 61 | GNNestimates_ref = GNNfilter(tracker_ref, initial_state, measdata, sensor_model, motion_model, meas_model); 62 | GNN_RMSE_ref = RMSE_n_objects(objectdata.X,GNNestimates_ref); 63 | 64 | 65 | for k = 1:K 66 | [~,I] = sort(GNNestimates_ref{k}(1,:)); 67 | GNNestimates_ref{k} = GNNestimates_ref{k}(:,I); 68 | end 69 | 70 | % Compare. 71 | assert(abs(GNN_RMSE-GNN_RMSE_ref) < 1e-3, 'Root Mean Square Error: Expected %f, got %f. Your GNN implementation is incorrect, please modify your code and submit again.', GNN_RMSE_ref, GNN_RMSE); 72 | assessVariableEqual('GNNestimates', GNNestimates_ref,'RelativeTolerance',0.03,'Feedback','Your GNN implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/HA2_test/SA3Ex3Test1.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.9; 3 | %Choose clutter rate 4 | lambda_c = 10; 5 | 6 | %Create sensor model 7 | %Range/bearing measurement range 8 | range_c = [-1000 1000;-pi pi]; 9 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | %Create nonlinear motion model (coordinate turn) 12 | T = 1; 13 | sigmaV = 1; 14 | sigmaOmega = pi/180; 15 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 16 | 17 | %Create nonlinear measurement model (range/bearing) 18 | sigma_r = 5; 19 | sigma_b = pi/180; 20 | s = [300;400]; 21 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 22 | 23 | %Creat ground truth model 24 | nbirths = 4; 25 | K = 20; 26 | 27 | initial_state = repmat(struct('x',[],'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,nbirths]); 28 | 29 | initial_state(1).x = [0; 0; 5; 0; pi/180]; tbirth(1) = 1; tdeath(1) = K; 30 | initial_state(2).x = [20; 20; -20; 0; pi/90]; tbirth(2) = 1; tdeath(2) = K; 31 | initial_state(3).x = [-20; 10; -10; 0; pi/360]; tbirth(3) = 1; tdeath(3) = K; 32 | initial_state(4).x = [-10; -10; 8; 0; pi/270]; tbirth(4) = 1; tdeath(4) = K; 33 | 34 | %% Generate true object data (noisy or noiseless) and measurement data 35 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 36 | ifnoisy = 0; 37 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 38 | measdata = measdatagen(objectdata,sensor_model,meas_model); 39 | 40 | %% N-object tracker parameter setting 41 | P_G = 0.999; %gating size in percentage 42 | w_min = 1e-3; %hypothesis pruning threshold 43 | merging_threshold = 2; %hypothesis merging threshold 44 | M = 100; %maximum number of hypotheses kept in MHT 45 | density_class_handle = feval(@GaussianDensity); %density class handle 46 | tracker = n_objectracker(); 47 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 48 | 49 | % Run learner solution. 50 | TOMHTestimates = TOMHT(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 51 | TOMHT_RMSE = RMSE_n_objects(objectdata.X,TOMHTestimates); 52 | 53 | for k = 1:K 54 | [~,I] = sort(TOMHTestimates{k}(1,:)); 55 | TOMHTestimates{k} = TOMHTestimates{k}(:,I); 56 | end 57 | 58 | 59 | tracker_ref = reference.n_objectracker(); 60 | tracker_ref = tracker_ref.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 61 | 62 | % Run reference solution. 63 | TOMHTestimates_ref = TOMHT(tracker_ref, initial_state, measdata, sensor_model, motion_model, meas_model); 64 | TOMHT_RMSE_ref = RMSE_n_objects(objectdata.X,TOMHTestimates_ref); 65 | 66 | for k = 1:K 67 | [~,I] = sort(TOMHTestimates_ref{k}(1,:)); 68 | TOMHTestimates_ref{k} = TOMHTestimates_ref{k}(:,I); 69 | end 70 | 71 | % Compare. 72 | assert(abs(TOMHT_RMSE-TOMHT_RMSE_ref) < 1e-3, 'Root Mean Square Error: Expected %f, got %f. Your TOMHT implementation is incorrect, please modify your code and submit again.', TOMHT_RMSE_ref, TOMHT_RMSE); 73 | assessVariableEqual('TOMHTestimates', TOMHTestimates_ref,'RelativeTolerance',0.03,'Feedback','Your implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/HA2_test/SA3Ex3Test2.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.5; 3 | %Choose clutter rate 4 | lambda_c = 30; 5 | 6 | %Create sensor model 7 | %Range/bearing measurement range 8 | range_c = [-1000 1000;-pi pi]; 9 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | %Create nonlinear motion model (coordinate turn) 12 | T = 1; 13 | sigmaV = 1; 14 | sigmaOmega = pi/180; 15 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 16 | 17 | %Create nonlinear measurement model (range/bearing) 18 | sigma_r = 5; 19 | sigma_b = pi/180; 20 | s = [300;400]; 21 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 22 | 23 | %Creat ground truth model 24 | nbirths = 4; 25 | K = 20; 26 | 27 | initial_state = repmat(struct('x',[],'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,nbirths]); 28 | 29 | initial_state(1).x = [0; 0; 5; 0; pi/180]; tbirth(1) = 1; tdeath(1) = K; 30 | initial_state(2).x = [20; 20; -20; 0; pi/90]; tbirth(2) = 1; tdeath(2) = K; 31 | initial_state(3).x = [-20; 10; -10; 0; pi/360]; tbirth(3) = 1; tdeath(3) = K; 32 | initial_state(4).x = [-10; -10; 8; 0; pi/270]; tbirth(4) = 1; tdeath(4) = K; 33 | 34 | %% Generate true object data (noisy or noiseless) and measurement data 35 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 36 | ifnoisy = 0; 37 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 38 | measdata = measdatagen(objectdata,sensor_model,meas_model); 39 | 40 | %% N-object tracker parameter setting 41 | P_G = 0.999; %gating size in percentage 42 | w_min = 1e-3; %hypothesis pruning threshold 43 | merging_threshold = 2; %hypothesis merging threshold 44 | M = 100; %maximum number of hypotheses kept in MHT 45 | density_class_handle = feval(@GaussianDensity); %density class handle 46 | tracker = n_objectracker(); 47 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 48 | 49 | % Run learner solution. 50 | TOMHTestimates = TOMHT(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 51 | TOMHT_RMSE = RMSE_n_objects(objectdata.X,TOMHTestimates); 52 | 53 | for k = 1:K 54 | [~,I] = sort(TOMHTestimates{k}(1,:)); 55 | TOMHTestimates{k} = TOMHTestimates{k}(:,I); 56 | end 57 | 58 | 59 | tracker_ref = reference.n_objectracker(); 60 | tracker_ref = tracker_ref.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 61 | 62 | % Run reference solution. 63 | TOMHTestimates_ref = TOMHT(tracker_ref, initial_state, measdata, sensor_model, motion_model, meas_model); 64 | TOMHT_RMSE_ref = RMSE_n_objects(objectdata.X,TOMHTestimates_ref); 65 | 66 | for k = 1:K 67 | [~,I] = sort(TOMHTestimates_ref{k}(1,:)); 68 | TOMHTestimates_ref{k} = TOMHTestimates_ref{k}(:,I); 69 | end 70 | 71 | % Compare. 72 | assert(abs(TOMHT_RMSE-TOMHT_RMSE_ref) < 1e-3, 'Root Mean Square Error: Expected %f, got %f. Your TOMHT implementation is incorrect, please modify your code and submit again.', TOMHT_RMSE_ref, TOMHT_RMSE); 73 | assessVariableEqual('TOMHTestimates', TOMHTestimates_ref,'RelativeTolerance',0.03,'Feedback','Your implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/HA2_test/SA3Ex2Test1.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.9; 3 | %Choose clutter rate 4 | lambda_c = 10; 5 | 6 | %Create sensor model 7 | %Range/bearing measurement range 8 | range_c = [-1000 1000;-pi pi]; 9 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | %Create nonlinear motion model (coordinate turn) 12 | T = 1; 13 | sigmaV = 1; 14 | sigmaOmega = pi/180; 15 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 16 | 17 | %Create nonlinear measurement model (range/bearing) 18 | sigma_r = 5; 19 | sigma_b = pi/180; 20 | s = [300;400]; 21 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 22 | 23 | %Creat ground truth model 24 | nbirths = 4; 25 | K = 20; 26 | initial_state = repmat(struct('x',[],'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,nbirths]); 27 | initial_state(1).x = [0; 0; 5; 0; pi/180]; tbirth(1) = 1; tdeath(1) = K; 28 | initial_state(2).x = [20; 20; -20; 0; pi/90]; tbirth(2) = 1; tdeath(2) = K; 29 | initial_state(3).x = [-20; 10; -10; 0; pi/360]; tbirth(3) = 1; tdeath(3) = K; 30 | initial_state(4).x = [-10; -10; 8; 0; pi/270]; tbirth(4) = 1; tdeath(4) = K; 31 | 32 | %% Generate true object data (noisy or noiseless) and measurement data 33 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 34 | ifnoisy = 0; 35 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 36 | measdata = measdatagen(objectdata,sensor_model,meas_model); 37 | 38 | %% N-object tracker parameter setting 39 | P_G = 0.999; %gating size in percentage 40 | w_min = 1e-3; %hypothesis pruning threshold 41 | merging_threshold = 2; %hypothesis merging threshold 42 | M = 100; %maximum number of hypotheses kept in MHT 43 | density_class_handle = feval(@GaussianDensity); %density class handle 44 | tracker = n_objectracker(); 45 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 46 | 47 | 48 | % Run learner solution. 49 | JPDAestimates = JPDAfilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 50 | JPDA_RMSE = RMSE_n_objects(objectdata.X,JPDAestimates); 51 | 52 | for k = 1:K 53 | [~,I] = sort(JPDAestimates{k}(1,:)); 54 | JPDAestimates{k} = JPDAestimates{k}(:,I); 55 | end 56 | 57 | 58 | tracker_ref = reference.n_objectracker(); 59 | tracker_ref = tracker_ref.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 60 | 61 | % Run reference solution. 62 | JPDAestimates_ref = JPDAfilter(tracker_ref, initial_state, measdata, sensor_model, motion_model, meas_model); 63 | JPDA_RMSE_ref = RMSE_n_objects(objectdata.X,JPDAestimates_ref); 64 | 65 | for k = 1:K 66 | [~,I] = sort(JPDAestimates_ref{k}(1,:)); 67 | JPDAestimates_ref{k} = JPDAestimates_ref{k}(:,I); 68 | end 69 | 70 | % Compare. 71 | assert(abs(JPDA_RMSE-JPDA_RMSE_ref) < 1e-3, 'Root Mean Square Error: Expected %f, got %f. Your JPDA implementation is incorrect, please modify your code and submit again.', JPDA_RMSE_ref, JPDA_RMSE); 72 | assessVariableEqual('JPDAestimates', JPDAestimates_ref,'RelativeTolerance',0.03,'Feedback','Your JPDA implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/HA2_test/SA3Ex2Test2.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.5; 3 | %Choose clutter rate 4 | lambda_c = 30; 5 | 6 | %Create sensor model 7 | %Range/bearing measurement range 8 | range_c = [-1000 1000;-pi pi]; 9 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 10 | 11 | %Create nonlinear motion model (coordinate turn) 12 | T = 1; 13 | sigmaV = 1; 14 | sigmaOmega = pi/180; 15 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 16 | 17 | %Create nonlinear measurement model (range/bearing) 18 | sigma_r = 5; 19 | sigma_b = pi/180; 20 | s = [300;400]; 21 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 22 | 23 | %Creat ground truth model 24 | nbirths = 4; 25 | K = 20; 26 | initial_state = repmat(struct('x',[],'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,nbirths]); 27 | initial_state(1).x = [0; 0; 5; 0; pi/180]; tbirth(1) = 1; tdeath(1) = K; 28 | initial_state(2).x = [20; 20; -20; 0; pi/90]; tbirth(2) = 1; tdeath(2) = K; 29 | initial_state(3).x = [-20; 10; -10; 0; pi/360]; tbirth(3) = 1; tdeath(3) = K; 30 | initial_state(4).x = [-10; -10; 8; 0; pi/270]; tbirth(4) = 1; tdeath(4) = K; 31 | 32 | %% Generate true object data (noisy or noiseless) and measurement data 33 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 34 | ifnoisy = 0; 35 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 36 | measdata = measdatagen(objectdata,sensor_model,meas_model); 37 | 38 | %% N-object tracker parameter setting 39 | P_G = 0.999; %gating size in percentage 40 | w_min = 1e-3; %hypothesis pruning threshold 41 | merging_threshold = 2; %hypothesis merging threshold 42 | M = 100; %maximum number of hypotheses kept in MHT 43 | density_class_handle = feval(@GaussianDensity); %density class handle 44 | tracker = n_objectracker(); 45 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 46 | 47 | 48 | % Run learner solution. 49 | JPDAestimates = JPDAfilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 50 | JPDA_RMSE = RMSE_n_objects(objectdata.X,JPDAestimates); 51 | 52 | for k = 1:K 53 | [~,I] = sort(JPDAestimates{k}(1,:)); 54 | JPDAestimates{k} = JPDAestimates{k}(:,I); 55 | end 56 | 57 | 58 | tracker_ref = reference.n_objectracker(); 59 | tracker_ref = tracker_ref.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 60 | 61 | % Run reference solution. 62 | JPDAestimates_ref = JPDAfilter(tracker_ref, initial_state, measdata, sensor_model, motion_model, meas_model); 63 | JPDA_RMSE_ref = RMSE_n_objects(objectdata.X,JPDAestimates_ref); 64 | 65 | for k = 1:K 66 | [~,I] = sort(JPDAestimates_ref{k}(1,:)); 67 | JPDAestimates_ref{k} = JPDAestimates_ref{k}(:,I); 68 | end 69 | 70 | % Compare. 71 | assert(abs(JPDA_RMSE-JPDA_RMSE_ref) < 1e-3, 'Root Mean Square Error: Expected %f, got %f. Your JPDA implementation is incorrect, please modify your code and submit again.', JPDA_RMSE_ref, JPDA_RMSE); 72 | assessVariableEqual('JPDAestimates', JPDAestimates_ref,'RelativeTolerance',0.03,'Feedback','Your JPDA implementation is incorrect, please modify your code and submit again.'); -------------------------------------------------------------------------------- /HA01/motionmodel.m: -------------------------------------------------------------------------------- 1 | classdef motionmodel 2 | %MOTIONMODEL is a class containing different motion models 3 | 4 | methods (Static) 5 | function obj = cvmodel(T,sigma) 6 | %CVMODEL creates a 2D nearly constant velocity model 7 | %INPUT: T: sampling time --- scalar 8 | % sigma: standard deviation of motion noise --- scalar 9 | %OUTPUT:obj.d: object state dimension --- scalar 10 | % obj.F: function handle return a motion transition 11 | % matrix --- 2 x 2 matrix 12 | % obj.Q: motion noise covariance --- 4 x 4 matrix 13 | % obj.f: function handle return state prediction --- 4 x 14 | % 1 vector 15 | %NOTE: the motion model assumes that the state vector x 16 | % consists of the following states: 17 | % px --- X-position 18 | % py --- Y-position 19 | % vx --- X-velocity 20 | % vy --- Y-velocity 21 | obj.d = 4; 22 | obj.F = @(x) [ 23 | 1 0 T 0; 24 | 0 1 0 T; 25 | 0 0 1 0; 26 | 0 0 0 1]; 27 | obj.Q = sigma^2*[ 28 | T^4/4 0 T^3/2 0; 29 | 0 T^4/4 0 T^3/2; 30 | T^3/2 0 T^2 0; 31 | 0 T^3/2 0 T^2; 32 | ]; 33 | obj.f = @(x) obj.F(x)*x; 34 | end 35 | 36 | function obj = ctmodel(T,sigmaV,sigmaOmega) 37 | %CTMODEL creates a 2D coordinate turn model with nearly 38 | %constant polar velocity and turn rate 39 | %INPUT: T: sampling time --- scalar 40 | % sigmaV: standard deviation of motion noise added to 41 | % polar velocity --- scalar 42 | % sigmaOmega: standard deviation of motion noise added to 43 | % turn rate --- scalar 44 | %OUTPUT:obj.d: object state dimension --- scalar 45 | % obj.F: function handle return a motion Jacobian matrix 46 | % --- 5 x 5 matrix 47 | % obj.f: function handle return state prediction --- 5 x 48 | % 1 vector 49 | % obj.Q: motion noise covariance --- 5 x 5 matrix 50 | %NOTE: the motion model assumes that the state vector x 51 | %consists of the following states: 52 | % px --- X-position 53 | % py --- Y-position 54 | % v --- velocity 55 | % phi --- heading 56 | % omega --- turn-rate 57 | obj.d = 5; 58 | obj.f = @(x) x + [ 59 | T*x(3)*cos(x(4)); 60 | T*x(3)*sin(x(4)); 61 | 0; 62 | T*x(5); 63 | 0]; 64 | obj.F = @(x) [ 65 | 1 0 T*cos(x(4)) -T*x(3)*sin(x(4)) 0; 66 | 0 1 T*sin(x(4)) T*x(3)*cos(x(4)) 0; 67 | 0 0 1 0 0; 68 | 0 0 0 1 T; 69 | 0 0 0 0 1 70 | ]; 71 | G = [zeros(2,2); 1 0; 0 0; 0 1]; 72 | obj.Q = G*diag([sigmaV^2 sigmaOmega^2])*G'; 73 | end 74 | 75 | end 76 | end -------------------------------------------------------------------------------- /HA02/motionmodel.m: -------------------------------------------------------------------------------- 1 | classdef motionmodel 2 | %MOTIONMODEL is a class containing different motion models 3 | 4 | methods (Static) 5 | function obj = cvmodel(T,sigma) 6 | %CVMODEL creates a 2D nearly constant velocity model 7 | %INPUT: T: sampling time --- scalar 8 | % sigma: standard deviation of motion noise --- scalar 9 | %OUTPUT:obj.d: object state dimension --- scalar 10 | % obj.F: function handle return a motion transition 11 | % matrix --- 2 x 2 matrix 12 | % obj.Q: motion noise covariance --- 4 x 4 matrix 13 | % obj.f: function handle return state prediction --- 4 x 14 | % 1 vector 15 | %NOTE: the motion model assumes that the state vector x 16 | % consists of the following states: 17 | % px --- X-position 18 | % py --- Y-position 19 | % vx --- X-velocity 20 | % vy --- Y-velocity 21 | obj.d = 4; 22 | obj.F = @(x) [ 23 | 1 0 T 0; 24 | 0 1 0 T; 25 | 0 0 1 0; 26 | 0 0 0 1]; 27 | obj.Q = sigma^2*[ 28 | T^4/4 0 T^3/2 0; 29 | 0 T^4/4 0 T^3/2; 30 | T^3/2 0 T^2 0; 31 | 0 T^3/2 0 T^2; 32 | ]; 33 | obj.f = @(x) obj.F(x)*x; 34 | end 35 | 36 | function obj = ctmodel(T,sigmaV,sigmaOmega) 37 | %CTMODEL creates a 2D coordinate turn model with nearly 38 | %constant polar velocity and turn rate 39 | %INPUT: T: sampling time --- scalar 40 | % sigmaV: standard deviation of motion noise added to 41 | % polar velocity --- scalar 42 | % sigmaOmega: standard deviation of motion noise added to 43 | % turn rate --- scalar 44 | %OUTPUT:obj.d: object state dimension --- scalar 45 | % obj.F: function handle return a motion Jacobian matrix 46 | % --- 5 x 5 matrix 47 | % obj.f: function handle return state prediction --- 5 x 48 | % 1 vector 49 | % obj.Q: motion noise covariance --- 5 x 5 matrix 50 | %NOTE: the motion model assumes that the state vector x 51 | %consists of the following states: 52 | % px --- X-position 53 | % py --- Y-position 54 | % v --- velocity 55 | % phi --- heading 56 | % omega --- turn-rate 57 | obj.d = 5; 58 | obj.f = @(x) x + [ 59 | T*x(3)*cos(x(4)); 60 | T*x(3)*sin(x(4)); 61 | 0; 62 | T*x(5); 63 | 0]; 64 | obj.F = @(x) [ 65 | 1 0 T*cos(x(4)) -T*x(3)*sin(x(4)) 0; 66 | 0 1 T*sin(x(4)) T*x(3)*cos(x(4)) 0; 67 | 0 0 1 0 0; 68 | 0 0 0 1 T; 69 | 0 0 0 0 1 70 | ]; 71 | G = [zeros(2,2); 1 0; 0 0; 0 1]; 72 | obj.Q = G*diag([sigmaV^2 sigmaOmega^2])*G'; 73 | end 74 | 75 | end 76 | end -------------------------------------------------------------------------------- /HA05/motionmodel.m: -------------------------------------------------------------------------------- 1 | classdef motionmodel 2 | %MOTIONMODEL is a class containing different motion models 3 | 4 | methods (Static) 5 | function obj = cvmodel(T,sigma) 6 | %CVMODEL creates a 2D nearly constant velocity model 7 | %INPUT: T: sampling time --- scalar 8 | % sigma: standard deviation of motion noise --- scalar 9 | %OUTPUT:obj.d: object state dimension --- scalar 10 | % obj.F: function handle return a motion transition 11 | % matrix --- 2 x 2 matrix 12 | % obj.Q: motion noise covariance --- 4 x 4 matrix 13 | % obj.f: function handle return state prediction --- 4 x 14 | % 1 vector 15 | %NOTE: the motion model assumes that the state vector x 16 | % consists of the following states: 17 | % px --- X-position 18 | % py --- Y-position 19 | % vx --- X-velocity 20 | % vy --- Y-velocity 21 | obj.d = 4; 22 | obj.F = @(x) [ 23 | 1 0 T 0; 24 | 0 1 0 T; 25 | 0 0 1 0; 26 | 0 0 0 1]; 27 | obj.Q = sigma^2*[ 28 | T^4/4 0 T^3/2 0; 29 | 0 T^4/4 0 T^3/2; 30 | T^3/2 0 T^2 0; 31 | 0 T^3/2 0 T^2; 32 | ]; 33 | obj.f = @(x) obj.F(x)*x; 34 | end 35 | 36 | function obj = ctmodel(T,sigmaV,sigmaOmega) 37 | %CTMODEL creates a 2D coordinate turn model with nearly 38 | %constant polar velocity and turn rate 39 | %INPUT: T: sampling time --- scalar 40 | % sigmaV: standard deviation of motion noise added to 41 | % polar velocity --- scalar 42 | % sigmaOmega: standard deviation of motion noise added to 43 | % turn rate --- scalar 44 | %OUTPUT:obj.d: object state dimension --- scalar 45 | % obj.F: function handle return a motion Jacobian matrix 46 | % --- 5 x 5 matrix 47 | % obj.f: function handle return state prediction --- 5 x 48 | % 1 vector 49 | % obj.Q: motion noise covariance --- 5 x 5 matrix 50 | %NOTE: the motion model assumes that the state vector x 51 | %consists of the following states: 52 | % px --- X-position 53 | % py --- Y-position 54 | % v --- velocity 55 | % phi --- heading 56 | % omega --- turn-rate 57 | obj.d = 5; 58 | obj.f = @(x) x + [ 59 | T*x(3)*cos(x(4)); 60 | T*x(3)*sin(x(4)); 61 | 0; 62 | T*x(5); 63 | 0]; 64 | obj.F = @(x) [ 65 | 1 0 T*cos(x(4)) -T*x(3)*sin(x(4)) 0; 66 | 0 1 T*sin(x(4)) T*x(3)*cos(x(4)) 0; 67 | 0 0 1 0 0; 68 | 0 0 0 1 T; 69 | 0 0 0 0 1 70 | ]; 71 | G = [zeros(2,2); 1 0; 0 0; 0 1]; 72 | obj.Q = G*diag([sigmaV^2 sigmaOmega^2])*G'; 73 | end 74 | 75 | end 76 | end -------------------------------------------------------------------------------- /HA03/main.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.98; 3 | %Choose clutter rate 4 | lambda_c = 5; 5 | %Choose object survival probability 6 | P_S = 0.99; 7 | 8 | %Create sensor model 9 | range_c = [-1000 1000;-1000 1000]; 10 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 11 | 12 | %Create linear motion model 13 | T = 1; 14 | sigma_q = 5; 15 | motion_model = motionmodel.cvmodel(T,sigma_q); 16 | 17 | %Create linear measurement model 18 | sigma_r = 10; 19 | meas_model = measmodel.cvmeasmodel(sigma_r); 20 | 21 | %Create ground truth model 22 | nbirths = 12; 23 | K = 100; 24 | tbirth = zeros(nbirths,1); 25 | tdeath = zeros(nbirths,1); 26 | 27 | initial_state(1).x = [ 0; 0; 0; -10 ]; tbirth(1) = 1; tdeath(1) = 70; 28 | initial_state(2).x = [ 400; -600; -10; 5 ]; tbirth(2) = 1; tdeath(2) = K+1; 29 | initial_state(3).x = [ -800; -200; 20; -5 ]; tbirth(3) = 1; tdeath(3) = 70; 30 | initial_state(4).x = [ 400; -600; -7; -4 ]; tbirth(4) = 20; tdeath(4) = K+1; 31 | initial_state(5).x = [ 400; -600; -2.5; 10 ]; tbirth(5) = 20; tdeath(5) = K+1; 32 | initial_state(6).x = [ 0; 0; 7.5; -5 ]; tbirth(6) = 20; tdeath(6) = K+1; 33 | initial_state(7).x = [ -800; -200; 12; 7 ]; tbirth(7) = 40; tdeath(7) = K+1; 34 | initial_state(8).x = [ -200; 800; 15; -10 ]; tbirth(8) = 40; tdeath(8) = K+1; 35 | initial_state(9).x = [ -800; -200; 3; 15 ]; tbirth(9) = 60; tdeath(9) = K+1; 36 | initial_state(10).x = [ -200; 800; -3; -15 ]; tbirth(10) = 60; tdeath(10) = K+1; 37 | initial_state(11).x = [ 0; 0; -20; -15 ]; tbirth(11) = 80; tdeath(11) = K+1; 38 | initial_state(12).x = [ -200; 800; 15; -5 ]; tbirth(12) = 80; tdeath(12) = K+1; 39 | 40 | birth_model = repmat(struct('w',log(0.03),'x',[],'P',400*eye(motion_model.d)),[1,4]); 41 | birth_model(1).x = [ 0; 0; 0; 0]; 42 | birth_model(2).x = [ 400; -600; 0; 0]; 43 | birth_model(3).x = [ -800; -200; 0; 0 ]; 44 | birth_model(4).x = [ -200; 800; 0; 0 ]; 45 | 46 | %Generate true object data (noisy or noiseless) and measurement data 47 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 48 | ifnoisy = 0; 49 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 50 | measdata = measdatagen(objectdata,sensor_model,meas_model); 51 | 52 | %Object tracker parameter setting 53 | P_G = 0.999; %gating size in percentage 54 | w_min = 1e-3; %hypothesis pruning threshold 55 | merging_threshold = 4; %hypothesis merging threshold 56 | M = 100; %maximum number of hypotheses kept in PHD 57 | density_class_handle = feval(@GaussianDensity); %density class handle 58 | 59 | %Please check the provided script multiobjectracker_HA3 for details of the function multiobjectracker 60 | tracker = multiobjectracker(); 61 | tracker = tracker.initialize(density_class_handle,P_S,P_G,meas_model.d,w_min,merging_threshold,M); 62 | 63 | %GM-PHD filter 64 | GMPHDestimates = GMPHDfilter(tracker, birth_model, measdata, sensor_model, motion_model, meas_model); 65 | 66 | %Trajectory plot 67 | true_state = cell2mat(objectdata.X'); 68 | GMPHD_estimated_state = cell2mat(GMPHDestimates'); 69 | 70 | figure 71 | hold on 72 | grid on 73 | 74 | h1 = plot(true_state(1,:), true_state(2,:), 'bo', 'Linewidth', 1); 75 | h2 = plot(GMPHD_estimated_state(1,:), GMPHD_estimated_state(2,:), 'r+', 'Linewidth', 1); 76 | 77 | xlabel('x (m)'); ylabel('y (m)') 78 | legend([h1 h2],'Ground Truth','PHD Estimates', 'Location', 'best') 79 | set(gca,'FontSize',12) -------------------------------------------------------------------------------- /HA03/trackgen.m: -------------------------------------------------------------------------------- 1 | function [object_tracks] = trackgen(K,measmodel,motionmodel,sensormodel,birthmodel,P_S) 2 | %TRACKGEN draw samples from RFS of objects (birth model, dynamic equations) 3 | %to simulate object trajectories. Assume that a 2D measurement model is used. 4 | %INPUT: K: total tracking time --- scalar 5 | % sensormodel: a structure specifies the sensor parameters 6 | % P_D: object detection probability --- scalar 7 | % lambda_c: average number of clutter measurements per time scan, 8 | % Poisson distributed --- scalar 9 | % pdf_c: clutter (Poisson) intensity --- scalar 10 | % motionmodel: a structure specifies the motion model parameters 11 | % d: object state dimension --- scalar 12 | % F: function handle return transition/Jacobian matrix 13 | % f: function handle return predicted object state 14 | % Q: motion noise covariance matrix 15 | % measmodel: a structure specifies the measurement model parameters 16 | % d: measurement dimension --- scalar 17 | % H: function handle return transition/Jacobian matrix 18 | % h: function handle return the observation of the target state 19 | % R: measurement noise covariance matrix 20 | % birthmodel: a structure array specifies the birth model (Gaussian 21 | % mixture density) parameters --- (1 x number of birth components) 22 | % w: weights of mixture components 23 | % x: mean of mixture components 24 | % P: covariance of mixture components 25 | % object survival probability --- scalar 26 | %OUTPUT:object_tracks: a structure array specifies object tracks --- 27 | % (number of tracks x 1) 28 | % tbirth: track initiate (object appear) time --- scalar 29 | % tdeath: track end time (object disappear) time --- scalar 30 | % x: object trajectory --- (object state dimension x time steps 31 | % object exists in the scen) 32 | %Note that if the number of tracks is zero, set the output to empty 33 | 34 | n = 0; 35 | 36 | %Convert birthmodel.w 37 | [log_w,log_sum_w] = normalizeLogWeights([birthmodel.w]); 38 | 39 | %surveillance area 40 | range_c = sensormodel.range_c; 41 | 42 | for k = 1:K 43 | %randomly sample number of births 44 | nb = poissrnd(exp(log_sum_w)); 45 | for i = 1:nb 46 | %randomly sample birth component 47 | idx = find(rand=range_c(1,1))&&(xpos<=range_c(1,2))... 65 | &&(ypos>=range_c(2,1))&&(ypos<=range_c(2,2))&&(time+1<=K) 66 | 67 | %simulate the kinematic state 68 | xk = mvnrnd(motionmodel.f(xk), motionmodel.Q)'; 69 | object_tracks(n,1).x = [object_tracks(n,1).x xk]; 70 | object_tracks(n,1).tdeath = object_tracks(n,1).tdeath + 1; 71 | 72 | %simulate measurements 73 | time = time + 1; 74 | xypos = mvnrnd(measmodel.h(xk)', measmodel.R)'; 75 | xpos = xypos(1); 76 | ypos = xypos(2); 77 | 78 | if rand > P_S 79 | ifalive = 0; 80 | end 81 | end 82 | end 83 | end 84 | 85 | if n==0 86 | object_tracks = []; 87 | end 88 | 89 | end -------------------------------------------------------------------------------- /HA05/hypothesisReduction.m: -------------------------------------------------------------------------------- 1 | classdef hypothesisReduction 2 | %HYPOTHESISREDUCTION is class containing different hypotheses reduction 3 | %method 4 | %PRUNE: prune hypotheses with small weights. 5 | %CAP: keep M hypotheses with the highest weights and discard the rest. 6 | %MERGE: merge similar hypotheses in the sense of small Mahalanobis 7 | % distance. 8 | 9 | methods (Static) 10 | function [hypothesesWeight, multiHypotheses] = ... 11 | prune(hypothesesWeight, multiHypotheses, threshold) 12 | %PRUNE prunes hypotheses with small weights 13 | %INPUT: hypothesesWeight: the weights of different hypotheses 14 | % in logarithmic scale --- (number of hypotheses) x 1 15 | % vector 16 | % multiHypotheses: (number of hypotheses) x 1 structure 17 | % threshold: hypotheses with weights smaller than this 18 | % threshold will be discarded --- scalar 19 | %OUTPUT:hypothesesWeight: hypotheses weights after pruning in 20 | % logarithmic scale --- (number of hypotheses after 21 | % pruning) x 1 vector 22 | % multiHypotheses: (number of hypotheses after pruning) x 23 | % 1 structure 24 | indices_keeped = hypothesesWeight >= threshold; 25 | hypothesesWeight = hypothesesWeight(indices_keeped); 26 | multiHypotheses = multiHypotheses(indices_keeped); 27 | end 28 | 29 | function [hypothesesWeight, multiHypotheses] = ... 30 | cap(hypothesesWeight, multiHypotheses, M) 31 | %CAP keeps M hypotheses with the highest weights and discard 32 | %the rest 33 | %INPUT: hypothesesWeight: the weights of different hypotheses 34 | % in logarithmic scale --- (number of hypotheses) x 1 35 | % vector 36 | % multiHypotheses: (number of hypotheses) x 1 structure 37 | % M: only keep M hypotheses --- scalar 38 | %OUTPUT:hypothesesWeight: hypotheses weights after capping in 39 | % logarithmic scale --- (number of hypotheses after 40 | % capping) x 1 vector 41 | % multiHypotheses: (number of hypotheses after capping) x 42 | % 1 structure 43 | if length(hypothesesWeight) > M 44 | [hypothesesWeight, sorted_idx] = sort(hypothesesWeight,'descend'); 45 | hypothesesWeight = hypothesesWeight(1:M); 46 | multiHypotheses = multiHypotheses(sorted_idx(1:M)); 47 | end 48 | end 49 | 50 | function [hypothesesWeight,multiHypotheses] = ... 51 | merge(hypothesesWeight,multiHypotheses,threshold,density) 52 | %MERGE merges hypotheses within small Mahalanobis distance 53 | %INPUT: hypothesesWeight: the weights of different hypotheses 54 | % in logarithmic scale --- (number of hypotheses) x 1 55 | % vector 56 | % multiHypotheses: (number of hypotheses) x 1 structure 57 | % threshold: merging threshold --- scalar 58 | % density: a class handle 59 | %OUTPUT:hypothesesWeight: hypotheses weights after merging in 60 | % logarithmic scale --- (number of hypotheses after 61 | % merging) x 1 vector 62 | % multiHypotheses: (number of hypotheses after merging) x 63 | % 1 structure 64 | 65 | [hypothesesWeight,multiHypotheses] = ... 66 | density.mixtureReduction(hypothesesWeight,multiHypotheses,threshold); 67 | 68 | end 69 | 70 | 71 | end 72 | end -------------------------------------------------------------------------------- /HA01/MATLABGraderTestPackage/assessVariableEqual.m: -------------------------------------------------------------------------------- 1 | function assessVariableEqual(VariableName, ExpectedValue, varargin) 2 | % assessVariableEqual checks whether variable properties are equal to expected values 3 | % 4 | % assessVariableEqual(variableName,expectedValue) determines whether the 5 | % value of variableName is equal to expectedValue. 6 | % 7 | % assessVariableEqual(variableName,expectedValue,Name,Value) uses 8 | % additional options specified by one or more Name,Value pair arguments. 9 | % 10 | % - Feedback - Specify additional feedback to display to the learner. 11 | % 12 | % - RelativeTolerance - Specify the relative tolerance to apply to the submitted variable value. The default relative tolerance is 0.1%. 13 | % 14 | % - Absolute Tolerance - Specify the absolute tolerance to apply to the submitted variable value. The default absolute tolerance is 1e-4. 15 | % 16 | % If no tolerance is specified, the function applies both tolerances with their default values. If either tolerance passes, the variable is considered equal. 17 | % 18 | % Examples 19 | % -------- 20 | % 21 | % % Assess value of variable studentValue. The expected 22 | % % value is 2. The submitted value is 1. 23 | % assessVariableEqual('studentValue', '2') 24 | % 25 | % Variable studentValue has an incorrect value: 1 26 | % 27 | % 28 | % % Assess value of variable studentArray and provide 29 | % % additional feedback. The expected value is 'two'. The 30 | % % submitted value is 'four'. 31 | % assessVariableEqual('studentArray', 'two', 'Feedback', 'Refer to the week2 handout on Prime Numbers') 32 | % 33 | % Variable studentString has an incorrect value: four. 34 | % Refer to the week2 handout on Prime Numbers. 35 | % 36 | % 37 | % % Assess value of variable studentArray using a relative 38 | % % tolerance of 0.5%. The variable has an expected value of [1 2 3 4 6 4 3 4 5]. The submitted value is [1 2 3 4 6 4 3 4 1]. 39 | % assessVariableEqual('studentArray',[1 2 3 4 6 4 3 4 5],'RelativeTolerance',0.5) 40 | % 41 | % Variable studentValue has an incorrect value: 1 2 3 4 6 4 3 4 1 42 | % 43 | % 44 | % See also assessFunctionPresence, assessFunctionAbsence, matlab.unittest.constraints.RelativeTolerance, matlab.unittest.constraints.AbsoluteTolerance 45 | 46 | %% Default Values 47 | DefaultRelativeTolerance = 0.001; 48 | DefaultAbsoluteTolerance = 0.0001; 49 | DefaultUserFeedback = ''; 50 | 51 | %% Input Handling 52 | inputHandler = inputParser; 53 | inputHandler.PartialMatching = false; 54 | addRequired(inputHandler,'VariableName',@ischar); 55 | addRequired(inputHandler,'ExpectedValue'); 56 | addParameter(inputHandler,'RelativeTolerance', DefaultRelativeTolerance, @isnumeric); 57 | addParameter(inputHandler,'AbsoluteTolerance', DefaultAbsoluteTolerance, @isnumeric); 58 | addParameter(inputHandler,'Feedback', DefaultUserFeedback, @ischar); 59 | parse(inputHandler, VariableName, ExpectedValue, varargin{:}); 60 | 61 | %% Variable Existence Check 62 | cmd = sprintf('exist(''%s'', ''var'')', VariableName); 63 | variableExists = isequal(evalin('caller', cmd), 1); 64 | if ~variableExists 65 | variableNotPresentMsg = sprintf('%s%s%s\n%s', 'The submission must contain a variable named ', ... 66 | VariableName, ... 67 | '.', inputHandler.Results.Feedback); 68 | ME = MException('AssessmentToolbox:Feedback:VariableNotPresent', variableNotPresentMsg); 69 | throwAsCaller(ME); 70 | end 71 | 72 | %% Get Value from VariableName 73 | ActualValue = evalin('caller', VariableName); 74 | 75 | %% Calling IsValueEqualTo Constraint 76 | 77 | variableEqualConstraint = IsValueEqualTo(ExpectedValue, varargin{:}, 'VariableName', VariableName); 78 | 79 | if ~variableEqualConstraint.satisfiedBy(ActualValue) 80 | diagnostics = variableEqualConstraint.getDiagnosticFor(ActualValue); 81 | diagnostics.diagnose; 82 | ME = MException(diagnostics.DiagnosticIdentifier, diagnostics.DiagnosticResult); 83 | throwAsCaller(ME) 84 | end 85 | end -------------------------------------------------------------------------------- /HA02/MATLABGraderTestPackage/assessVariableEqual.m: -------------------------------------------------------------------------------- 1 | function assessVariableEqual(VariableName, ExpectedValue, varargin) 2 | % assessVariableEqual checks whether variable properties are equal to expected values 3 | % 4 | % assessVariableEqual(variableName,expectedValue) determines whether the 5 | % value of variableName is equal to expectedValue. 6 | % 7 | % assessVariableEqual(variableName,expectedValue,Name,Value) uses 8 | % additional options specified by one or more Name,Value pair arguments. 9 | % 10 | % - Feedback - Specify additional feedback to display to the learner. 11 | % 12 | % - RelativeTolerance - Specify the relative tolerance to apply to the submitted variable value. The default relative tolerance is 0.1%. 13 | % 14 | % - Absolute Tolerance - Specify the absolute tolerance to apply to the submitted variable value. The default absolute tolerance is 1e-4. 15 | % 16 | % If no tolerance is specified, the function applies both tolerances with their default values. If either tolerance passes, the variable is considered equal. 17 | % 18 | % Examples 19 | % -------- 20 | % 21 | % % Assess value of variable studentValue. The expected 22 | % % value is 2. The submitted value is 1. 23 | % assessVariableEqual('studentValue', '2') 24 | % 25 | % Variable studentValue has an incorrect value: 1 26 | % 27 | % 28 | % % Assess value of variable studentArray and provide 29 | % % additional feedback. The expected value is 'two'. The 30 | % % submitted value is 'four'. 31 | % assessVariableEqual('studentArray', 'two', 'Feedback', 'Refer to the week2 handout on Prime Numbers') 32 | % 33 | % Variable studentString has an incorrect value: four. 34 | % Refer to the week2 handout on Prime Numbers. 35 | % 36 | % 37 | % % Assess value of variable studentArray using a relative 38 | % % tolerance of 0.5%. The variable has an expected value of [1 2 3 4 6 4 3 4 5]. The submitted value is [1 2 3 4 6 4 3 4 1]. 39 | % assessVariableEqual('studentArray',[1 2 3 4 6 4 3 4 5],'RelativeTolerance',0.5) 40 | % 41 | % Variable studentValue has an incorrect value: 1 2 3 4 6 4 3 4 1 42 | % 43 | % 44 | % See also assessFunctionPresence, assessFunctionAbsence, matlab.unittest.constraints.RelativeTolerance, matlab.unittest.constraints.AbsoluteTolerance 45 | 46 | %% Default Values 47 | DefaultRelativeTolerance = 0.001; 48 | DefaultAbsoluteTolerance = 0.0001; 49 | DefaultUserFeedback = ''; 50 | 51 | %% Input Handling 52 | inputHandler = inputParser; 53 | inputHandler.PartialMatching = false; 54 | addRequired(inputHandler,'VariableName',@ischar); 55 | addRequired(inputHandler,'ExpectedValue'); 56 | addParameter(inputHandler,'RelativeTolerance', DefaultRelativeTolerance, @isnumeric); 57 | addParameter(inputHandler,'AbsoluteTolerance', DefaultAbsoluteTolerance, @isnumeric); 58 | addParameter(inputHandler,'Feedback', DefaultUserFeedback, @ischar); 59 | parse(inputHandler, VariableName, ExpectedValue, varargin{:}); 60 | 61 | %% Variable Existence Check 62 | cmd = sprintf('exist(''%s'', ''var'')', VariableName); 63 | variableExists = isequal(evalin('caller', cmd), 1); 64 | if ~variableExists 65 | variableNotPresentMsg = sprintf('%s%s%s\n%s', 'The submission must contain a variable named ', ... 66 | VariableName, ... 67 | '.', inputHandler.Results.Feedback); 68 | ME = MException('AssessmentToolbox:Feedback:VariableNotPresent', variableNotPresentMsg); 69 | throwAsCaller(ME); 70 | end 71 | 72 | %% Get Value from VariableName 73 | ActualValue = evalin('caller', VariableName); 74 | 75 | %% Calling IsValueEqualTo Constraint 76 | 77 | variableEqualConstraint = IsValueEqualTo(ExpectedValue, varargin{:}, 'VariableName', VariableName); 78 | 79 | if ~variableEqualConstraint.satisfiedBy(ActualValue) 80 | diagnostics = variableEqualConstraint.getDiagnosticFor(ActualValue); 81 | diagnostics.diagnose; 82 | ME = MException(diagnostics.DiagnosticIdentifier, diagnostics.DiagnosticResult); 83 | throwAsCaller(ME) 84 | end 85 | end -------------------------------------------------------------------------------- /HA03/MATLABGraderTestPackage/assessVariableEqual.m: -------------------------------------------------------------------------------- 1 | function assessVariableEqual(VariableName, ExpectedValue, varargin) 2 | % assessVariableEqual checks whether variable properties are equal to expected values 3 | % 4 | % assessVariableEqual(variableName,expectedValue) determines whether the 5 | % value of variableName is equal to expectedValue. 6 | % 7 | % assessVariableEqual(variableName,expectedValue,Name,Value) uses 8 | % additional options specified by one or more Name,Value pair arguments. 9 | % 10 | % - Feedback - Specify additional feedback to display to the learner. 11 | % 12 | % - RelativeTolerance - Specify the relative tolerance to apply to the submitted variable value. The default relative tolerance is 0.1%. 13 | % 14 | % - Absolute Tolerance - Specify the absolute tolerance to apply to the submitted variable value. The default absolute tolerance is 1e-4. 15 | % 16 | % If no tolerance is specified, the function applies both tolerances with their default values. If either tolerance passes, the variable is considered equal. 17 | % 18 | % Examples 19 | % -------- 20 | % 21 | % % Assess value of variable studentValue. The expected 22 | % % value is 2. The submitted value is 1. 23 | % assessVariableEqual('studentValue', '2') 24 | % 25 | % Variable studentValue has an incorrect value: 1 26 | % 27 | % 28 | % % Assess value of variable studentArray and provide 29 | % % additional feedback. The expected value is 'two'. The 30 | % % submitted value is 'four'. 31 | % assessVariableEqual('studentArray', 'two', 'Feedback', 'Refer to the week2 handout on Prime Numbers') 32 | % 33 | % Variable studentString has an incorrect value: four. 34 | % Refer to the week2 handout on Prime Numbers. 35 | % 36 | % 37 | % % Assess value of variable studentArray using a relative 38 | % % tolerance of 0.5%. The variable has an expected value of [1 2 3 4 6 4 3 4 5]. The submitted value is [1 2 3 4 6 4 3 4 1]. 39 | % assessVariableEqual('studentArray',[1 2 3 4 6 4 3 4 5],'RelativeTolerance',0.5) 40 | % 41 | % Variable studentValue has an incorrect value: 1 2 3 4 6 4 3 4 1 42 | % 43 | % 44 | % See also assessFunctionPresence, assessFunctionAbsence, matlab.unittest.constraints.RelativeTolerance, matlab.unittest.constraints.AbsoluteTolerance 45 | 46 | %% Default Values 47 | DefaultRelativeTolerance = 0.001; 48 | DefaultAbsoluteTolerance = 0.0001; 49 | DefaultUserFeedback = ''; 50 | 51 | %% Input Handling 52 | inputHandler = inputParser; 53 | inputHandler.PartialMatching = false; 54 | addRequired(inputHandler,'VariableName',@ischar); 55 | addRequired(inputHandler,'ExpectedValue'); 56 | addParameter(inputHandler,'RelativeTolerance', DefaultRelativeTolerance, @isnumeric); 57 | addParameter(inputHandler,'AbsoluteTolerance', DefaultAbsoluteTolerance, @isnumeric); 58 | addParameter(inputHandler,'Feedback', DefaultUserFeedback, @ischar); 59 | parse(inputHandler, VariableName, ExpectedValue, varargin{:}); 60 | 61 | %% Variable Existence Check 62 | cmd = sprintf('exist(''%s'', ''var'')', VariableName); 63 | variableExists = isequal(evalin('caller', cmd), 1); 64 | if ~variableExists 65 | variableNotPresentMsg = sprintf('%s%s%s\n%s', 'The submission must contain a variable named ', ... 66 | VariableName, ... 67 | '.', inputHandler.Results.Feedback); 68 | ME = MException('AssessmentToolbox:Feedback:VariableNotPresent', variableNotPresentMsg); 69 | throwAsCaller(ME); 70 | end 71 | 72 | %% Get Value from VariableName 73 | ActualValue = evalin('caller', VariableName); 74 | 75 | %% Calling IsValueEqualTo Constraint 76 | 77 | variableEqualConstraint = IsValueEqualTo(ExpectedValue, varargin{:}, 'VariableName', VariableName); 78 | 79 | if ~variableEqualConstraint.satisfiedBy(ActualValue) 80 | diagnostics = variableEqualConstraint.getDiagnosticFor(ActualValue); 81 | diagnostics.diagnose; 82 | ME = MException(diagnostics.DiagnosticIdentifier, diagnostics.DiagnosticResult); 83 | throwAsCaller(ME) 84 | end 85 | end -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/assessVariableEqual.m: -------------------------------------------------------------------------------- 1 | function assessVariableEqual(VariableName, ExpectedValue, varargin) 2 | % assessVariableEqual checks whether variable properties are equal to expected values 3 | % 4 | % assessVariableEqual(variableName,expectedValue) determines whether the 5 | % value of variableName is equal to expectedValue. 6 | % 7 | % assessVariableEqual(variableName,expectedValue,Name,Value) uses 8 | % additional options specified by one or more Name,Value pair arguments. 9 | % 10 | % - Feedback - Specify additional feedback to display to the learner. 11 | % 12 | % - RelativeTolerance - Specify the relative tolerance to apply to the submitted variable value. The default relative tolerance is 0.1%. 13 | % 14 | % - Absolute Tolerance - Specify the absolute tolerance to apply to the submitted variable value. The default absolute tolerance is 1e-4. 15 | % 16 | % If no tolerance is specified, the function applies both tolerances with their default values. If either tolerance passes, the variable is considered equal. 17 | % 18 | % Examples 19 | % -------- 20 | % 21 | % % Assess value of variable studentValue. The expected 22 | % % value is 2. The submitted value is 1. 23 | % assessVariableEqual('studentValue', '2') 24 | % 25 | % Variable studentValue has an incorrect value: 1 26 | % 27 | % 28 | % % Assess value of variable studentArray and provide 29 | % % additional feedback. The expected value is 'two'. The 30 | % % submitted value is 'four'. 31 | % assessVariableEqual('studentArray', 'two', 'Feedback', 'Refer to the week2 handout on Prime Numbers') 32 | % 33 | % Variable studentString has an incorrect value: four. 34 | % Refer to the week2 handout on Prime Numbers. 35 | % 36 | % 37 | % % Assess value of variable studentArray using a relative 38 | % % tolerance of 0.5%. The variable has an expected value of [1 2 3 4 6 4 3 4 5]. The submitted value is [1 2 3 4 6 4 3 4 1]. 39 | % assessVariableEqual('studentArray',[1 2 3 4 6 4 3 4 5],'RelativeTolerance',0.5) 40 | % 41 | % Variable studentValue has an incorrect value: 1 2 3 4 6 4 3 4 1 42 | % 43 | % 44 | % See also assessFunctionPresence, assessFunctionAbsence, matlab.unittest.constraints.RelativeTolerance, matlab.unittest.constraints.AbsoluteTolerance 45 | 46 | %% Default Values 47 | DefaultRelativeTolerance = 0.001; 48 | DefaultAbsoluteTolerance = 0.0001; 49 | DefaultUserFeedback = ''; 50 | 51 | %% Input Handling 52 | inputHandler = inputParser; 53 | inputHandler.PartialMatching = false; 54 | addRequired(inputHandler,'VariableName',@ischar); 55 | addRequired(inputHandler,'ExpectedValue'); 56 | addParameter(inputHandler,'RelativeTolerance', DefaultRelativeTolerance, @isnumeric); 57 | addParameter(inputHandler,'AbsoluteTolerance', DefaultAbsoluteTolerance, @isnumeric); 58 | addParameter(inputHandler,'Feedback', DefaultUserFeedback, @ischar); 59 | parse(inputHandler, VariableName, ExpectedValue, varargin{:}); 60 | 61 | %% Variable Existence Check 62 | cmd = sprintf('exist(''%s'', ''var'')', VariableName); 63 | variableExists = isequal(evalin('caller', cmd), 1); 64 | if ~variableExists 65 | variableNotPresentMsg = sprintf('%s%s%s\n%s', 'The submission must contain a variable named ', ... 66 | VariableName, ... 67 | '.', inputHandler.Results.Feedback); 68 | ME = MException('AssessmentToolbox:Feedback:VariableNotPresent', variableNotPresentMsg); 69 | throwAsCaller(ME); 70 | end 71 | 72 | %% Get Value from VariableName 73 | ActualValue = evalin('caller', VariableName); 74 | 75 | %% Calling IsValueEqualTo Constraint 76 | 77 | variableEqualConstraint = IsValueEqualTo(ExpectedValue, varargin{:}, 'VariableName', VariableName); 78 | 79 | if ~variableEqualConstraint.satisfiedBy(ActualValue) 80 | diagnostics = variableEqualConstraint.getDiagnosticFor(ActualValue); 81 | diagnostics.diagnose; 82 | ME = MException(diagnostics.DiagnosticIdentifier, diagnostics.DiagnosticResult); 83 | throwAsCaller(ME) 84 | end 85 | end -------------------------------------------------------------------------------- /HA03/auctionAlgortihm.m: -------------------------------------------------------------------------------- 1 | function [person_to_obj, obj_to_person, opt_cost] ... 2 | = auctionAlgortihm(cost_mat, max_iter) 3 | % AUTHOR: Abu Sajana Rahmathullah 4 | % DATE OF CREATION: 7 August, 2017 5 | % 6 | % [person_to_obj, obj_to_person, opt_cost] ... 7 | % = auctionAlgortihm(cost_mat, epsil, max_iter) 8 | % returns the results of auction algorithm in assigning 'm' unique persons 9 | % to the 'n' objects, such that the overall cost is maximised. 10 | % 11 | % REFERENCE: Section 6.5.1 in 'Design and Analysis of Modern Tracking 12 | % Systems' by Blackman and Popoli, 1999 edition 13 | % 14 | % INPUT: 15 | % cost_mat: matrix of size mxn consising of cost of assigning one of m 16 | % persons to one of each n objects 17 | % max_iter: maximum number of iterations to terminate the alg 18 | % 19 | % OUTPUT: 20 | % person_to_obj: A vector of size 1xm which has indices of the assigned 21 | % objects or '0' if unassigned 22 | % obj_to_person: A vector of size 1xn, which has indices of the 23 | % assigned persons and a vector 24 | % opt_cost : A scalar, value of the (optimal) assignment made 25 | % 26 | 27 | m_person = size(cost_mat, 1); n_obj = size(cost_mat,2); 28 | 29 | % index for objects that will be left unassigned 30 | un_ass_ind = 0; 31 | 32 | % corner cases when there is only one person and/or one object 33 | if m_person == 1 34 | [opt_cost, person_to_obj] = max(cost_mat); 35 | obj_to_person = un_ass_ind * ones(n_obj, 1); 36 | obj_to_person(person_to_obj) = 1; 37 | return; 38 | end 39 | if n_obj == 1 40 | [opt_cost, obj_to_person] = max(cost_mat); 41 | person_to_obj = un_ass_ind * ones(m_person, 1); 42 | person_to_obj(obj_to_person) = 1; 43 | return; 44 | end 45 | 46 | swap_dim_flag = false; 47 | epsil = 1/ max(n_obj, m_person); 48 | 49 | if n_obj < m_person % the below implementation works for m_person<=n_obj. 50 | % If not satisifed, swap the cost matrix 51 | cost_mat = cost_mat'; 52 | m_person = size(cost_mat, 1); n_obj = size(cost_mat,2); 53 | swap_dim_flag = true; 54 | end 55 | 56 | 57 | % obj ind of assignment for person 1 to m 58 | person_to_obj = un_ass_ind * ones(m_person, 1); 59 | 60 | % person ind of assignemnt for obj 1 to n 61 | obj_to_person = un_ass_ind * ones(n_obj, 1); 62 | opt_cost = 0; 63 | p_obj = zeros(1, n_obj); % price for each object 64 | 65 | iter = 0; 66 | while(~all(person_to_obj ~= un_ass_ind)) 67 | if iter > max_iter 68 | warning(['Maximum number of iterations reached! Retry with ' ... 69 | 'more than ' num2str(max_iter) ' number of iterations.']); 70 | break; 71 | end 72 | for i = 1:m_person 73 | if(person_to_obj(i) == un_ass_ind) 74 | % pick the unassigned person i to bid for its best obj jStar 75 | 76 | % value for each object for the person i 77 | [val_i_j, j] = sort(cost_mat(i, :) - p_obj, 2, 'descend'); 78 | 79 | % j_star is the best object for person i 80 | j_star = j(1); 81 | 82 | % 1st and 2nd best value for person i 83 | v_ijStar = val_i_j(1); 84 | w_ijStar = val_i_j(2); 85 | 86 | 87 | % bid for obj jStar 88 | if w_ijStar ~= -Inf % if there is no 2nd best 89 | p_obj(j_star) = p_obj(j_star) + v_ijStar - w_ijStar + epsil; 90 | else 91 | p_obj(j_star) = p_obj(j_star) + v_ijStar + epsil; 92 | end 93 | 94 | 95 | if obj_to_person(j_star) ~= un_ass_ind % if j_star is uassigned 96 | % if j_star is assigned to obj_to_person(j_star) before, 97 | % remove the cost of old assignment 98 | % and unassign the person obj_to_person(j_star) 99 | opt_cost = opt_cost ... 100 | - cost_mat(obj_to_person(j_star), j_star); 101 | person_to_obj(obj_to_person(j_star)) = un_ass_ind; 102 | end 103 | obj_to_person(j_star) = i; % assign i to j_star 104 | person_to_obj(i) = j_star; % assign j_star to i 105 | 106 | % update the cost of new assignemnt 107 | opt_cost = opt_cost + cost_mat(i, j_star); 108 | end 109 | end 110 | iter = iter+1; 111 | end 112 | 113 | % swap the dimensions to compensate for the swapping of the cost matrix 114 | % done in the beginning 115 | if swap_dim_flag == true 116 | tmp = obj_to_person; 117 | obj_to_person = person_to_obj; 118 | person_to_obj = tmp; 119 | end 120 | -------------------------------------------------------------------------------- /HA05/auctionAlgortihm.m: -------------------------------------------------------------------------------- 1 | function [person_to_obj, obj_to_person, opt_cost] ... 2 | = auctionAlgortihm(cost_mat, max_iter) 3 | % AUTHOR: Abu Sajana Rahmathullah 4 | % DATE OF CREATION: 7 August, 2017 5 | % 6 | % [person_to_obj, obj_to_person, opt_cost] ... 7 | % = auctionAlgortihm(cost_mat, epsil, max_iter) 8 | % returns the results of auction algorithm in assigning 'm' unique persons 9 | % to the 'n' objects, such that the overall cost is maximised. 10 | % 11 | % REFERENCE: Section 6.5.1 in 'Design and Analysis of Modern Tracking 12 | % Systems' by Blackman and Popoli, 1999 edition 13 | % 14 | % INPUT: 15 | % cost_mat: matrix of size mxn consising of cost of assigning one of m 16 | % persons to one of each n objects 17 | % max_iter: maximum number of iterations to terminate the alg 18 | % 19 | % OUTPUT: 20 | % person_to_obj: A vector of size 1xm which has indices of the assigned 21 | % objects or '0' if unassigned 22 | % obj_to_person: A vector of size 1xn, which has indices of the 23 | % assigned persons and a vector 24 | % opt_cost : A scalar, value of the (optimal) assignment made 25 | % 26 | 27 | m_person = size(cost_mat, 1); n_obj = size(cost_mat,2); 28 | 29 | % index for objects that will be left unassigned 30 | un_ass_ind = 0; 31 | 32 | % corner cases when there is only one person and/or one object 33 | if m_person == 1 34 | [opt_cost, person_to_obj] = max(cost_mat); 35 | obj_to_person = un_ass_ind * ones(n_obj, 1); 36 | obj_to_person(person_to_obj) = 1; 37 | return; 38 | end 39 | if n_obj == 1 40 | [opt_cost, obj_to_person] = max(cost_mat); 41 | person_to_obj = un_ass_ind * ones(m_person, 1); 42 | person_to_obj(obj_to_person) = 1; 43 | return; 44 | end 45 | 46 | swap_dim_flag = false; 47 | epsil = 1/ max(n_obj, m_person); 48 | 49 | if n_obj < m_person % the below implementation works for m_person<=n_obj. 50 | % If not satisifed, swap the cost matrix 51 | cost_mat = cost_mat'; 52 | m_person = size(cost_mat, 1); n_obj = size(cost_mat,2); 53 | swap_dim_flag = true; 54 | end 55 | 56 | 57 | % obj ind of assignment for person 1 to m 58 | person_to_obj = un_ass_ind * ones(m_person, 1); 59 | 60 | % person ind of assignemnt for obj 1 to n 61 | obj_to_person = un_ass_ind * ones(n_obj, 1); 62 | opt_cost = 0; 63 | p_obj = zeros(1, n_obj); % price for each object 64 | 65 | iter = 0; 66 | while(~all(person_to_obj ~= un_ass_ind)) 67 | if iter > max_iter 68 | warning(['Maximum number of iterations reached! Retry with ' ... 69 | 'more than ' num2str(max_iter) ' number of iterations.']); 70 | break; 71 | end 72 | for i = 1:m_person 73 | if(person_to_obj(i) == un_ass_ind) 74 | % pick the unassigned person i to bid for its best obj jStar 75 | 76 | % value for each object for the person i 77 | [val_i_j, j] = sort(cost_mat(i, :) - p_obj, 2, 'descend'); 78 | 79 | % j_star is the best object for person i 80 | j_star = j(1); 81 | 82 | % 1st and 2nd best value for person i 83 | v_ijStar = val_i_j(1); 84 | w_ijStar = val_i_j(2); 85 | 86 | 87 | % bid for obj jStar 88 | if w_ijStar ~= -Inf % if there is no 2nd best 89 | p_obj(j_star) = p_obj(j_star) + v_ijStar - w_ijStar + epsil; 90 | else 91 | p_obj(j_star) = p_obj(j_star) + v_ijStar + epsil; 92 | end 93 | 94 | 95 | if obj_to_person(j_star) ~= un_ass_ind % if j_star is uassigned 96 | % if j_star is assigned to obj_to_person(j_star) before, 97 | % remove the cost of old assignment 98 | % and unassign the person obj_to_person(j_star) 99 | opt_cost = opt_cost ... 100 | - cost_mat(obj_to_person(j_star), j_star); 101 | person_to_obj(obj_to_person(j_star)) = un_ass_ind; 102 | end 103 | obj_to_person(j_star) = i; % assign i to j_star 104 | person_to_obj(i) = j_star; % assign j_star to i 105 | 106 | % update the cost of new assignemnt 107 | opt_cost = opt_cost + cost_mat(i, j_star); 108 | end 109 | end 110 | iter = iter+1; 111 | end 112 | 113 | % swap the dimensions to compensate for the swapping of the cost matrix 114 | % done in the beginning 115 | if swap_dim_flag == true 116 | tmp = obj_to_person; 117 | obj_to_person = person_to_obj; 118 | person_to_obj = tmp; 119 | end 120 | -------------------------------------------------------------------------------- /HA02/KeyVal.m: -------------------------------------------------------------------------------- 1 | classdef KeyVal 2 | %KEYVAL A class that encapsulates a key and a value, which could be 3 | % an instance of a handle class. It is assumed that standard 4 | % comparison operations can be performed on the keys. This class 5 | % is useful for inserting elements into a BinaryHeap or other 6 | % data structure that requires a key and a value. 7 | % 8 | %DEPENDENCIES: NONE 9 | % 10 | %Instances of the KeyVal class can be compared to each other by key or to 11 | %other objects that have the same type of key. For example, 12 | %keyVal1obj2.key; 52 | elseif(isa(obj1,'KeyVal')==true) 53 | val=obj1.key>obj2; 54 | else 55 | val=obj1>obj2.key; 56 | end 57 | end 58 | 59 | function val=le(obj1,obj2) 60 | %%LE Less than or equal to comparison 61 | 62 | if(isa(obj1,'KeyVal')==true&&isa(obj2,'KeyVal')==true) 63 | val=obj1.key<=obj2.key; 64 | elseif(isa(obj1,'KeyVal')==true) 65 | val=obj1.key<=obj2; 66 | else 67 | val=obj1<=obj2.key; 68 | end 69 | end 70 | 71 | function val=ge(obj1,obj2) 72 | %%GE Greater than or equal to comparison 73 | 74 | if(isa(obj1,'KeyVal')==true&&isa(obj2,'KeyVal')==true) 75 | val=obj1.key>=obj2.key; 76 | elseif(isa(obj1,'KeyVal')==true) 77 | val=obj1.key>=obj2; 78 | else 79 | val=obj1>=obj2.key; 80 | end 81 | end 82 | 83 | function val=ne(obj1,obj2) 84 | %%NE Not equal to comparison 85 | 86 | if(isa(obj1,'KeyVal')==true&&isa(obj2,'KeyVal')==true) 87 | val=obj1.key~=obj2.key; 88 | elseif(isa(obj1,'KeyVal')==true) 89 | val=obj1.key~=obj2; 90 | else 91 | val=obj1~=obj2.key; 92 | end 93 | end 94 | 95 | function val=eq(obj1,obj2) 96 | %%EQ Equal to comparison 97 | 98 | if(isa(obj1,'KeyVal')==true&&isa(obj2,'KeyVal')==true) 99 | val=obj1.key==obj2.key; 100 | elseif(isa(obj1,'KeyVal')==true) 101 | val=obj1.key==obj2; 102 | else 103 | val=obj1==obj2.key; 104 | end 105 | end 106 | end 107 | end 108 | 109 | %LICENSE: 110 | % 111 | %The source code is in the public domain and not licensed or under 112 | %copyright. The information and software may be used freely by the public. 113 | %As required by 17 U.S.C. 403, third parties producing copyrighted works 114 | %consisting predominantly of the material produced by U.S. government 115 | %agencies must provide notice with such work(s) identifying the U.S. 116 | %Government material incorporated and stating that such material is not 117 | %subject to copyright protection. 118 | % 119 | %Derived works shall not identify themselves in a manner that implies an 120 | %endorsement by or an affiliation with the Naval Research Laboratory. 121 | % 122 | %RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE 123 | %SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL 124 | %RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS 125 | %OF RECIPIENT IN THE USE OF THE SOFTWARE. 126 | -------------------------------------------------------------------------------- /HA05/KeyVal.m: -------------------------------------------------------------------------------- 1 | classdef KeyVal 2 | %KEYVAL A class that encapsulates a key and a value, which could be 3 | % an instance of a handle class. It is assumed that standard 4 | % comparison operations can be performed on the keys. This class 5 | % is useful for inserting elements into a BinaryHeap or other 6 | % data structure that requires a key and a value. 7 | % 8 | %DEPENDENCIES: NONE 9 | % 10 | %Instances of the KeyVal class can be compared to each other by key or to 11 | %other objects that have the same type of key. For example, 12 | %keyVal1obj2.key; 52 | elseif(isa(obj1,'KeyVal')==true) 53 | val=obj1.key>obj2; 54 | else 55 | val=obj1>obj2.key; 56 | end 57 | end 58 | 59 | function val=le(obj1,obj2) 60 | %%LE Less than or equal to comparison 61 | 62 | if(isa(obj1,'KeyVal')==true&&isa(obj2,'KeyVal')==true) 63 | val=obj1.key<=obj2.key; 64 | elseif(isa(obj1,'KeyVal')==true) 65 | val=obj1.key<=obj2; 66 | else 67 | val=obj1<=obj2.key; 68 | end 69 | end 70 | 71 | function val=ge(obj1,obj2) 72 | %%GE Greater than or equal to comparison 73 | 74 | if(isa(obj1,'KeyVal')==true&&isa(obj2,'KeyVal')==true) 75 | val=obj1.key>=obj2.key; 76 | elseif(isa(obj1,'KeyVal')==true) 77 | val=obj1.key>=obj2; 78 | else 79 | val=obj1>=obj2.key; 80 | end 81 | end 82 | 83 | function val=ne(obj1,obj2) 84 | %%NE Not equal to comparison 85 | 86 | if(isa(obj1,'KeyVal')==true&&isa(obj2,'KeyVal')==true) 87 | val=obj1.key~=obj2.key; 88 | elseif(isa(obj1,'KeyVal')==true) 89 | val=obj1.key~=obj2; 90 | else 91 | val=obj1~=obj2.key; 92 | end 93 | end 94 | 95 | function val=eq(obj1,obj2) 96 | %%EQ Equal to comparison 97 | 98 | if(isa(obj1,'KeyVal')==true&&isa(obj2,'KeyVal')==true) 99 | val=obj1.key==obj2.key; 100 | elseif(isa(obj1,'KeyVal')==true) 101 | val=obj1.key==obj2; 102 | else 103 | val=obj1==obj2.key; 104 | end 105 | end 106 | end 107 | end 108 | 109 | %LICENSE: 110 | % 111 | %The source code is in the public domain and not licensed or under 112 | %copyright. The information and software may be used freely by the public. 113 | %As required by 17 U.S.C. 403, third parties producing copyrighted works 114 | %consisting predominantly of the material produced by U.S. government 115 | %agencies must provide notice with such work(s) identifying the U.S. 116 | %Government material incorporated and stating that such material is not 117 | %subject to copyright protection. 118 | % 119 | %Derived works shall not identify themselves in a manner that implies an 120 | %endorsement by or an affiliation with the Naval Research Laboratory. 121 | % 122 | %RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE 123 | %SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL 124 | %RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS 125 | %OF RECIPIENT IN THE USE OF THE SOFTWARE. 126 | -------------------------------------------------------------------------------- /HA01/hypothesisReduction.m: -------------------------------------------------------------------------------- 1 | classdef hypothesisReduction 2 | %HYPOTHESISREDUCTION is class containing different hypotheses reduction 3 | %method 4 | %PRUNE: prune hypotheses with small weights. 5 | %CAP: keep M hypotheses with the highest weights and discard the rest. 6 | %MERGE: merge similar hypotheses in the sense of small Mahalanobis 7 | % distance. 8 | % 9 | % Note: Usually, one need to re-normalise the weights after pruning/capping; 10 | % however, this is not done inside the functions you are going to implement. 11 | 12 | methods (Static) 13 | function [hypothesesWeight, multiHypotheses] = ... 14 | prune(hypothesesWeight, multiHypotheses, threshold) 15 | %PRUNE prunes hypotheses with small weights 16 | %INPUT: hypothesesWeight: the weights of different hypotheses 17 | % in logarithmic scale --- (number of hypotheses) x 1 18 | % vector 19 | % multiHypotheses: (number of hypotheses) x 1 structure 20 | % threshold: hypotheses with weights smaller than this 21 | % threshold will be discarded --- scalar in logarithmic scale 22 | %OUTPUT:hypothesesWeight: hypotheses weights after pruning in 23 | % logarithmic scale --- (number of hypotheses after 24 | % pruning) x 1 vector 25 | % multiHypotheses: (number of hypotheses after pruning) x 26 | % 1 structure 27 | idx_keep = hypothesesWeight > threshold; 28 | % select 29 | hypothesesWeight = hypothesesWeight(idx_keep); 30 | % select indices 31 | multiHypotheses = multiHypotheses(idx_keep); 32 | end 33 | 34 | function [hypothesesWeight, multiHypotheses] = ... 35 | cap(hypothesesWeight, multiHypotheses, M) 36 | %CAP keeps M hypotheses with the highest weights and discard 37 | %the rest 38 | %INPUT: hypothesesWeight: the weights of different hypotheses 39 | % in logarithmic scale --- (number of hypotheses) x 1 40 | % vector 41 | % multiHypotheses: (number of hypotheses) x 1 structure 42 | % M: only keep M hypotheses --- scalar 43 | %OUTPUT:hypothesesWeight: hypotheses weights after capping in 44 | % logarithmic scale --- (number of hypotheses after 45 | % capping) x 1 vector 46 | % multiHypotheses: (number of hypotheses after capping) x 47 | % 1 structure 48 | 49 | % sort by descending hypothesis weight 50 | [w_sorted,idx] = sort(hypothesesWeight,'descend'); 51 | % keep best M hypothesis 52 | M = min(M,length(hypothesesWeight)); 53 | idx_keep = idx(1:M); 54 | % select indices 55 | hypothesesWeight = hypothesesWeight(idx_keep); 56 | multiHypotheses = multiHypotheses(idx_keep); 57 | end 58 | 59 | function [hypothesesWeight,multiHypotheses] = ... 60 | merge(hypothesesWeight,multiHypotheses,threshold,density) 61 | %MERGE merges hypotheses within small Mahalanobis distance 62 | %INPUT: hypothesesWeight: the weights of different hypotheses 63 | % in logarithmic scale --- (number of hypotheses) x 1 64 | % vector 65 | % multiHypotheses: (number of hypotheses) x 1 structure 66 | % threshold: merging threshold --- scalar 67 | % density: a class handle 68 | %OUTPUT:hypothesesWeight: hypotheses weights after merging in 69 | % logarithmic scale --- (number of hypotheses after 70 | % merging) x 1 vector 71 | % multiHypotheses: (number of hypotheses after merging) x 72 | % 1 structure 73 | 74 | [hypothesesWeight,multiHypotheses] = ... 75 | density.mixtureReduction(hypothesesWeight,multiHypotheses,threshold); 76 | 77 | end 78 | 79 | end 80 | end 81 | 82 | 83 | 84 | %% test 85 | 86 | % %Note that multiHypotheses does not really need to be a struct array to make the function work properly 87 | % multiHypotheses = 1:100; 88 | % %Generate some random weights 89 | % hypothesesWeight = rand(100,1); 90 | % hypothesesWeight = log(hypothesesWeight/sum(hypothesesWeight)); 91 | % %Pruning 92 | % [hypothesesWeight_hat, multiHypotheses_hat] = hypothesisReduction.prune(hypothesesWeight, multiHypotheses, log(1e-2)); 93 | % %Capping 94 | % [hypothesesWeight_hat, multiHypotheses_hat] = hypothesisReduction.cap(hypothesesWeight, multiHypotheses, 50); 95 | -------------------------------------------------------------------------------- /HA02/hypothesisReduction.m: -------------------------------------------------------------------------------- 1 | classdef hypothesisReduction 2 | %HYPOTHESISREDUCTION is class containing different hypotheses reduction 3 | %method 4 | %PRUNE: prune hypotheses with small weights. 5 | %CAP: keep M hypotheses with the highest weights and discard the rest. 6 | %MERGE: merge similar hypotheses in the sense of small Mahalanobis 7 | % distance. 8 | % 9 | % Note: Usually, one need to re-normalise the weights after pruning/capping; 10 | % however, this is not done inside the functions you are going to implement. 11 | 12 | methods (Static) 13 | function [hypothesesWeight, multiHypotheses] = ... 14 | prune(hypothesesWeight, multiHypotheses, threshold) 15 | %PRUNE prunes hypotheses with small weights 16 | %INPUT: hypothesesWeight: the weights of different hypotheses 17 | % in logarithmic scale --- (number of hypotheses) x 1 18 | % vector 19 | % multiHypotheses: (number of hypotheses) x 1 structure 20 | % threshold: hypotheses with weights smaller than this 21 | % threshold will be discarded --- scalar in logarithmic scale 22 | %OUTPUT:hypothesesWeight: hypotheses weights after pruning in 23 | % logarithmic scale --- (number of hypotheses after 24 | % pruning) x 1 vector 25 | % multiHypotheses: (number of hypotheses after pruning) x 26 | % 1 structure 27 | idx_keep = hypothesesWeight > threshold; 28 | % select 29 | hypothesesWeight = hypothesesWeight(idx_keep); 30 | % select indices 31 | multiHypotheses = multiHypotheses(idx_keep); 32 | end 33 | 34 | function [hypothesesWeight, multiHypotheses] = ... 35 | cap(hypothesesWeight, multiHypotheses, M) 36 | %CAP keeps M hypotheses with the highest weights and discard 37 | %the rest 38 | %INPUT: hypothesesWeight: the weights of different hypotheses 39 | % in logarithmic scale --- (number of hypotheses) x 1 40 | % vector 41 | % multiHypotheses: (number of hypotheses) x 1 structure 42 | % M: only keep M hypotheses --- scalar 43 | %OUTPUT:hypothesesWeight: hypotheses weights after capping in 44 | % logarithmic scale --- (number of hypotheses after 45 | % capping) x 1 vector 46 | % multiHypotheses: (number of hypotheses after capping) x 47 | % 1 structure 48 | 49 | % sort by descending hypothesis weight 50 | [w_sorted,idx] = sort(hypothesesWeight,'descend'); 51 | % keep best M hypothesis 52 | M = min(M,length(hypothesesWeight)); 53 | idx_keep = idx(1:M); 54 | % select indices 55 | hypothesesWeight = hypothesesWeight(idx_keep); 56 | multiHypotheses = multiHypotheses(idx_keep); 57 | end 58 | 59 | function [hypothesesWeight,multiHypotheses] = ... 60 | merge(hypothesesWeight,multiHypotheses,threshold,density) 61 | %MERGE merges hypotheses within small Mahalanobis distance 62 | %INPUT: hypothesesWeight: the weights of different hypotheses 63 | % in logarithmic scale --- (number of hypotheses) x 1 64 | % vector 65 | % multiHypotheses: (number of hypotheses) x 1 structure 66 | % threshold: merging threshold --- scalar 67 | % density: a class handle 68 | %OUTPUT:hypothesesWeight: hypotheses weights after merging in 69 | % logarithmic scale --- (number of hypotheses after 70 | % merging) x 1 vector 71 | % multiHypotheses: (number of hypotheses after merging) x 72 | % 1 structure 73 | 74 | [hypothesesWeight,multiHypotheses] = ... 75 | density.mixtureReduction(hypothesesWeight,multiHypotheses,threshold); 76 | 77 | end 78 | 79 | end 80 | end 81 | 82 | 83 | 84 | %% test 85 | 86 | % %Note that multiHypotheses does not really need to be a struct array to make the function work properly 87 | % multiHypotheses = 1:100; 88 | % %Generate some random weights 89 | % hypothesesWeight = rand(100,1); 90 | % hypothesesWeight = log(hypothesesWeight/sum(hypothesesWeight)); 91 | % %Pruning 92 | % [hypothesesWeight_hat, multiHypotheses_hat] = hypothesisReduction.prune(hypothesesWeight, multiHypotheses, log(1e-2)); 93 | % %Capping 94 | % [hypothesesWeight_hat, multiHypotheses_hat] = hypothesisReduction.cap(hypothesesWeight, multiHypotheses, 50); 95 | -------------------------------------------------------------------------------- /HA01/Animate_2D_tracking.m: -------------------------------------------------------------------------------- 1 | classdef Animate_2D_tracking 2 | %ANIMATE_2D_TRACKING Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | 7 | end 8 | 9 | methods(Static) 10 | function [ xy ] = sigmaEllipse2D( mu, Sigma, level, npoints ) 11 | %SIGMAELLIPSE2D generates x,y-points which lie on the ellipse describing 12 | % a sigma level in the Gaussian density defined by mean and covariance. 13 | % 14 | %Input: 15 | % MU [2 x 1] Mean of the Gaussian density 16 | % SIGMA [2 x 2] Covariance matrix of the Gaussian density 17 | % LEVEL Which sigma level curve to plot. Can take any positive value, 18 | % but common choices are 1, 2 or 3. Default = 3. 19 | % NPOINTS Number of points on the ellipse to generate. Default = 32. 20 | % 21 | %Output: 22 | % XY [2 x npoints] matrix. First row holds x-coordinates, second 23 | % row holds the y-coordinates. First and last columns should 24 | % be the same point, to create a closed curve. 25 | 26 | %Setting default values, in case only mu and Sigma are specified. 27 | if nargin < 3 28 | level = 3; 29 | end 30 | if nargin < 4 31 | npoints = 32; 32 | end 33 | 34 | % Procedure: 35 | % - A 3 sigma level curve is given by {x} such that (x-mux)'*Q^-1*(x-mux) = 3^2 36 | % or in scalar form: (x-mux) = sqrt(Q)*3 37 | % - replacing z= sqrtm(Q^-1)*(x-mux), such that we have now z'*z = 3^2 38 | % which is now a circle with radius equal 3. 39 | % - Sampling in z, we have z = 3*[cos(theta); sin(theta)]', for theta=1:2*pi 40 | % - Back to x we get: x = mux + 3* sqrtm(Q)*[cos(theta); sin(theta)]' 41 | 42 | xy = []; 43 | for ang = linspace(0,2*pi,npoints) 44 | xy(:,end+1) = mu + level * sqrtm(Sigma) * [cos(ang) sin(ang)]'; 45 | end 46 | end 47 | 48 | function [zk,S] = estimated_meas(x,P,measmodel) 49 | % measurement covariance 50 | Hx = measmodel.H(x); 51 | % Innovation covariance 52 | S = Hx * P * Hx' + measmodel.R; 53 | % ensure it is positive definite 54 | S = (S+S')/2; 55 | % object measurement 56 | zk = measmodel.h(x); 57 | end 58 | end 59 | 60 | methods 61 | function obj = animate_2D_tracking(obj) 62 | end 63 | 64 | function animate(obj, est, initial_state, measdata, measmodel, range_c) 65 | fig = figure; 66 | hold on; 67 | xlim(range_c(1,:)); 68 | ylim(range_c(2,:)); 69 | 70 | % plot meas data 71 | meas_x = measdata{1}(1,:); 72 | meas_y = measdata{1}(2,:); 73 | pl_meas = scatter(meas_x, meas_y, 50, 'o', 'filled'); 74 | pl_meas.XDataSource = 'meas_x'; 75 | pl_meas.YDataSource = 'meas_y'; 76 | 77 | [zk,S] = obj.estimated_meas(initial_state.x,initial_state.P,measmodel); 78 | 79 | % plot estimated covariance 80 | ellipse = obj.sigmaEllipse2D(zk, S); 81 | S_x = ellipse(1,:); 82 | S_y = ellipse(2,:); 83 | pl_cov = plot(S_x,S_y); 84 | pl_cov.XDataSource = 'S_x'; 85 | pl_cov.YDataSource = 'S_y'; 86 | 87 | % plot estimated mean 88 | zk_x = zk(1); 89 | zk_y = zk(2); 90 | pl_mean = scatter(zk_x, zk_y,100, 'o', 'filled'); 91 | pl_mean.XDataSource = 'zk_x'; 92 | pl_mean.YDataSource = 'zk_y'; 93 | 94 | for i=1:numel(measdata) 95 | [zk,S] = obj.estimated_meas(est(i).x,est(i).P,measmodel); 96 | ellipse = obj.sigmaEllipse2D(zk, S); 97 | % variance 98 | S_x = ellipse(1,:); 99 | S_y = ellipse(2,:); 100 | % mean 101 | zk_x = zk(1); 102 | zk_y = zk(2); 103 | % meas 104 | meas_x = measdata{i}(1,:); 105 | meas_y = measdata{i}(2,:); 106 | 107 | refreshdata(fig,'caller'); 108 | drawnow; 109 | % pause(0.05); 110 | end 111 | end 112 | 113 | end 114 | end 115 | 116 | 117 | 118 | 119 | 120 | -------------------------------------------------------------------------------- /HA02/Animate_2D_tracking.m: -------------------------------------------------------------------------------- 1 | classdef Animate_2D_tracking 2 | %ANIMATE_2D_TRACKING Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | 7 | end 8 | 9 | methods(Static) 10 | function [ xy ] = sigmaEllipse2D( mu, Sigma, level, npoints ) 11 | %SIGMAELLIPSE2D generates x,y-points which lie on the ellipse describing 12 | % a sigma level in the Gaussian density defined by mean and covariance. 13 | % 14 | %Input: 15 | % MU [2 x 1] Mean of the Gaussian density 16 | % SIGMA [2 x 2] Covariance matrix of the Gaussian density 17 | % LEVEL Which sigma level curve to plot. Can take any positive value, 18 | % but common choices are 1, 2 or 3. Default = 3. 19 | % NPOINTS Number of points on the ellipse to generate. Default = 32. 20 | % 21 | %Output: 22 | % XY [2 x npoints] matrix. First row holds x-coordinates, second 23 | % row holds the y-coordinates. First and last columns should 24 | % be the same point, to create a closed curve. 25 | 26 | %Setting default values, in case only mu and Sigma are specified. 27 | if nargin < 3 28 | level = 3; 29 | end 30 | if nargin < 4 31 | npoints = 32; 32 | end 33 | 34 | % Procedure: 35 | % - A 3 sigma level curve is given by {x} such that (x-mux)'*Q^-1*(x-mux) = 3^2 36 | % or in scalar form: (x-mux) = sqrt(Q)*3 37 | % - replacing z= sqrtm(Q^-1)*(x-mux), such that we have now z'*z = 3^2 38 | % which is now a circle with radius equal 3. 39 | % - Sampling in z, we have z = 3*[cos(theta); sin(theta)]', for theta=1:2*pi 40 | % - Back to x we get: x = mux + 3* sqrtm(Q)*[cos(theta); sin(theta)]' 41 | 42 | xy = []; 43 | for ang = linspace(0,2*pi,npoints) 44 | xy(:,end+1) = mu + level * sqrtm(Sigma) * [cos(ang) sin(ang)]'; 45 | end 46 | end 47 | 48 | function [zk,S] = estimated_meas(x,P,measmodel) 49 | % measurement covariance 50 | Hx = measmodel.H(x); 51 | % Innovation covariance 52 | S = Hx * P * Hx' + measmodel.R; 53 | % ensure it is positive definite 54 | S = (S+S')/2; 55 | % object measurement 56 | zk = measmodel.h(x); 57 | end 58 | end 59 | 60 | methods 61 | function obj = animate_2D_tracking(obj) 62 | end 63 | 64 | function animate(obj, est, initial_state, measdata, measmodel, range_c) 65 | fig = figure; 66 | hold on; 67 | xlim(range_c(1,:)); 68 | ylim(range_c(2,:)); 69 | 70 | n = numel(initial_state); 71 | 72 | % plot meas data 73 | meas_x = measdata{1}(1,:); 74 | meas_y = measdata{1}(2,:); 75 | pl_meas = scatter(meas_x, meas_y, 50, 'o', 'filled'); 76 | pl_meas.XDataSource = 'meas_x'; 77 | pl_meas.YDataSource = 'meas_y'; 78 | 79 | % initialize objects 80 | for i=1:n 81 | [zk,S] = obj.estimated_meas(initial_state(i).x,initial_state(i).P,measmodel); 82 | 83 | % plot estimated covariance 84 | ellipse{i} = obj.sigmaEllipse2D(zk, S); 85 | S_x{i} = ellipse{i}(1,:); 86 | S_y{i} = ellipse{i}(2,:); 87 | pl_cov = plot(S_x{i},S_y{i}); 88 | pl_cov.XDataSource = ['S_x{',num2str(i),'}']; 89 | pl_cov.YDataSource = ['S_y{',num2str(i),'}']; 90 | 91 | % plot estimated mean 92 | zk_x{i} = zk(1); 93 | zk_y{i} = zk(2); 94 | pl_mean = scatter(zk_x{i}, zk_y{i},100, 'o', 'filled'); 95 | pl_mean.XDataSource = ['zk_x{',num2str(i),'}']; 96 | pl_mean.YDataSource = ['zk_y{',num2str(i),'}']; 97 | end 98 | 99 | 100 | % --- ITERATE ANIMATION --- 101 | for k=1:numel(measdata) 102 | 103 | % meas 104 | meas_x = measdata{k}(1,:); 105 | meas_y = measdata{k}(2,:); 106 | 107 | for i=1:n 108 | [zk,S] = obj.estimated_meas(est(k).x(:,i),est(k).P(:,:,i),measmodel); 109 | ellipse = obj.sigmaEllipse2D(zk, S); 110 | % variance 111 | S_x{i} = ellipse(1,:); 112 | S_y{i} = ellipse(2,:); 113 | % mean 114 | zk_x{i} = zk(1); 115 | zk_y{i} = zk(2); 116 | end 117 | 118 | refreshdata(fig,'caller'); 119 | drawnow; 120 | % pause(0.05); 121 | end 122 | end 123 | 124 | end 125 | end 126 | 127 | 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /HA04/MATLABGraderTestPackage/HA4_test/SA5Ex1Test9.m: -------------------------------------------------------------------------------- 1 | %Choose object detection probability 2 | P_D = 0.9; 3 | %Choose clutter rate 4 | lambda_c = 10; 5 | %Choose object survival probability 6 | P_S = 0.99; 7 | %Create sensor model 8 | %Range/bearing measurement range 9 | range_c = [-1000 1000;-pi pi]; 10 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 11 | 12 | %Create nonlinear motion model (coordinate turn) 13 | T = 1; 14 | sigmaV = 1; 15 | sigmaOmega = pi/180; 16 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 17 | 18 | %Create nonlinear measurement model (range/bearing) 19 | sigma_r = 5; 20 | sigma_b = pi/180; 21 | s = [300;400]; 22 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 23 | 24 | %Creat ground truth model 25 | nbirths = 4; 26 | K = 10; 27 | tbirth = zeros(nbirths,1); 28 | tdeath = zeros(nbirths,1); 29 | 30 | initial_state(1).x = [0; 0; 5; 0; pi/180]; tbirth(1) = 1; tdeath(1) = 50; 31 | initial_state(2).x = [20; 20; -20; 0; pi/90]; tbirth(2) = 20; tdeath(2) = 70; 32 | initial_state(3).x = [-20; 10; -10; 0; pi/360]; tbirth(3) = 40; tdeath(3) = 90; 33 | initial_state(4).x = [-10; -10; 8; 0; pi/270]; tbirth(4) = 60; tdeath(4) = K; 34 | 35 | birth_model = repmat(struct('w',log(0.03),'x',[],'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,4]); 36 | birth_model(1).x = [0; 0; 5; 0; pi/180]; 37 | birth_model(2).x = [20; 20; -20; 0; pi/90]; 38 | birth_model(3).x = [-20; 10; -10; 0; pi/360]; 39 | birth_model(4).x = [-10; -10; 8; 0; pi/270]; 40 | 41 | %Generate true object data (noisy or noiseless) and measurement data 42 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 43 | ifnoisy = 0; 44 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 45 | measdata = measdatagen(objectdata,sensor_model,meas_model); 46 | 47 | %Object tracker parameter setting 48 | P_G = 0.999; %gating size in percentage 49 | w_min = 1e-3; %hypothesis pruning threshold 50 | merging_threshold = 2; %hypothesis merging threshold 51 | M = 100; %maximum number of hypotheses kept 52 | r_min = 1e-3; %Bernoulli component pruning threshold 53 | r_recycle = 0.1; %Bernoulli component recycling threshold 54 | r_estimate = 0.4; %Threshold used to extract estimates from Bernoullis 55 | density_class_handle = feval(@GaussianDensity); %density class handle 56 | tracker = multiobjectracker(); 57 | tracker = tracker.initialize(density_class_handle,P_S,P_G,meas_model.d,w_min,merging_threshold,M,r_min,r_recycle,r_estimate); 58 | 59 | %Create a class instance 60 | PMBM_ref = reference.PMBMfilter(); 61 | PMBM_ref = PMBM_ref.initialize(tracker.density,birth_model); 62 | 63 | %Create a class instance 64 | PMBM = PMBMfilter(); 65 | PMBM = PMBM.initialize(tracker.density,birth_model); 66 | 67 | for k = 1:K 68 | % Run learner solution. 69 | % PMBM = PMBM_ref; 70 | PMBM = PMBM_update(PMBM,measdata{k},meas_model,sensor_model,tracker.gating,tracker.reduction.w_min,tracker.reduction.M); 71 | % Run reference solution. 72 | %PMBM update 73 | PMBM_ref = PMBM_ref.PMBM_update(measdata{k},meas_model,sensor_model,tracker.gating,tracker.reduction.w_min,tracker.reduction.M); 74 | 75 | PPP_ref.w = PMBM_ref.paras.PPP.w; 76 | [PPP_ref_w,I] = sort(PPP_ref.w); 77 | PPP_ref.states = PMBM_ref.paras.PPP.states; 78 | PPP_ref_states = PPP_ref.states(I); 79 | 80 | MBM.w_ref = PMBM_ref.paras.MBM.w; 81 | [MBM_w_ref,Iw] = sort(MBM.w_ref); 82 | 83 | tt_ref = cellfun(@(x) x(1).state.x(1), PMBM_ref.paras.MBM.tt); 84 | [~,I] = sort(tt_ref); 85 | temp = PMBM_ref.paras.MBM.tt(I); 86 | MBM_tt_ref = cell(length(temp),1); 87 | for i = 1:length(temp) 88 | state_ref = arrayfun(@(x) x.state.x(1), temp{i}); 89 | [~,I] = sort(state_ref); 90 | MBM_tt_ref{i} = temp{i}(I); 91 | end 92 | 93 | PPP.w = PMBM.paras.PPP.w; 94 | [PPP_w,I] = sort(PPP.w); 95 | PPP.states = PMBM.paras.PPP.states; 96 | PPP_states = PPP.states(I); 97 | 98 | MBM.w = PMBM.paras.MBM.w; 99 | [MBM_w,Iw] = sort(MBM.w); 100 | 101 | tt = cellfun(@(x) x(1).state.x(1), PMBM.paras.MBM.tt); 102 | [~,I] = sort(tt); 103 | temp = PMBM.paras.MBM.tt(I); 104 | MBM_tt = cell(length(temp),1); 105 | for i = 1:length(temp) 106 | state = arrayfun(@(x) x.state.x(1), temp{i}); 107 | [~,I] = sort(state); 108 | MBM_tt{i} = temp{i}(I); 109 | end 110 | 111 | %PMBM prediction 112 | PMBM_ref = PMBM_ref.PMBM_predict(P_S,motion_model,birth_model); 113 | PMBM = PMBM.PMBM_predict(P_S,motion_model,birth_model); 114 | 115 | % Compare. 116 | assessVariableEqual('PPP_ref_w', PPP_w,'RelativeTolerance',0.0001,'Feedback','Parameter PPP.w is incorrect, please modify your code and submit again.'); 117 | assessVariableEqual('PPP_ref_states', PPP_states,'RelativeTolerance',0.001,'Feedback','Parameter PPP.states is incorrect, please modify your code and submit again.'); 118 | assessVariableEqual('MBM_w_ref', MBM_w,'RelativeTolerance',0.0001,'Feedback','Parameter MBM.w is incorrect, please modify your code and submit again.'); 119 | assessVariableEqual('MBM_tt_ref', MBM_tt,'RelativeTolerance',0.001,'Feedback','Parameter MBM.tt is incorrect, please modify your code and submit again.'); 120 | end 121 | 122 | 123 | -------------------------------------------------------------------------------- /HA01/simulation.m: -------------------------------------------------------------------------------- 1 | %You can use this live script to generate your own tracking scenario and apply the single 2 | %object trackers you have implemented. Try to compare the estimation performance of 3 | %different trackers, from simple scenario with linear motion/measurement model, high 4 | %object detection probability and low clutter rate to more complex scenario with 5 | %nonlinear motion/measurement model, low detection probability and high clutter rate. 6 | %In complex scenarios, it should be easy to verify that, on average, the Gaussian sum 7 | %filter has the best performance. We also encourage you to tune the different model 8 | %parameters and to observe how they affact the tracking performance. 9 | %You might also be interested in writting your own plotting function to illustrate 10 | %the estimation uncertainty of the object state and study how it changes over time. 11 | 12 | clear; close all; clc 13 | dbstop if error 14 | 15 | 16 | %Choose object detection probability 17 | P_D = 0.5; 18 | %Choose clutter rate 19 | lambda_c = 100; 20 | 21 | %Choose linear or nonlinear scenario 22 | scenario_type = 'nonlinear'; 23 | 24 | %Create tracking scenario 25 | switch(scenario_type) 26 | case 'linear' 27 | %Create sensor model 28 | range_c = [-1000 1000;-1000 1000]; 29 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 30 | 31 | %Create ground truth model 32 | nbirths = 1; 33 | K = 100; 34 | initial_state.x = [0; 0; 10; 10]; 35 | initial_state.P = eye(4); 36 | ground_truth = modelgen.groundtruth(nbirths,initial_state.x,1,K+1,K); 37 | 38 | %Create linear motion model 39 | T = 1; 40 | sigma_q = 5; 41 | motion_model = motionmodel.cvmodel(T,sigma_q); 42 | 43 | %Create linear measurement model 44 | sigma_r = 10; 45 | meas_model = measmodel.cvmeasmodel(sigma_r); 46 | 47 | case 'nonlinear' 48 | %Create sensor model 49 | %Range/bearing measurement range 50 | range_c = [-1000 1000;-pi pi]; 51 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 52 | 53 | %Creat ground truth model 54 | nbirths = 1; 55 | K = 100; 56 | initial_state.x = [0; 0; 10; 0; pi/180]; 57 | initial_state.P = diag([1 1 1 1*pi/180 1*pi/180].^2); 58 | ground_truth = modelgen.groundtruth(nbirths,initial_state.x,1,K+1,K); 59 | 60 | %Create nonlinear motion model (coordinate turn) 61 | T = 1; 62 | sigmaV = 1; 63 | sigmaOmega = pi/180; 64 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 65 | 66 | %Create nonlinear measurement model (range/bearing) 67 | sigma_r = 5; 68 | sigma_b = pi/180; 69 | s = [300;400]; 70 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 71 | end 72 | 73 | 74 | %Generate true object data (noisy or noiseless) and measurement data 75 | ifnoisy = 0; 76 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 77 | measdata = measdatagen(objectdata,sensor_model,meas_model); 78 | 79 | 80 | %Single object tracker parameter setting 81 | P_G = 0.999; %gating size in percentage 82 | w_min = 1e-3; %hypothesis pruning threshold 83 | merging_threshold = 2; %hypothesis merging threshold 84 | M = 100; %maximum number of hypotheses kept in MHT 85 | density_class_handle = feval(@GaussianDensity); %density class handle 86 | tracker = singleobjectracker(); 87 | tracker = tracker.initialize(density_class_handle,P_G,meas_model.d,w_min,merging_threshold,M); 88 | 89 | %Nearest neighbour filter 90 | nearestNeighborEstimates = nearestNeighbourFilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 91 | nearestNeighborRMSE = RMSE(nearestNeighborEstimates,objectdata.X); 92 | 93 | %Probabilistic data association filter 94 | probDataAssocEstimates = probDataAssocFilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 95 | probDataAssocRMSE = RMSE(probDataAssocEstimates,objectdata.X); 96 | 97 | %Gaussian sum filter 98 | GaussianSumEstimates = GaussianSumFilter(tracker, initial_state, measdata, sensor_model, motion_model, meas_model); 99 | GaussianSumRMSE = RMSE(GaussianSumEstimates,objectdata.X); 100 | 101 | X = sprintf('Root mean square error: Nearest neighbour: %.3f; Probabilistic data association: %.3f; Gaussian sum filtering: %.3f.'... 102 | ,nearestNeighborRMSE,probDataAssocRMSE,GaussianSumRMSE); 103 | disp(X) 104 | 105 | %Ploting 106 | true_state = cell2mat(objectdata.X'); 107 | NN_estimated_state = cell2mat(nearestNeighborEstimates'); 108 | PDA_estimated_state = cell2mat(probDataAssocEstimates'); 109 | GS_estimated_state = cell2mat(GaussianSumEstimates'); 110 | 111 | figure 112 | hold on 113 | grid on 114 | 115 | plot(true_state(1,:), true_state(2,:), 'g','Linewidth', 2) 116 | plot(NN_estimated_state(1,:), NN_estimated_state(2,:), 'r-s' , 'Linewidth', 1) 117 | plot(PDA_estimated_state(1,:), PDA_estimated_state(2,:), 'm-o' , 'Linewidth', 1) 118 | plot(GS_estimated_state(1,:), GS_estimated_state(2,:), 'b-d' , 'Linewidth', 1) 119 | 120 | xlabel('x (m)') 121 | ylabel('y (m)') 122 | legend('Ground Truth','Nearest Neighbour', 'Probalistic Data Association', 'Gaussian Sum', 'Location', 'best') 123 | 124 | set(gca,'FontSize',12) 125 | -------------------------------------------------------------------------------- /HA03/multiobjectracker_HA3.m: -------------------------------------------------------------------------------- 1 | classdef multiobjectracker 2 | %MULTIOBJECTRACKER is a class containing functions to track multiple 3 | %object in clutter. 4 | %Model structures need to be called: 5 | %sensormodel: a structure specifies the sensor parameters 6 | % P_D: object detection probability --- scalar 7 | % lambda_c: average number of clutter measurements per time 8 | % scan, Poisson distributed --- scalar 9 | % pdf_c: value of clutter pdf --- scalar 10 | % intensity_c: clutter (Poisson) intensity --- scalar 11 | %motionmodel: a structure specifies the motion model parameters 12 | % d: object state dimension --- scalar 13 | % F: function handle return transition/Jacobian matrix 14 | % f: function handle return predicted object state 15 | % Q: motion noise covariance matrix 16 | %measmodel: a structure specifies the measurement model parameters 17 | % d: measurement dimension --- scalar 18 | % H: function handle return transition/Jacobian matrix 19 | % h: function handle return the observation of the object 20 | % state 21 | % R: measurement noise covariance matrix 22 | 23 | properties 24 | gating %specify gating parameter 25 | reduction %specify hypothesis reduction parameter 26 | density %density class handle 27 | P_S %object survival probability 28 | end 29 | 30 | methods 31 | 32 | function obj = initialize(obj,density_class_handle,P_S,P_G,m_d,w_min,merging_threshold,M) 33 | %INITIATOR initializes singleobjectracker class 34 | %INPUT: density_class_handle: density class handle 35 | % P_S: object survival probability 36 | % P_G: gating size in decimal --- scalar 37 | % m_d: measurement dimension --- scalar 38 | % wmin: allowed minimum hypothesis weight --- scalar 39 | % merging_threshold: merging threshold --- scalar 40 | % M: allowed maximum number of hypotheses --- scalar 41 | %OUTPUT: obj.density: density class handle 42 | % obj.gating.P_G: gating size in decimal --- scalar 43 | % obj.gating.size: gating size --- scalar 44 | % obj.reduction.w_min: allowed minimum hypothesis 45 | % weight in logarithmic scale --- scalar 46 | % obj.reduction.merging_threshold: merging threshold 47 | % --- scalar 48 | % obj.reduction.M: allowed maximum number of hypotheses 49 | % --- scalar 50 | % obj.P_S: object survival probability 51 | obj.density = density_class_handle; 52 | obj.gating.P_G = P_G; 53 | obj.gating.size = chi2inv(obj.gating.P_G,m_d); 54 | obj.reduction.w_min = log(w_min); 55 | obj.reduction.merging_threshold = merging_threshold; 56 | obj.reduction.M = M; 57 | obj.P_S = P_S; 58 | end 59 | 60 | function estimates = GMPHDfilter(obj, birthmodel, Z, sensormodel, motionmodel, measmodel) 61 | %GMPHDFILTER tracks multiple objects using Gaussian mixture 62 | %probability hypothesis density filter 63 | %INPUT: birthmodel: object birth model: structure array of size 64 | % (1, number of hypothesised new born objects per time 65 | % step) with three fields: 66 | % w: object birth weight --- scalar in 67 | % logarithm domain 68 | % x: object initial state mean --- (object 69 | % state dimension) x 1 vector 70 | % P: object initial state covariance --- 71 | % (object state dimension) x (object state 72 | % dimension) matrix 73 | % Z: cell array of size (total tracking time, 1), each 74 | % cell stores measurements of size (measurement 75 | % dimension) x (number of measurements at corresponding 76 | % time step) 77 | %OUTPUT:estimates: cell array of size (total tracking time, 1), 78 | % each cell stores estimated object state of size (object 79 | % state dimension) x (number of objects) 80 | 81 | %Preallocate memory 82 | K = length(Z); 83 | estimates = cell(K,1); 84 | 85 | %Create a class instance 86 | PPP = PHDfilter(); 87 | %Initialize the PPP 88 | PPP = initialize(PPP,obj.density,birthmodel); 89 | 90 | for k = 1:K 91 | %PPP update 92 | PPP = update(PPP,Z{k},measmodel,sensormodel.intensity_c,sensormodel.P_D,obj.gating); 93 | %PPP approximation 94 | PPP = componentReduction(PPP,obj.reduction); 95 | %Extract state estimates from the PPP 96 | estimates{k} = PHD_estimator(PPP); 97 | %PPP prediction 98 | PPP = predict(PPP,motionmodel,obj.P_S,birthmodel); 99 | end 100 | 101 | end 102 | 103 | end 104 | end 105 | 106 | -------------------------------------------------------------------------------- /HA05/Simulation.m: -------------------------------------------------------------------------------- 1 | clear all; close all; clc 2 | 3 | dbstop if error 4 | 5 | %Choose object detection probability 6 | P_D = 0.98; 7 | %Choose clutter rate 8 | lambda_c = 10; 9 | %Choose object survival probability 10 | P_S = 0.99; 11 | 12 | %Choose linear or nonlinear scenario 13 | scenario_type = 'linear'; 14 | 15 | %% Create tracking scenario 16 | switch(scenario_type) 17 | case 'linear' 18 | %Create sensor model 19 | range_c = [-1000 1000;-1000 1000]; 20 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 21 | 22 | %Create linear motion model 23 | T = 1; 24 | sigma_q = 5; 25 | motion_model = motionmodel.cvmodel(T,sigma_q); 26 | 27 | %Create linear measurement model 28 | sigma_r = 10; 29 | meas_model = measmodel.cvmeasmodel(sigma_r); 30 | 31 | %Create ground truth model 32 | nbirths = 12; 33 | K = 100; 34 | tbirth = zeros(nbirths,1); 35 | tdeath = zeros(nbirths,1); 36 | 37 | initial_state(1).x = [ 0; 0; 0; -10 ]; tbirth(1) = 1; tdeath(1) = 70; 38 | initial_state(2).x = [ 400; -600; -10; 5 ]; tbirth(2) = 1; tdeath(2) = K+1; 39 | initial_state(3).x = [ -800; -200; 20; -5 ]; tbirth(3) = 1; tdeath(3) = 70; 40 | initial_state(4).x = [ 400; -600; -7; -4 ]; tbirth(4) = 20; tdeath(4) = K+1; 41 | initial_state(5).x = [ 400; -600; -2.5; 10 ]; tbirth(5) = 20; tdeath(5) = K+1; 42 | initial_state(6).x = [ 0; 0; 7.5; -5 ]; tbirth(6) = 20; tdeath(6) = K+1; 43 | initial_state(7).x = [ -800; -200; 12; 7 ]; tbirth(7) = 40; tdeath(7) = K+1; 44 | initial_state(8).x = [ -200; 800; 15; -10 ]; tbirth(8) = 40; tdeath(8) = K+1; 45 | initial_state(9).x = [ -800; -200; 3; 15 ]; tbirth(9) = 60; tdeath(9) = K+1; 46 | initial_state(10).x = [ -200; 800; -3; -15 ]; tbirth(10) = 60; tdeath(10) = K+1; 47 | initial_state(11).x = [ 0; 0; -20; -15 ]; tbirth(11) = 80; tdeath(11) = K+1; 48 | initial_state(12).x = [ -200; 800; 15; -5 ]; tbirth(12) = 80; tdeath(12) = K+1; 49 | 50 | birth_model = repmat(struct('w',0.03,'x',[],'P',400*eye(motion_model.d)),[1,4]); 51 | birth_model(1).x = [ 0; 0; 0; 0]; 52 | birth_model(2).x = [ 400; -600; 0; 0]; 53 | birth_model(3).x = [ -800; -200; 0; 0 ]; 54 | birth_model(4).x = [ -200; 800; 0; 0 ]; 55 | 56 | case 'nonlinear' 57 | %Create sensor model 58 | %Range/bearing measurement range 59 | range_c = [-1000 1000;-pi pi]; 60 | sensor_model = modelgen.sensormodel(P_D,lambda_c,range_c); 61 | 62 | %Create nonlinear motion model (coordinate turn) 63 | T = 1; 64 | sigmaV = 1; 65 | sigmaOmega = pi/180; 66 | motion_model = motionmodel.ctmodel(T,sigmaV,sigmaOmega); 67 | 68 | %Create nonlinear measurement model (range/bearing) 69 | sigma_r = 5; 70 | sigma_b = pi/180; 71 | s = [300;400]; 72 | meas_model = measmodel.rangebearingmeasmodel(sigma_r, sigma_b, s); 73 | 74 | %Creat ground truth model 75 | nbirths = 4; 76 | K = 100; 77 | tbirth = zeros(nbirths,1); 78 | tdeath = zeros(nbirths,1); 79 | 80 | initial_state(1).x = [0; 0; 5; 0; pi/180]; tbirth(1) = 1; tdeath(1) = 50; 81 | initial_state(2).x = [20; 20; -20; 0; pi/90]; tbirth(2) = 20; tdeath(2) = 70; 82 | initial_state(3).x = [-20; 10; -10; 0; pi/360]; tbirth(3) = 40; tdeath(3) = 90; 83 | initial_state(4).x = [-10; -10; 8; 0; pi/270]; tbirth(4) = 60; tdeath(4) = K; 84 | 85 | birth_model = repmat(struct('w',0.03,'x',[],'P',diag([1 1 1 1*pi/90 1*pi/90].^2)),[1,4]); 86 | birth_model(1).x = [0; 0; 5; 0; pi/180]; 87 | birth_model(2).x = [20; 20; -20; 0; pi/90]; 88 | birth_model(3).x = [-20; 10; -10; 0; pi/360]; 89 | birth_model(4).x = [-10; -10; 8; 0; pi/270]; 90 | end 91 | 92 | %% Generate true object data (noisy or noiseless) and measurement data 93 | ground_truth = modelgen.groundtruth(nbirths,[initial_state.x],tbirth,tdeath,K); 94 | ifnoisy = 0; 95 | objectdata = objectdatagen(ground_truth,motion_model,ifnoisy); 96 | measdata = measdatagen(objectdata,sensor_model,meas_model); 97 | 98 | %% Object tracker parameter setting 99 | P_G = 0.999; %gating size in percentage 100 | w_min = 1e-4; %hypothesis pruning threshold 101 | M = 100; %maximum number of hypotheses kept 102 | r_min = 1e-4; %Bernoulli component pruning threshold 103 | r_estimate = 0.5; %Threshold used to extract estimates from Bernoullis 104 | % density_class_handle = feval(@trajectoryGaussianFilteringForm); %filtering 105 | density_class_handle = feval(@trajectoryAccumulatedStateDensity); %smoothing-while-filtering 106 | tracker = multiobjectracker(); 107 | tracker = tracker.initialize(density_class_handle,P_S,P_G,meas_model.d,w_min,M,r_min,r_estimate); 108 | 109 | %% Trajectory PMBM filter 110 | PMBMestimates = PMBMtracker(tracker, birth_model, measdata, sensor_model, motion_model, meas_model); 111 | 112 | %% Ploting 113 | true_state = cell2mat(objectdata.X'); 114 | %Extract the set of all trajectories estimates from the last time step 115 | PMBM_estimated_state = cell2mat(PMBMestimates{end}'); 116 | 117 | %Trajectory plot 118 | figure 119 | hold on 120 | grid on 121 | 122 | h1 = plot(true_state(1,:), true_state(2,:), 'bo'); 123 | h2 = plot(PMBM_estimated_state(1,:), PMBM_estimated_state(2,:),'r+'); 124 | 125 | xlabel('x (m)'); ylabel('y (m)') 126 | legend([h1 h2],'Ground Truth','PMBM', 'Location', 'best') 127 | set(gca,'FontSize',12) -------------------------------------------------------------------------------- /HA05/multiobjectracker.m: -------------------------------------------------------------------------------- 1 | classdef multiobjectracker 2 | %MULTIOBJECTRACKER is a class containing functions to track multiple objects in clutter. 3 | %Model structures need to be called: 4 | %sensormodel: a structure specifies the sensor parameters 5 | % P_D: object detection probability --- scalar 6 | % lambda_c: average number of clutter measurements per time 7 | % scan, Poisson distributed --- scalar 8 | % pdf_c: clutter (Poisson) intensity --- scalar 9 | % intensity_c: clutter (Poisson) intensity --- scalar 10 | %motionmodel: a structure specifies the motion model parameters 11 | % d: object state dimension --- scalar 12 | % F: function handle return transition/Jacobian matrix 13 | % f: function handle return predicted object state 14 | % Q: motion noise covariance matrix 15 | %measmodel: a structure specifies the measurement model parameters 16 | % d: measurement dimension --- scalar 17 | % H: function handle return transition/Jacobian matrix 18 | % h: function handle return the observation of the target state 19 | % R: measurement noise covariance matrix 20 | 21 | properties 22 | gating %specify gating parameter 23 | reduction %specify hypothesis reduction parameter 24 | density %density class handle 25 | P_S %object survival probability 26 | r_estimate %threshold used to extract estimates from Bernoullis 27 | end 28 | 29 | methods 30 | 31 | function obj = initialize(obj,density_class_handle,P_S,P_G,m_d,w_min,M,r_min,r_estimate) 32 | %INITIATOR initializes singleobjectracker class 33 | %INPUT: density_class_handle: density class handle 34 | % P_S: object survival probability --- scalar 35 | % P_G: gating size in decimal --- scalar 36 | % m_d: measurement dimension --- scalar 37 | % w_min: allowed minimum hypothesis weight --- scalar 38 | % M: allowed maximum number of hypotheses --- scalar 39 | % r_min: allowed minimum object's probability of 40 | % existence --- scalar 41 | % r_estimate: threshold used to extract estimates from 42 | % Bernoullis --- scalar 43 | %OUTPUT: obj.density: density class handle 44 | % obj.gating.P_G: gating size in decimal --- scalar 45 | % obj.gating.size: gating size --- scalar 46 | % obj.reduction.w_min: allowed minimum hypothesis 47 | % weight in logarithmic scale --- scalar 48 | % obj.reduction.M: allowed maximum number of hypotheses 49 | % --- scalar 50 | % obj.reduction.r_min: allowed minimum object's 51 | % probability of existence --- scalar 52 | % obj.r_estimate: threshold used to extract estimates from 53 | % Bernoullis --- scalar 54 | 55 | obj.density = density_class_handle; 56 | obj.gating.P_G = P_G; 57 | obj.gating.size = chi2inv(obj.gating.P_G,m_d); 58 | obj.reduction.w_min = log(w_min); 59 | obj.reduction.M = M; 60 | obj.reduction.r_min = r_min; 61 | obj.P_S = P_S; 62 | obj.r_estimate = r_estimate; 63 | end 64 | 65 | function estimates = PMBMtracker(obj, birthmodel, Z, sensormodel, motionmodel, measmodel) 66 | %PMBMTRACKER tracks multiple objects using Poisson 67 | %multi-Bernoulli mixture filter 68 | %INPUT: birthmodel: object birth model: structure array of size 69 | % (1, number of hypothesised new born objects per time 70 | % step) with three fields: 71 | % w: object birth weight --- scalar 72 | % x: object initial state mean --- (object 73 | % state dimension) x 1 vector 74 | % P: object initial state covariance --- 75 | % (object state dimension) x (object state 76 | % dimension) matrix 77 | % Z: cell array of size (total tracking time, 1), each 78 | % cell stores measurements of size (measurement 79 | % dimension) x (number of measurements at corresponding 80 | % time step) 81 | %OUTPUT:estimates: cell array of size (total tracking time, 1), 82 | % each cell stores estimated object state of size (object 83 | % state dimension) x (number of objects) 84 | 85 | %Preallocate memory 86 | K = length(Z); 87 | estimates = cell(K,1); 88 | 89 | %Create a class instance 90 | PMBM = PMBMfilter(); 91 | %Initialize the PMBM 92 | PMBM = initialize(PMBM,obj.density,birthmodel); 93 | 94 | for k = 1:K 95 | %PMBM update 96 | PMBM = PMBM_update(PMBM,k,Z{k},measmodel,sensormodel,obj.gating,obj.reduction.w_min,obj.reduction.M); 97 | %Object state extraction 98 | estimates{k} = PMBM_estimator(PMBM,obj.r_estimate); 99 | %Recycling 100 | PMBM = Bern_prune(PMBM,obj.reduction.r_min); 101 | %PPP components reduction 102 | PMBM = PPP_prune(PMBM,obj.reduction.w_min); 103 | %PMBM prediction 104 | PMBM = PMBM_predict(PMBM,obj.P_S,motionmodel,birthmodel,obj.reduction.r_min); 105 | end 106 | 107 | end 108 | 109 | end 110 | end 111 | 112 | --------------------------------------------------------------------------------