alexliniger
commented
3 years ago - this is a naive initial guess when no solution from the last iteration is available. In normal operation the initial guess is the shifted solution from the last time step
- The cost is not computed based on the initial guess, the initial guess is only required to compute the QP approximation of the NLP
- The mixing term is mainly for stability of the algorithm, ideally one would solve a line search to determine this parameter dynamically
Hello, I have a doubt, can you point out my mistake In the generateNewInitialGuess function, the control variables are set to 0; the initialGuess state variables of N steps are obtained, and the sqpmixing variable and the optimization result are used to obtain an average value. my question is,
Is initialGuess calculated only based on the reference track? If so, in the subsequent cost calculation, the error of each step N is also calculated based on the initialGuess and the reference trajectory. Shouldn't the error always be 0? Take the code as an example to illustrate: CostMatrix Cost::getHeadingCost(const ArcLengthSpline &track, const State &x, int k) const { // get heading of the track const Eigen::Vector2d dpos_ref = track.getDerivative(x.s); const double dx_ref = dpos_ref(0); const double dy_ref = dpos_ref(1); // angle of the reference path double theta_ref = atan2(dy_ref, dx_ref); theta_ref += 2.0 M_PI std::round((x.phi - theta_ref) / (2.0 * M_PI));
....... } the input State x is based on the reference track, and theta_ref is based on the track (x.s), so the x.phi == theta_ref all the time??