The calculation of FrenetPathDynamics::dx

[zhangnage123]

Thanks for sharing your great work!

There is some confusion regarding the calculation of FrenetPathDynamics::dx. The system dynamics model should be as follows.

I understand that in the code, the control variable kappa_rate should be the derivative of kappa with respect to time t, whereas in the dynamic model, it is the derivative of kappa with respect to s.Therefore, there should be a conversion here.

{\Large {\color{Red} \frac{\mathrm{d}k }{\mathrm{ds} } = \frac{\mathrm{d} k}{\mathrm{d}t } \frac{\mathrm{d}t }{\mathrm{d}s } 
=\frac{\dot{k} }{\dot{s} } } } 

where $${\Large {\color{Red} \dot s = \frac{v \cos e_{\psi}}{1 - k_r l} } } $$ so $${\color{Red} {\Large k^\prime = \frac{\dot k}{ \dot s} = \dot k \frac{1 - krl}{v \cos e\psi } } } $$
I noticed that v disappeared when calculating the state matrix. I wonder why? Maybe I missed something.

 ret << 1 - move_dist * tan(hd) * kappa_ref, move_dist * (1 - kappa_ref * l) / cos(hd) / cos(hd), 0.0,
        -move_dist * kappa * kappa_ref / cos(hd), 1 + move_dist * (1 - kappa_ref * l) * kappa * tan(hd) / cos(hd), move_dist * (1 - kappa_ref * l) / cos(hd),
        -move_dist * kappa_rate * kappa_ref / cos(hd), move_dist * (1 - kappa_ref * l) * kappa_rate * tan(hd) / cos(hd), 1.0;

[LiJiangnanBit]

Hi, The control variable is the kappa rate w.r.t the actual path length $\bar s$, instead of the length for the reference line length $s$. so $u = {dk \over d\bar s}$, where $v = {d\bar s \over dt}$. If we put $u$ in the penalty term, we are actually optimizing the lateral comfortability regardless of the shape of the reference line. But the dynamics are parameterized by the ref $s$. The dynamics for $k$ become:

$$ {dk \over ds} = {{ dk \over d\bar s}{d\bar s \over dt} \over {ds \over dt}} = {uv \over ({vcos(e_\phi) \over (1-k_r l)})} = ... $$

so $v$ is eliminated. Hope this helps :)

[zhangnage123]

got it, thank you for the reply.