I have a path of state vectors where each state vector consist on:
$$ \underline{x} = \left[ \begin{array}{c} x_{trailer}\\ y_{trailer}\\ \theta_{trailer} \end{array} \right] $$ $$ \underline{u} = \left[ \begin{array}{c} \kappa_{trailer}\\ \Delta_{s} \end{array} \right] $$
I want to smooth my current path making use of iLQR and for that I have defined my cost function as: $$ J = \sum_{\tau=0}^{N-1}l(x_{i},u_{i})+l_{f}(x_{N}) $$ $$ l(x_{i},u_{i})= W_u \cdot u_{i}^2 + W_{jerk} \cdot \frac{(u_{i+1}-u_i)^{2}}{\Delta_{s_i}} + W_{snap}\cdot \frac{(u_{i+1}-2u_i +u_{i-1})^2}{\Delta_{s_i}^2}+ W_{\text{traj}} \cdot (\underline{x_{i}}-\underline{x}_{\tau_{i}})^{2} $$ Where $W_u$, $W_{jerk}$, $W_{snap}$ and $W_{\text{traj}}$ are weighting matrices known in advance. The problem is this cost function is not quadratic right now due to $\Delta_{s_i}$ in the denominator but I need to include it as I want to minimize first and second derivative of $\kappa$, which are approximated by the terms after $W_{jerk}$ and $W_{snap}$ in $J$.
Moreover, my state dynamics are the following ones (consider a discretized system where dS defines the amount of change between two consecutive states):
$$ \dot{x} = \left[ \begin{array}{c} \Delta_{s_i} \cdot cos(\theta_{trailer}) \\ \Delta_{s_i} \cdot sin(\theta_{trailer}) \\ \Delta_{s_i} \cdot \kappa_{trailer} \end{array} \right] $$