Added reference step
Fixes to get a working simulation!