Fixed reference speed not to include nans in stopgaps

Fixes #3