State-command structures

Connect state feedback and observers to tracking commands instead of only regulation.