Refactoring
Some starting penalty force code
More progress towards velocity filter