[Bug Report] Physics error results in high jerk

[Gamenot]

High Level Description

Jerk is the third derivative as relates to position. This results in situations like the following where jerk can become very large at low velocity:

# p: position, dt: time step(delta time), v: velocity, j: jerk
>>> p1 = 0.000
>>> p2 = 0.001 # (perhaps due to accuracy or physics error)
>>> p3 = 0.000
>>> p4 = 0.000
>>> dt = 0.1
>>> v1 = (p2 - p1) / dt
>>> v2 = (p3 - p2) / dt
>>> v3 = (p4 - p3) / dt
>>> v1
0.01
>>> a1 = (v2 - v1) / dt
>>> a2 = (v3 - v2) / dt
>>> a1
-0.19999999999999998
>>> j = (a2 - a1) / dt
>>> j
2.9999999999999996

A value of 3/mps^3 is very high as relates to the safe and comfortable values of jerk. Currently, this is not mitigated in SMARTS.

Version

Version 1.0.11

Steps to reproduce the bug

No response

System info

No response

Error logs and screenshots

No response

Impact (If known)

No response

[Adaickalavan]

If we are unable to completely avoid vehicle position inaccuracies or jitters, then consider the following:

def get_acc_jerk() -> Callable[[Observation], Tuple[float, float]]:
    min_disp = 0.1  # Minimum displacement to filter-out coordinate jitter. Units: m
    vehicle_pos = deque(maxlen=4)

    def func(obs: Observation) -> Tuple[float, float]:
        nonlocal min_disp, vehicle_pos

        vehicle_pos.appendleft(obs.ego_vehicle_state.position[:2])
        jerk = 0
        acc = 0
        if len(vehicle_pos) >= 3:
            disp_0 = np.linalg.norm(vehicle_pos[0] - vehicle_pos[1])
            disp_1 = np.linalg.norm(vehicle_pos[1] - vehicle_pos[2])
            speed_0 = disp_0 / dt
            speed_1 = disp_1 / dt
            if valid_0 := (disp_0 > min_disp and disp_1 > min_disp):
                acc = (speed_0 - speed_1) / dt
            if valid_0 and len(vehicle_pos) == 4:
                disp_2 = np.linalg.norm(vehicle_pos[2] - vehicle_pos[3])
                speed_2 = disp_2 / dt
                acc_1 = (speed_1 - speed_2) / dt
                if disp_2 > min_disp:
                    jerk = (acc - acc_1) / dt

        return acc, jerk

    return func