Matrix_Math: added matrix_add_3band_diag