Lagrange multiplier update: Lagrange multipliers are updated based on [1]}, [2]} in the following manner.

\({^{k+1}}\lambda _{l} \hspace{-2.84544pt}= \hspace{-1.9919pt}{^{k}}\lambda _{l} \hspace{-2.27626pt}- \rho ( \textbf {F}^{k+1} \xi _{1,l}\hspace{-1.9919pt}-\hspace{-1.9919pt}\textbf {g}_{l}(^{k+1}\xi _{2,l} , ^{k+1}\xi _{3,l},\hspace{-1.13791pt}^{k+1}\xi _{4,l}\hspace{-0.85355pt} ))\textbf {F} \\{^{k+1}}\lambda _{\psi ,l} \hspace{-2.84544pt}= \hspace{-1.42271pt}{^{k}}\lambda _{\psi ,l} \hspace{-1.9919pt}-\rho _{\psi }(\arctan 2({^{k+1}}s_{l},{^{k+1}}c_{l})-{^{k+1}}\psi _{l})\textbf {P}\) 
