Clean stage, add helper vec->mat
Add stability checking to stage