everything is now working. I have solved the problem of velocity NaN
values and have fixed the position update problem
Basically finished, going to convert to aspectJ project for final tests
All tests have run, results are in