Add simple_kinematics test, fix kinematics_step

Was incorrectly calculating new velocity/position.