Cleanup for initState and stateStorage
(also added the option to setUToFitAngularVelocity,
needs to override state)