Profiler: Trajectory is saved incorrectly

[jara001]

Added in 1702cf4 as a comment from Tomáš.

_beta = math.atan(_lr * k[_i])  # << k[_i] in the CoG
"k_radpm": k[_i],  # << k[i] in the CoG
"vx_mps": v[_i] * math.cos(_beta),  # << k[_i] and v[_i] in the CoG
"vy_mps": v[_i] * math.sin(_beta),  # << k[_i] and v[_i] in the CoG
"a_mps2": a[_i],  # << a[_i] in the center of gravity
"omega_radps": v[_i] * math.cos(_beta) * k[_i], # << k[_i] and v[_i] in the CoG
"delta_rad": np.atan((_lf + _lr) * k[_i])  # << k[_i] in the center of rear axle

Purely rear axle should be:

# _beta not required
writer.writerow(
    {
        "t_s": t[_i],
        "d_m": d[_i],
        "x_m": x[_i],
        "y_m": y[_i],
        "psi_rad": psi[_i],
        "k_radpm": k[_i],
        "vx_mps": v[_i],
        "vy_mps": 0,
        "a_mps2": a[_i],
        "omega_radps": v[_i] * k[_i],
        "delta_rad": math.atan((_lf + _lr) * k[_i])
    }
)

Purely CoG should be:

_beta = math.asin(_lr * k[_i])

writer.writerow(
    {
        "t_s": t[_i],
        "d_m": d[_i],
        "x_m": x[_i],
        "y_m": y[_i],
        "psi_rad": psi[_i] - _beta,
        "k_radpm": k[_i],
        "vx_mps": v[_i] * math.cos(_beta),
        "vy_mps": v[_i] * math.sin(_beta),
        "a_mps2": a[_i],
        "omega_radps": v[_i] * k[_i],
        "delta_rad": math.atan((_lf + _lr)/_lr * math.tan(_beta))
    }
)

[jara001]

My flow to investigate further:

x, y, d, k are clear, as the car is at that position; in addition the car is there at time t.

That leaves us psi, vx, vy, a, omega and delta.

Heading

Heading psi should be a difference in the position, as the car should track the trajectory precisely. I think that is should match in both cases.

Velocity

Velocity v (or vx, vy) is expressed with respect to the CoG. This should be an image of velocity placed in the center of the rear axle as;

F_y = m · dpsi · v_x = m · k · v_x^2

constrains the maximum velocity and is placed in the center of the rear axle. In addition, this is a velocity that is computed in the optimization. To create the image of velocity in the CoG (and its components) we use angle Beta:

Beta = atan(l_r · k)

Therefore, v_x = v · cos(Beta) and v_y = v · sin(Beta). And I agree that for rear axle frame, v_x = v and v_y = 0.

In addition, Autoware Design says, that v_x, v_y should be referred to in the CoG.

Acceleration

No differences here.

Heading rate

No differences here.

Steering

(Really) original version was: delta = L · k which used some angle assumption and led to wrong values. Currently used version is:

delta = atan(L · k)

and the suggested version is:

delta = atan(L / l_r · tan(Beta))

However, they are equal as:

Beta = atan(l_r · k) → tan(Beta) = l_r · k → k = tan(Beta) / l_r
delta = atan(L / l_r · tan(Beta)) = atan(L · (tan(Beta) / l_r)) = atan(L · k)