bdiagMatrix function

Create a block diagonal matrix