Created
June 13, 2025 06:13
-
-
Save rahulbhadani/c376404baa35c9e37f22dd7eceeb9d7f to your computer and use it in GitHub Desktop.
follow_the_leader_model
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| % Rahul Bhadani | |
| % Two car systems: Follow-the-leader Model | |
| % \xdot_i = v_i | |
| % \vdot_i = beta\cfrac{\Delta v}{s_i^2} | |
| % s_i = x_{i-1} - x_i - L | |
| % \Delta v_i = v_i - v_{i-1} | |
| function follow_the_leader_model() | |
| beta = 51.1; | |
| L = 4.0; | |
| try | |
| data = readtable("speed.txt"); | |
| t_leader = data.Time; | |
| v_leader = data.speed; | |
| catch | |
| t_leader = (0:0.1:50.0)'; | |
| v_leader = 20*(1-exp(-t_leader/5)).*(1 + 0.2*sin(0.2*t_leader)); | |
| end | |
| f = figure; | |
| plot(t_leader, v_leader, 'LineWidth',2, 'Color','#254422'); | |
| xlabel('Time [s]', 'Interpreter','latex', 'FontSize',14); | |
| xlabel('Speed [m/s]', 'Interpreter','latex', 'FontSize',14) | |
| grid on; | |
| title('Leader Vehicle Speed Profile', 'Interpreter','latex', 'FontSize',16); | |
| %% | |
| % Compute leader position | |
| x_leader = zeros(size(t_leader)); | |
| % Use trapezoidal integration | |
| for i = 2: length(t_leader) | |
| dt = t_leader(i) - t_leader(i-1); | |
| avg_speed = ( v_leader(i-1) + v_leader(i))/2; | |
| x_leader(i) = x_leader(i-1) + avg_speed*dt; | |
| end | |
| x_follower0 = -10; % Initial position of the follower | |
| v_follower0 = 2.0; % Initial speed of follower | |
| % Initial condition | |
| y0 = [x_follower0, v_follower0]; | |
| t_span = [ min(t_leader), max(t_leader)]; | |
| % Define ODE with nested function for interpolation | |
| [t_sol, y_sol] = ode45(@follower_ode, t_span, y0); | |
| % Extract the solution | |
| x2_sol = y_sol(:, 1); | |
| v2_sol = y_sol(:, 2); | |
| plot(t_leader, v_leader, 'LineWidth',2, 'Color','#254422'); | |
| hold on; | |
| plot(t_sol, v2_sol, 'LineWidth', 2, 'Color', '#34eb77'); | |
| xlabel('Time [s]', 'Interpreter', 'latex', 'FontSize', 14); | |
| ylabel('Velocity [m/s]', 'Interpreter', 'latex', 'FontSize', 14); | |
| title('Velocity of Leader and Follower', 'Interpreter', 'latex', 'FontSize', 16); | |
| legend('Leader', 'Follower', 'Interpreter', 'latex', 'FontSize', 12); | |
| grid on; | |
| function dydt = follower_ode(t, y) | |
| % Interpolate leader data at time t | |
| v1 = interp1(t_leader, v_leader, t, 'pchip'); | |
| x1 = interp1(t_leader, x_leader, t, 'pchip'); | |
| % Follower states | |
| x2 = y(1); | |
| v2 = y(2); | |
| gap = (x1 -x2 - L); | |
| min_gap = 0.1; | |
| if gap <= 0.0 | |
| gap = min_gap; | |
| warning('Gap <= 0 at t = %.2f. Using min_gap.', t); | |
| end | |
| % differential equation | |
| dx2dt = v2; | |
| dv2dt = beta*(v2-v1)/(gap^2); | |
| dydt = [dx2dt; dv2dt]; | |
| end | |
| end |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment