Move step matrix stuff to branch