Make identity matrix diagonal explicit in state vector