Function constrainedLevenbergMarquardt

Performs constrained Levenberg-Marquardt optimization for nonlinear least squares problems.

Algorithm:

1. Start with initial parameters p0, states x0, and lambda (damping parameter)
2. Compute effective Jacobian J_eff = r_p - r_x C_x^+ C_p
3. Solve damped normal equations: (J_eff^T J_eff + λI) δ = -J_eff^T r
4. Try step: p_new = p_old + δ, x_new updated using linear approximation
5. If cost decreases: accept step, decrease lambda
6. If cost increases: reject step, increase lambda
7. Repeat until convergence

The damping parameter lambda interpolates between:

• Constrained Gauss-Newton (λ → 0): fast convergence near solution
• Constrained gradient descent (λ → ∞): robust but slow