Workin PF with const vel model, estimated speeds instead of measuring it