More accurate jerk limits

[GregorKikelj]

No description provided.

[GregorKikelj]

Here's the idea

$a_{lat}=v^2\kappa$, where $\kappa$ is the curvature of the current path. To get jerk we need to differentiate the expression.
$$\dot{a_{lat}}=j=(v^2\kappa)'=(v^2)'\kappa+v^2(\kappa)'=2v\cdot (v)'\kappa+v^2\dot{\kappa}=2va\kappa+v^2\dot{\kappa}$$

We know that $-j_max\le j\le j_{max}$.

Expressing $\dot{\kappa}$ we get
$$-j_{max}\le 2va\kappa+v^2\dot{\kappa} \le j_{max}$$
$$-j_{max}-2va\kappa\le v^2\dot{\kappa} \le j_{max}-2va\kappa$$
$$\frac{-j_{max}-2va\kappa}{v^2}\le \dot{\kappa} \le \frac{j_{max}-2va\kappa}{v^2}$$

which needs to hold to ensure jerk is within the $5\frac{m}{s^3}$ limit.

The term $2va\kappa$ is not insignificant when computing jerk. For example at $10m/s$ with a lateral acceleration of $3m/s^2$ and longitudinal acceleration of $2m/s^2$ we get $\kappa=0.03$ and $2va\kappa=1.2m/s^3$

[haraschax]

Math looks correct!

But, not sure if I see the need to fix these. It works fine as is and I'd rather not complicate this code. Can we just add a comment instead noting it isn't exactly correct and the correct formula?

[haraschax]

Math looks correct!

But, not sure if I see the need to fix these. It works fine as is and I'd rather not complicate this code. Can we just add a comment instead noting it isn't exactly correct and the correct formula?