Added code to reroute the columns of allP and allK to take into consideration state blocks. 
Added code to reroute the columns of allP and allK to take into consideration state blocks. 

For example, if A = eye(3) and the 3rd column should be added to the first, the resulting matrix will be A = [1 0; 0 1; 1 0];

MPC-Berkeley/BLOM,MPC-Berkeley/BLOM,MPC-Berkeley/BLOM