These fixes are provided by neurocopter but with my name because of
merge conflicts
_position and _velocity vectors added to save some floating point add
operations
unused reference to ins in constructor removed