fix matrix diag bug