Add checks on NaN's when updating in simulation

This MR adds some checks on the state vector before and after the update step, as nan's may occur and propagate without any warning.