This is a matlab implementation to generate the cubic spiral which is illustrated in Self driving courses in Coursera.
Here, I used the syms in matlab to get the expression of the objective function and its gradient. The reason why I use this method to calculate the gradient is that it's difficult to get the correct one by manual computation. Please refer to gradient.m and gradient.txt for details.
Then, I tried to use L-BFGS-B to solve this nolinear programming problem. Run cubic_spiral.m to get the example of cubic spiral. The following images show different cubic spirals when they end at different poses.
kappa_final = 0 theta_final = 0.5*pi:
kappa_final = 0.15 theta_final = 0.5*pi:
kappa_final = 0.15 theta_final = pi:
