bdiag_check function

Create block diagonal matrix (safe version)