stsm_bdiag function

Build a block diagonal matrix from two matrices