Hi Wegge,
As per my understanding, you are trying to implement a pure pursuit controller. The goal of this controller is to steer a vehicle from its current path, which is the line y = x, to a new path, the line y = x + 5. However, the vehicle is having trouble aligning with this new path.
Assuming that the remaining code is correct, here is how the calculation of ‘alpha’ can be modified to get the desired result. Please refer to the following code snippet:
Ld = 5; % Lookahead distance
% Extract the vehicle state
x = x(i);
y = y(i);
theta = theta(i);
% For the line y = x + 5, find a point ahead at the lookahead distance
x_ahead = x + Ld / sqrt(2);
y_ahead = x_ahead + 5;
goal_point = [x_ahead, y_ahead];
% Compute the relative position of the goal point to the vehicle
dx = goal_point(1) - x;
dy = goal_point(2) - y;
% Transform to vehicle coordinates
alpha = atan2(dy, dx) - theta;
Please refer to the following documentation of ‘pure-pursuit-controller’ for more information:
Hope this helps!
