Merit function scaling

[ManifoldFR]

At the moment, the merit function looks like (for the primal, proximal augmented Lagrangian with no constraints other than system dynamics)

$$ \mathcal{L}_k(\mathbf x, \mathbf u; \boldsymbol\lambda^k) = \sum_{i=0}^{N-1} \ell_i(x_i, u_i) + \frac{1}{2\mu_k} \| f(x_i, u_i, x_{i+1}) + \mu_k\lambda_{i+1}^k \|^2_2 + \frac{\rho_k}2 \| (\mathbf x, \mathbf u) \ominus (\mathbf x^k, \mathbf u^k) \|_2^2 $$

The issue here is the scaling of the penalty function, which scales with the number of nodes N and makes the merit function much steeper for problems with more discretization nodes.

My idea:

• keep (μ_k) the same and controlled by BCL (or filter in the future see Filter line-search #31)
• scale the penalty terms according to number of nodes or timestep
• adjust the direction computation accordingly