Improve NSD implementation, add the matrix case