Working PF with constant velocity model! (instead of const pos)