getContouringCost() detailed description

[Geonhee-LEE]

Hi, @alexliniger. Thank you for your contribution! I successfully run the C++ execution. Then I have been trying to understand your codes but confront the difficult parts about getContouringCost() function:

https://github.com/alexliniger/MPCC/blob/84cc61d628a165a424c805bbe071fe96b88da2d0/C%2B%2B/Cost/cost.cpp#L140

I don't understand (1) why partial derivatives(gradient) of contouring error are included in Q_contouring_cost and (2) why contouring_error_zero is multiplied by d_contouring_error:

https://github.com/alexliniger/MPCC/blob/84cc61d628a165a424c805bbe071fe96b88da2d0/C%2B%2B/Cost/cost.cpp#L164

https://github.com/alexliniger/MPCC/blob/84cc61d628a165a424c805bbe071fe96b88da2d0/C%2B%2B/Cost/cost.cpp#L170

I already read your paper about Optimization-Based Autonomous Racing of 1:43 Scale RC Cars. I guess $\Gamma_k$ may means the corresponding matrix operations but couldn't find the detailed description.

Please help me if you possible.

In advance I appreciate it. Regards.

[Geonhee-LEE]

I found the similar issue (https://github.com/alexliniger/MPCC/issues/29). However, What I don't understand is that I know the Hassian matrix should be calculated by $\Delta x ^ T$ and $\Delta x$ but MPCC contouring quadratic cost function is calculated with $x^T$ and $x$. Or am I misunderstanding?

In short, I want to know which of states or states's differences are calculated when cost matrices are calculated into scalar cost.

If $x_k$ is multiplied with quadratic cost matrix($Q_k$), I wonder what I didn't catch.

[alexliniger]

The paper is maybe a bit misleading since we tried to formulate it nicely as a cost function, but not necessarily how you would implement it. Γ is the weight matrix and most of the complexity of what you can find in the code is part of the term around Γ.

Regarding Δx vs x, I deal with this using the constant term discussed in this comment https://github.com/alexliniger/MPCC/issues/29#issuecomment-774301971 Δx = (x - x0). Depending on the implementation this term is used differently. The master implementation follows the paper, the implementation in the two other branches is a more classical SQP approach where the optimization variable is Δx.

For the contouring cost only X, Y, and s have an influence.

[npu-wkl]

Hi, @alexliniger. why contouring_error_zero is multiplied by d_contouring_error in q_contouring_cost?

[alexliniger]

Can you be a bit more specific? And maybe the discussion in the linked issue may help.

Best, Alex

[npu-wkl]

Thank you @alexliniger ,I just sent you an email to your gmail detailing my confusion.Hope to get your guidance

[alexliniger]

There are two steps which are important, if we have the cost which is f(x)^2, we first approximate f(x) as an affine function using a Taylor series expansion

f(x) ~ df/dx(x-x0) + f(x0) = df/dx x + [f(x0) - df/dx x0]

If we rewrite this as f(x) ~ Gx + g we can no approximate f(x)^2

(Gx + g)^T (Gx + g) = x^T G^T G x + 2 g^T G x + g^T g

where the last term is constant, and the others are basically matched to the solver. Note that we need to multiple G^T G by 2, because the solver uses a 0.5 in front of the quadratic cost.

[npu-wkl]

@alexliniger Your explanation is very detailed and I have understood it