Support `OrthogonalCollocation` for the `MovingHorizonEstimator`

[franckgaga]

This is how I will be able to close #332. It should not be too hard, now that the machinery for linear and nonlinear equality constraints is established in the MovingHorizonEstimator.