Hi Simon,
Dealing with the discontinuity in yaw angle (heading) that jumps from (\pi) to (-\pi) (or vice versa) is a common issue in vehicle simulations and robotics, particularly when using control systems like the Stanley controller in vehicle dynamics simulations. This discontinuity can confuse the controller, causing erratic behavior around the transition points.
One common approach to handle this discontinuity is to ensure that the yaw angle difference (error) passed to the controller is always within a continuous range, typically between (-\pi) and (\pi). This can be achieved by adjusting the calculation of the yaw error to account for the discontinuity.
Here's a conceptual approach to adjust the yaw error calculation:
function yaw_error = calculateYawError(target_yaw, current_yaw)
% Calculate the raw yaw error
raw_error = target_yaw - current_yaw;
% Adjust the yaw error to be within -pi to pi
yaw_error = atan2(sin(raw_error), cos(raw_error));
end
When integrating with the Stanley controller, you would use this approach to calculate the yaw error before passing it to the controller. Here's a conceptual example of how you might integrate the yaw error adjustment:
% Example values for demonstration
target_yaw = pi; % Target yaw angle
current_yaw = -pi + 0.1; % Current yaw angle close to the discontinuity
% Calculate the adjusted yaw error
yaw_error = calculateYawError(target_yaw, current_yaw);
% Now, use yaw_error with the Stanley controller or your control logic
% For example:
% control_signal = stanleyControlFunction(yaw_error, other_control_inputs...);
By carefully handling the yaw angle discontinuity and possibly adjusting the controller's parameters, you should be able to achieve smoother behavior from the vehicle simulation, even when driving in full circles or when the yaw angle transitions across the discontinuity.
Hope it helps!
