Ensure NaN check is executed before any parts of the filter are run. Fix...

... GPS velocity reset for case of initialized GPS

@sjwilks Could you give this a try?