Added the diag function to make diagonal matrices.