Hierarchical control matlab