gsidiagExtract function

Internal functions: Get the diagonal of a matrix