Generate Valid Trajectory to Complete Track

- Update input values
- Generate full set of inputs and simulate entire trajectory

sravan_part_1.m

@@ -33,121 +33,153 @@ state_0 = [x_0, u_0, y_0, v_0, psi_0, r_0];
 
 load('TestTrack.mat')
 
-%% Trajectory Synthesis: Segment 1
+%% Trajectory Synthesis
+% Segment 1
 segment_num = 1;
 num_pts(segment_num) = 6e2;
 delta_vals(segment_num) = -0.004;
 F_x_vals(segment_num) = 3900;
-[Y,T,U] = simulate_segment(segment_num, [], [], [], num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 2
+% Segment 2
 segment_num = 2;
 num_pts(segment_num) = 4e2;
 delta_vals(segment_num) = -0.3;
 F_x_vals(segment_num) = -2000;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 3
+% Segment 3
 segment_num = 3;
 num_pts(segment_num) = 1e2;
 delta_vals(segment_num) = -0.05;
 F_x_vals(segment_num) = 0;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 4
+% Segment 4
 segment_num = 4;
 num_pts(segment_num) = 7.5e2;
 delta_vals(segment_num) = 0.0;
 F_x_vals(segment_num) = 1000;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 5
+% Segment 5
 segment_num = 5;
 num_pts(segment_num) = 3e2;
 delta_vals(segment_num) = 0.3;
 F_x_vals(segment_num) = -500;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 6
+% Segment 6
 segment_num = 6;
 num_pts(segment_num) = 3.5e2;
 delta_vals(segment_num) = -0.03;
 F_x_vals(segment_num) = 1000;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 7
+% Segment 7
 segment_num = 7;
 num_pts(segment_num) = 1e2;
 delta_vals(segment_num) = -0.005;
 F_x_vals(segment_num) = -1000;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 8
+% Segment 8
 segment_num = 8;
 num_pts(segment_num) = 2e2;
-delta_vals(segment_num) = 0.025;
+delta_vals(segment_num) = 0.0275;
 F_x_vals(segment_num) = -750;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 9
+% Segment 9
 segment_num = 9;
-num_pts(segment_num) = 2.5e2;
+num_pts(segment_num) = 2.4e2;
 delta_vals(segment_num) = 0.5;
 F_x_vals(segment_num) = -500;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 10
+% Segment 10
 segment_num = 10;
 num_pts(segment_num) = 5e2;
 delta_vals(segment_num) = -0.02;
 F_x_vals(segment_num) = 0;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 11
+% Segment 11
 segment_num = 11;
 num_pts(segment_num) = 2.5e2;
 delta_vals(segment_num) = -0.05;
 F_x_vals(segment_num) = 500;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 12
+% Segment 12
 segment_num = 12;
 num_pts(segment_num) = 2e2;
 delta_vals(segment_num) = -0.01;
 F_x_vals(segment_num) = 5000;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 13
+% Segment 13
 segment_num = 13;
 num_pts(segment_num) = 2e2;
 delta_vals(segment_num) = -0.1;
 F_x_vals(segment_num) = -2000;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 14
+% Segment 14
 segment_num = 14;
 num_pts(segment_num) = 3e2;
 delta_vals(segment_num) = 0.175;
 F_x_vals(segment_num) = -2000;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 15
+% Segment 15
 segment_num = 15;
-num_pts(segment_num) = 5e2;
+num_pts(segment_num) = 4.75e2;
-delta_vals(segment_num) = -0.01;
+delta_vals(segment_num) = 0.0025;
 F_x_vals(segment_num) = 1000;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
 
-%% Trajectory Synthesis: Segment 16
+% Segment 16
 segment_num = 16;
-num_pts(segment_num) = 5e2;
+num_pts(segment_num) = 4.5e2;
 delta_vals(segment_num) = 0.05;
 F_x_vals(segment_num) = 0;
-[Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0);
+
+% Segment 17
+segment_num = 17;
+num_pts(segment_num) = 5e2;
+delta_vals(segment_num) = 0.0;
+F_x_vals(segment_num) = 500;
+
+% Segment 18
+segment_num = 18;
+num_pts(segment_num) = 8e2;
+delta_vals(segment_num) = -0.05;
+F_x_vals(segment_num) = -500;
+
+% Segment 19
+segment_num = 19;
+num_pts(segment_num) = 5.8e2;
+delta_vals(segment_num) = 0.065;
+F_x_vals(segment_num) = 0;
+
+% Segment 20
+segment_num = 20;
+num_pts(segment_num) = 7.5e2;
+delta_vals(segment_num) = 0;
+F_x_vals(segment_num) = 2000;
+
+% Segment 21
+segment_num = 21;
+num_pts(segment_num) = 2.25e2;
+delta_vals(segment_num) = 0.5;
+F_x_vals(segment_num) = -4400;
+
+% Segment 22
+segment_num = 22;
+num_pts(segment_num) = 9e2;
+delta_vals(segment_num) = 0.0;
+F_x_vals(segment_num) = 5000;
+
+%% Simulate Trajectory
+for i = 1:length(num_pts)
+    [start_idx, end_idx] = get_indices(i, num_pts);
+
+    delta = delta_vals(i);
+    F_x = F_x_vals(i);
+    U(start_idx:end_idx,:) = [delta * ones(num_pts(i),1), F_x * ones(num_pts(i),1)];
+end
+
+[Y, T] = forwardIntegrateControlInput(U, state_0);
+info = getTrajectoryInfo(Y,U)
 
 % Figures
 close all;
-clc;
 
 figure(1)
 hold on;
@@ -157,31 +189,12 @@ for i = 1:length(num_pts)
     [start_idx, end_idx] = get_indices(i, num_pts);
     plot(Y(start_idx:end_idx,1), Y(start_idx:end_idx,3), '-');
 end
+
 plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '--r');
 plot(TestTrack.br(1,:), TestTrack.br(2,:), '--r');
 plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '-.g');
 
-info = getTrajectoryInfo(Y,U)
-
 %% Functions
-function [Y,T,U] = simulate_segment(segment_num, Y, T, U, num_pts, delta_vals, F_x_vals, state_0)
-    [start_idx, end_idx] = get_indices(segment_num, num_pts);
-    
-    if segment_num ~= 1
-        state_0 = Y(start_idx-1,:);
-    end
-
-    delta = delta_vals(segment_num);
-    F_x = F_x_vals(segment_num);
-    
-    U_segment = [delta * ones(num_pts(segment_num),1), F_x * ones(num_pts(segment_num),1)];
-    [Y_segment, T_segment] = forwardIntegrateControlInput(U_segment, state_0);
-
-    Y(start_idx:end_idx,:) = Y_segment;
-    T(start_idx:end_idx)   = T_segment;
-    U(start_idx:end_idx,:) = U_segment;
-end
-
 function [start_idx, end_idx] = get_indices(segment_num, num_pts)
     if segment_num == 1
         start_idx = 1;