[pillefitz]

You can implement it as MPC and minimize e.g. the trace of the covariance matrix that describes the state uncertainty. Wrote my thesis on something similar.