Theoretical question

[hect1995]

I have a reference trajectory that I want to follow but I want as well to reduce some input in my truck (lets say the steering of the vehicle in order to smooth a little bit the original trajectory). The dynamics of my truck are non-linear so every time step I linearize them based on my current trajectory and current controls. I have read that when you linearize the resulting state and control vector are no longer x and u, but x* = x-x_linearized and u*=u-u_linearized. So my question is if what I say is true, how can I adapt my cost function to follow trajectory but reduce steering? Does it make sense something like:
J= np.dot(state_vector, state_vector)+np.dot(control_vector+current_control, control_vector+current_control)
Where current_control is the control obtained in the last iteration so as we are linearizing based on them if we do u*+u_linearized I obtain u which is what I want to linearize. Does it make sense?

[hect1995]

Moreover, Whe I apply the iLQR algorithm in order to smooth a trajectory I have (I try to reduce the constrols while not diverging too much from the trajectory to follow) if the trajectory I am considering has many steps the Quu results in infinite and the algorithm breaks. How could it be solved?