Working fine

Error corrected, seems to work OK (tested on 2x2 matrix)