ekf: Put reset statements after variable zero operation to ensure values get initialized correctly
Better error handling / reporting in filter
Added trap to filter to catch NaN handling