Motion Computation CTRV prediction is wrong

[MishkaMN]

Summary

During testing of VOICES with live vehicle driving before carma vehicle, carma vehicle yields based on the CTRV model of the observed vehicle. However, it was noticed that sometimes the prediction would go backwards:

Where a live vehicle is driving north on the right lane and carma is on the left driving north as well. Here, the red line indicates the predicted trajectory of vehicle which incorrectly shows south. This prediction switches back and forth a lot, which significantly decreases yielding capability of carma.

Background on the CTRV Model
The CTRV model assumes that the object moves with a constant velocity and a constant yaw rate (turn rate). The equations of motion for the object's position and orientation are derived from these assumptions.

Original (Incorrect) Equations

next_state.x = v_w * sin(wT + state.yaw) - v_w * sin_yaw + state.x;
next_state.y = -v_w * cos(wT + state.yaw) + v_w * sin_yaw + state.y;

These equations improperly handle the trigonometric relationships for the new position, resulting in incorrect predictions.

Corrected Equations

next_state.x = state.x + v_w * (sin(state.yaw + wT) - sin_yaw);
next_state.y = state.y + v_w * (cos_yaw - cos(state.yaw + wT));

Actual equation derivations:
https://www.mathworks.com/help/fusion/ug/motion-model-state-and-process-noise.html

Version

4.5.0 (Current)

Expected Behavior

See above

Actual Behavior

See above

Steps to Reproduce the Actual Behavior

it was discovered in simulation.

1. started carma in TFHRC CARLA
2. nearby spawn a live vehicle driving in CARLA
3. manually drive the vehicle and see motion prediction red lines on rviz

Related Work

No response

[MishkaMN]

Resolved with above PR