state matrix add iterator support

RowMajor StateMatrix's col iterator can be less efficient
ColMajor StateMatrix's row iterator can be less efficient