Tried to add math to particle filter