slowly manually merging trajectory2
particle filter lab commit 1