Clear matrix between steps
Fix algorithm