Working PF with constant motion model! Replaced update and resample functions with own code (predict, weight, resample (low variance sampler)).