diff --git a/.gitignore b/.gitignore
index e43b0f988953ae3a84b00331d0ccf5f7d51cb3cf..737883cf6450360f29da98a44661479adfcb7b2a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,3 @@
 .DS_Store
+Figures&Animations/*
+SimData&Params/*
diff --git a/Figures/save_current_figure.m b/Figures/save_current_figure.m
index 3e1d36970ddb4bb43d909afe9f7348b30becfe05..e317d73d98d615e22c505d565b1584b9e9aced9d 100644
--- a/Figures/save_current_figure.m
+++ b/Figures/save_current_figure.m
@@ -1,14 +1,21 @@
-function [fig_num] = save_current_figure(prefix,fig_num,save_figs_bool)
+function [fig_num] = save_current_figure(prefix,fig_num, run_num, save_figs_bool)
+
 %SAVE_CURRENT_FIGURE
     if ~save_figs_bool 
         fig_num = fig_num + 1;
         return 
     else
-        figname = strcat(prefix,string(fig_num));
-        fig_num = fig_num + 1;
-        savefig(strcat('Figures/',figname,'.fig'));
-        saveas(gcf,strcat('Figures/',figname,'.png'));
-        saveas(gcf,strcat('Figures/',figname,'.pdf'));
-    end
+        figname = strcat(prefix,string(run_num));
+        %fig_num = fig_num + 1;
+        newfolder = [pwd '/' 'Figures&Animations/' 'Run' int2str(run_num)];
+        if ~exist(newfolder, 'dir')
+            mkdir(newfolder);        
+        end
+        addpath(newfolder)
+        for k = 1:6
+            savefig(figure(k), strcat('Figures&Animations/',['Run' int2str(run_num)], '/', figname,'_', int2str(k),'.fig'));
+            saveas(figure(k),strcat('Figures&Animations/',['Run' int2str(run_num)], '/', figname, '_', int2str(k), '.png'));
+            saveas(figure(k),strcat('Figures&Animations/',['Run' int2str(run_num)], '/', figname, '_', int2str(k), '.pdf'));            
+        end
 end
 
diff --git a/Figures/save_video.m b/Figures/save_video.m
new file mode 100644
index 0000000000000000000000000000000000000000..b3c067663627210fddecf114b5b3179e27e3b88d
--- /dev/null
+++ b/Figures/save_video.m
@@ -0,0 +1,150 @@
+%unnecessary function
+function [] = save_video(name, simData,params)
+     writerObj = VideoWriter(['Figures&Animations/' name '.avi']);
+     writerObj.FrameRate = 60;
+     open(writerObj);
+     
+         %% Process Input into simpler names
+    T0 = simData.T';
+    X0 = simData.X';
+    anim_FPS = params.anim_FPS;
+    
+    
+    %% Interpolate to get requested display rate
+    deltaT = 1 / anim_FPS;
+    t = T0(1) : deltaT : T0(end);
+
+    load_ind = params.num_vehicles + 1;
+    
+    % Get Data for Each Vehicle
+    for quad = 1:params.num_vehicles
+        %% Obtain X,Y,Z from state 
+        X{quad} = interp1(T0,X0(:,(quad-1)*12 + 1),t);
+        Y{quad} = interp1(T0,X0(:,(quad-1)*12 + 2),t);
+        Z{quad} = interp1(T0,X0(:,(quad-1)*12 + 3),t);
+
+        %% Obtain Roll, Pitch, Yaw from state
+        Phi{quad} =   interp1(T0,X0(:,(quad-1)*12 + 7),t);
+        Theta{quad} = interp1(T0,X0(:,(quad-1)*12 + 8),t);
+        Psi{quad} =   interp1(T0,X0(:,(quad-1)*12 + 9),t);
+        
+        %% Process mounting points
+        r_Pi_P{quad} = params.multirotors{quad}.r_Pi_P;
+    end   
+    
+    %% Obtain Load X,Y,Z from state
+    L_X = interp1(T0,X0(:,params.num_vehicles*12 + 1),t);
+    L_Y = interp1(T0,X0(:,params.num_vehicles*12 + 2),t);
+    L_Z = interp1(T0,X0(:,params.num_vehicles*12 + 3),t);
+    
+    %% Obtain Load Roll, Pitch, Yaw from state
+    Phi{load_ind} =   interp1(T0,X0(:,params.num_vehicles*12 + 7),t);
+    Theta{load_ind} = interp1(T0,X0(:,params.num_vehicles*12 + 8),t);
+    Psi{load_ind} =   interp1(T0,X0(:,params.num_vehicles*12 + 9),t);
+    
+    %% Set Graph Limits
+    cubeSideLength = 3;
+    
+    % X Graph Limits
+    xUlim = cubeSideLength;
+    xLlim = -cubeSideLength;
+
+    % Y Graph Limtis
+    yUlim = cubeSideLength;
+    yLlim = -cubeSideLength;
+    
+    % Z Graph Limits
+    zUlim = cubeSideLength;
+    zLlim = -cubeSideLength;
+%     zUlim = -1;
+%     zLlim = -2;
+
+    %% Set up the graph
+    figure(100); clf;
+    axis([xLlim xUlim yLlim yUlim zLlim zUlim]);
+    ax = gca;
+    xlabel('X');
+    ylabel('Y');
+    zlabel('Z');
+    % Flip Z Axis and Y Axis
+    set(gca, 'Zdir', 'reverse');
+    set(gca, 'Ydir', 'reverse');
+    grid on;
+    movegui(gcf,'north');
+    movegui(gcf,'offscreen');
+    
+    %% Draw Multirotor  
+    % Quadrotor in Cross Configuration
+    qw = 0.25; %Quad Width 
+    qh = 0.04;  %Quad Height  
+    quadFrontColor = [1 0 0];
+    quadBackColor = [0 0 0];
+    multiRotor_T = eye(4);
+    
+    for quad = 1 : params.num_vehicles
+        mr{quad} = drawQuad(ax,qw,qh,quadFrontColor,quadBackColor);
+        set(mr{quad},'Matrix',multiRotor_T);
+    end
+    
+    %% Draw Tether
+    for quad = 1 : params.num_vehicles
+        tether{quad} = line([X{quad}(1) L_X(1)],[Y{quad}(1) L_Y(1)],[Z{quad}(1) L_Z(1)]);
+        set(tether{quad},'LineWidth',2,'Color','g');
+    end
+    
+    
+    %% Draw Payload
+    pw = 0.05;
+    ph = 0.05;
+    payloadColor = 'r';
+    p = drawPayload(ax,pw,ph,payloadColor);
+    payload_T = eye(4);
+    set(p,'Matrix',payload_T);
+  
+    %% Change Viewpoint
+    view(-75,50);
+     
+      tic
+    for i=1:length(t)   
+        title(['t = ' num2str( t(i),'%2.1f') ' s']);     
+        
+        % Update Payload
+        payload_T = makehgtform('zrotate',Psi{load_ind}(i)) *...
+                    makehgtform('yrotate',Theta{load_ind}(i)) *...
+                    makehgtform('xrotate',Phi{load_ind}(i));
+        payload_T(1:3,4) = [L_X(i); L_Y(i); L_Z(i)];
+        set(p,'Matrix',payload_T);
+        
+        R_P_G = payload_T(1:3,1:3);
+        r_P_G = [L_X(i); L_Y(i); L_Z(i)];
+        
+        for quad = 1 : params.num_vehicles
+            % Update Multirotor
+            multiRotor_T = makehgtform('zrotate',Psi{quad}(i)) *...
+                           makehgtform('yrotate',Theta{quad}(i)) *...
+                           makehgtform('xrotate',Phi{quad}(i));
+            multiRotor_T(1:3,4) = [X{quad}(i); Y{quad}(i); Z{quad}(i)];
+            set(mr{quad},'Matrix',multiRotor_T);
+
+            % Update Tether
+            r_Pi_G = R_P_G * r_Pi_P{quad} + r_P_G;
+            set(tether{quad},'XData',[X{quad}(i) r_Pi_G(1)],'YData',[Y{quad}(i) r_Pi_G(2)],'ZData',[Z{quad}(i) r_Pi_G(3)]);
+        end
+        
+        %Draws a point tracing the payload's path
+        plot3(L_X(i), L_Y(i), L_Z(i), 'c.')
+        
+        % Display the Animation and sleep to maintain the display FPS
+        drawnow;
+        sleepTime = deltaT - toc;
+        if (sleepTime > 0)
+            java.lang.Thread.sleep((sleepTime)*1000);
+        end
+        
+        tic;
+        frame = getframe;
+        writeVideo(writerObj,frame);
+    end
+
+     close(writerObj);
+end
\ No newline at end of file
diff --git a/GenerateSimData.asv b/GenerateSimData.asv
new file mode 100644
index 0000000000000000000000000000000000000000..d815bc5e4e37146fabec2b8adf226ffec4762508
--- /dev/null
+++ b/GenerateSimData.asv
@@ -0,0 +1,46 @@
+%% Add Necessary Folders to MATLAB Path (add everything)
+addpath(genpath(pwd))
+
+
+%% Loop Over Every Parameter
+
+
+
+for num = 1:3
+    coopPayloadSim
+end
+
+
+
+%% Load Data (if user requested)
+answer = input('Would you like to load from a matlab file? (Y/N) ', 's');
+if strcmp(answer, 'Y') == 1
+    file = input('Enter file name: ', 's');
+    load(file);
+    params.curr_run = int8(str2double(extractBetween(file, 4, 4)));
+
+%% Set Up Parameters
+else
+    params = setUpParameters();
+end
+
+%% Simulate
+simData = runSimulation(params);
+
+%% Save Params & simData
+if strcmp(answer, 'Y') ~= 1
+    saveData(simData,params, 'run');
+end
+%% Plot
+plotData(simData,params);
+if strcmp(answer, 'Y') == 1
+    save_current_figure("WPs_set", 1, params.curr_run, 1);
+end
+
+%% Animate
+if strcmp(answer, 'Y') == 1
+     %save_video(['Run_' int2str(params.curr_run)], simData,params);
+     animate(['Run_' int2str(params.curr_run)], simData,params);
+else
+    animate('none', simData,params);
+end
diff --git a/GenerateSimData.m b/GenerateSimData.m
new file mode 100644
index 0000000000000000000000000000000000000000..94dee718f1b3f729c785dd041302a195c6059289
--- /dev/null
+++ b/GenerateSimData.m
@@ -0,0 +1,37 @@
+%% GenerateSimData
+    % Generates simData and params by doing a parameter sweep over
+    % parameters of interest
+
+%% Add Necessary Folders to MATLAB Path (add everything)
+addpath(genpath(pwd))
+
+%% 1) Loop Over Every Parameter & Generate simData
+trajectories = {1, 2, 3, 10, 11, 16, 19, 20, 21, 22};
+payload_masses = {0.1, 0.5}; 
+wind_speed = {0 , 5}; 
+run_counter = 1;
+
+for traj = trajectories
+    %% Set up parameters
+    params = setUpParameters(traj{1});
+    
+    for payload_mass = payload_masses
+        %% Change parameter
+        params.payload.m = payload_mass{1};
+        for wind = wind_speed
+            %% Change parameter
+            params.wind.enable = 1;
+            params.wind.speed = wind{1};
+            %% Simulate
+            simData = runSimulation(params);
+
+            %% Save Data to file
+            saveData(simData,params, 'run');
+
+            %% Increment Run Counter
+            run_counter = run_counter + 1;
+        end
+    end
+end
+
+%PlotSimData(1, run_counter);
\ No newline at end of file
diff --git a/Guidance/WPs_set21_simple_flight_traj.m b/Guidance/WPs_set21_simple_flight_traj.m
new file mode 100644
index 0000000000000000000000000000000000000000..6c3a47ccf1dd909f7d12c7b861e7473c6cbce109
--- /dev/null
+++ b/Guidance/WPs_set21_simple_flight_traj.m
@@ -0,0 +1,15 @@
+function [coarse_WPs] = WPs_set21_simple_flight_traj()  
+%WPs_set21
+    coarse_WPs = [-2, 0, 0, 0;
+                  -2, 0, -2, 3;
+                  0, 0, -2, 6;
+                  2, 0, -2, 9;
+                  2, 0, 0, 12];
+    
+end
+
+
+
+
+
+
diff --git a/Guidance/WPs_set22_pyramid_traj.m b/Guidance/WPs_set22_pyramid_traj.m
new file mode 100644
index 0000000000000000000000000000000000000000..2214d0042fd321b3efa66f666ec19ae8ed296209
--- /dev/null
+++ b/Guidance/WPs_set22_pyramid_traj.m
@@ -0,0 +1,18 @@
+function [coarse_WPs] = WPs_set22_pyramid_traj()  
+%WPs_set22
+    coarse_WPs = [1, 0, 0, 0;
+                  -1, 0, 0, 3;
+                  0, 2, 0, 6;
+                  1, 0, 0, 9;
+                  1, 1, 2, 12;
+                  -1, 0, 0, 15;
+                  0, 2, 0, 18;
+                  1, 1, 2, 21];
+    
+end
+
+
+
+
+
+
diff --git a/Guidance/WPs_set9_helix_traj.m b/Guidance/WPs_set9_helix_traj.m
index c64b8610de5f8fd6de7d7f915db570041ca58baf..536ceec46d58bd5a9dce3ee94d9a12e227d00b6e 100644
--- a/Guidance/WPs_set9_helix_traj.m
+++ b/Guidance/WPs_set9_helix_traj.m
@@ -6,7 +6,7 @@ function [coarse_WPs] = WPs_set9_helix_traj()
     for time = 0:.5:24
         x_pos = r*cos(time);
         y_pos = r*sin(time);
-        z_pos = c*time - 3;
+        z_pos = -(c*time - 3);
         B = [x_pos, y_pos, z_pos, time];
         coarse_WPs = [coarse_WPs; B];
     end
diff --git a/Guidance/gen_payload_traj.m b/Guidance/gen_payload_traj.m
index 113500e1874d6289699ed0f4a2b02d59bbf3dc55..43b5ea0993f91f3bb86ffe9fb7ba4259f572698e 100644
--- a/Guidance/gen_payload_traj.m
+++ b/Guidance/gen_payload_traj.m
@@ -2,28 +2,54 @@ function [Xd, T] = gen_payload_traj(params)
 %gen_payload_traj
     
     %% 1) Design Coarse Waypoints [x, y, z, t] in meters and sec
-%    coarse_WPs = WPs_set1_hover();  
-%    coarse_WPs = WPs_set2_square();
-%    coarse_WPs = WPs_set3_rise_and_fall();
-%    coarse_WPs = WPs_set4_rectangle();
-%    coarse_WPs = WPs_set5_linear_function();
-%    coarse_WPs = WPs_set6_quadratic_function();
-%    coarse_WPs = WPs_set7_sine_traj();
-%    coarse_WPs = WPs_set8_circle_traj();
-%    coarse_WPs = WPs_set8b_vertCircle_traj();
-%    coarse_WPs = WPs_set9_helix_traj();
-%    coarse_WPs = WPs_set10_cardioid_traj();
-%    coarse_WPs = WPs_set11_3dlimacon_traj();
-%    coarse_WPs = WPs_set12_circles_traj();
-%    coarse_WPs = WPs_set13_ellipse_traj();
-%    coarse_WPs = WPs_set14_cycloid_traj();
-%    coarse_WPs = WPs_set15_staircase_traj();
-%    coarse_WPs = WPs_set16_triangle_traj();
-%    coarse_WPs = WPs_set17_hourglass_traj();
-%    coarse_WPs = WPs_set18_star_traj();
-%    coarse_WPs = WPs_set19_spiral_traj();
-    coarse_WPs = WPs_set20_lissajous_traj();
-    
+    switch params.traj_num
+        case 1
+            coarse_WPs = WPs_set1_hover();  
+        case 2
+            coarse_WPs = WPs_set2_square();
+        case 3
+            coarse_WPs = WPs_set3_rise_and_fall();
+        case 4
+            coarse_WPs = WPs_set4_rectangle();
+        case 5
+            coarse_WPs = WPs_set5_linear_function();
+        case 6
+            coarse_WPs = WPs_set6_quadratic_function();
+        case 7
+            coarse_WPs = WPs_set7_sine_traj();
+        case 8
+            coarse_WPs = WPs_set8_circle_traj();
+        case 9
+            coarse_WPs = WPs_set8b_vertCircle_traj();
+        case 10
+            coarse_WPs = WPs_set9_helix_traj();
+        case 11
+            coarse_WPs = WPs_set10_cardioid_traj();
+        case 12
+            coarse_WPs = WPs_set11_3dlimacon_traj();
+        case 13
+            coarse_WPs = WPs_set12_circles_traj();
+        case 14
+            coarse_WPs = WPs_set13_ellipse_traj();
+        case 15
+            coarse_WPs = WPs_set14_cycloid_traj();
+        case 16
+            coarse_WPs = WPs_set15_staircase_traj();
+        case 17
+            coarse_WPs = WPs_set16_triangle_traj();
+        case 18
+            coarse_WPs = WPs_set17_hourglass_traj();
+        case 19
+            coarse_WPs = WPs_set18_star_traj();
+        case 20
+            coarse_WPs = WPs_set19_spiral_traj();
+        case 21
+            coarse_WPs = WPs_set20_lissajous_traj();
+        case 22
+            coarse_WPs = WPs_set21_simple_flight_traj();
+        case 23
+            coarse_WPs = WPs_set22_pyramid_traj(); 
+    end
 
     %% 2) Extract Relevant Params
     dt = params.multirotors{1}.ctlDt;
diff --git a/Parameters/setUpParameters.m b/Parameters/setUpParameters.m
index f552a542758bd94a8c1090504c1fa40f479a702e..369fe81c0eb5e78726dad396a5a742bef4910108 100644
--- a/Parameters/setUpParameters.m
+++ b/Parameters/setUpParameters.m
@@ -1,4 +1,4 @@
-function [params] = setUpParameters()
+function [params] = setUpParameters(traj_num)
 %setUpParameters
     % Calls parameters_Quad() and parameters_Payload() with default
     % settings and then adjusts from there. All edits should be made to
@@ -12,6 +12,7 @@ function [params] = setUpParameters()
     
     %% Global Parameters
     params.g = 9.81;
+    params.curr_run = 1;
     
     %% Time Step Parameters
     params.ctlDt = 0.01;                      % Controller Step Size (s)
@@ -31,7 +32,8 @@ function [params] = setUpParameters()
     %% Load Payload Parameters
     params.payload = parameters_Payload(params);
     params.payload.distSTD = load_dist_STD;
-    params.payload.m = 0.5;
+    %params.payload.m = 0.5;
+    params.payload.m = 0.1;
     r = params.payload.diameter / 2;
 
     %% Adjust Payload Controller Parameters
@@ -87,6 +89,7 @@ function [params] = setUpParameters()
     params.anim_FPS = 30;                % Animation FPS
 
     %% Set Guidamce
+    params.traj_num = traj_num;
     [params.payload.xd, params.tFinal] = gen_payload_traj(params);
     params.payload.x0 = params.payload.xd(:,1);
     params = compute_quad_guidance_2(params);
diff --git a/PlotSimData.m b/PlotSimData.m
new file mode 100644
index 0000000000000000000000000000000000000000..43a2b9137698cde0d5f8ac2c0bf0a99e3c41fbd2
--- /dev/null
+++ b/PlotSimData.m
@@ -0,0 +1,41 @@
+%% PlotSimData
+    % Plots simData and params and saves them 
+
+function [] = PlotSimData(end_file_num, optionalVarChangeDefaultParam)
+    %% Determine if there is an optional argument
+    start_file_num = 1;
+
+    if nargin > 1
+      start_file_num = optionalVarChangeDefaultParam;
+    end
+
+    %% 1) Loop Over Every Simulation data file 
+    for file = start_file_num:end_file_num
+        %% Set up file info
+        filename = ['run' int2str(file) '.mat'];
+        load(filename);
+        params.curr_run = file;
+        
+        %% Display current round
+        fprintf(['Run' int2str(file) '\n']);
+
+        %% Plot
+        plotData(simData,params);
+        save_current_figure("WPs_run", 1, params.curr_run, 1);
+        
+        %% Transition to animation
+        pause(10);
+        %close all;
+        close(figure(1));
+        close(figure(2));
+        close(figure(3));
+        close(figure(4));
+        close(figure(5));
+
+        %% Animate
+        animate(['Run_' int2str(params.curr_run)], simData,params);
+        
+        pause(10);
+        close all
+    end
+end
\ No newline at end of file
diff --git a/Plotting/animate.m b/Plotting/animate.m
index 4abb81e145050e9e5f95c03f25d5ed5be2f87dd3..acebed1da8b5bfc1702c51da4f939e6338aecb2e 100644
--- a/Plotting/animate.m
+++ b/Plotting/animate.m
@@ -1,4 +1,4 @@
-function [] = animate(simData,params)
+function [] = animate(name, simData,params)
 %animate Animates the multirotor
     % Inputs:
     %           simData.T0: time vector (Nx1)
@@ -12,6 +12,13 @@ function [] = animate(simData,params)
     X0 = simData.X';
     anim_FPS = params.anim_FPS;
     
+    %% Set up video recorder
+    if strcmp(name, 'none') ~= 1 
+        writerObj = VideoWriter(['Figures&Animations/' name '.avi']);
+        writerObj.FrameRate = 20;
+        open(writerObj);
+    end
+    
     
     %% Interpolate to get requested display rate
     deltaT = 1 / anim_FPS;
@@ -145,6 +152,88 @@ function [] = animate(simData,params)
         end
         
         tic;
+        %recording
+        if strcmp(name, 'none') ~= 1 
+            frame = getframe;
+            writeVideo(writerObj,frame);
+        end
     end
+    
+    %close video object
+    if strcmp(name, 'none') ~= 1 
+        close(writerObj);
+    end
+     %% Set Up limits
+    cubeSideLength = 3;
+    
+    % X Graph Limits
+    xUlim = cubeSideLength;
+    xLlim = -cubeSideLength;
+
+    % Y Graph Limtis
+    yUlim = cubeSideLength;
+    yLlim = -cubeSideLength;
+    
+    % Z Graph Limits
+    zUlim = cubeSideLength;
+    zLlim = -cubeSideLength;
+    
+     %% Plot X, Y
+    figure(7);
+    
+    plot(simData.X(params.num_vehicles*12 + 1,:), simData.X(params.num_vehicles*12 + 2,:));
+    
+    axis([xLlim xUlim yLlim yUlim]);
+    ax = gca;
+    
+    xlabel('x (m)');
+    ylabel('y (m)');
+    title('Load Position (in terms of X, Y)');
+    set(gca, 'Ydir', 'reverse');
+    grid on;
+
+    %fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
+    
+    movegui(gcf,'southwest');
+    movegui(gcf,'onscreen');
+    
+    %% Plot Y,Z
+    figure(8);
+    
+    plot(simData.X(params.num_vehicles*12 + 2,:), simData.X(params.num_vehicles*12 + 3,:));
+    
+    axis([yLlim yUlim zLlim zUlim]);
+    ax = gca;
+    
+    xlabel('y (m)');
+    ylabel('z (m)');
+    title('Load Position (in terms of Y, Z)');
+    set(gca, 'Xdir', 'reverse');
+    set(gca, 'Ydir', 'reverse');
+    grid on;
+
+    %fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
+    
+    movegui(gcf,'southeast');
+    movegui(gcf,'onscreen');
+    
+    %% Plot X,Z
+    figure(9);
+    
+    plot(simData.X(params.num_vehicles*12 + 1,:), simData.X(params.num_vehicles*12 + 3,:));
+    
+    axis([xLlim xUlim zLlim zUlim]);
+    ax = gca;
+    
+    xlabel('x (m)');
+    ylabel('z (m)');
+    title('Load Position (in terms of X, Z)');
+    set(gca, 'Ydir', 'reverse');
+    grid on;
+
+    %fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
+    
+    movegui(gcf,'northwest');
+    movegui(gcf,'onscreen');
 end
 
diff --git a/Plotting/plotData_payload.m b/Plotting/plotData_payload.m
index f0f92301585f45029c9a266827753099051531e1..847cdb712b87c46205b5efd10609b3cf1902560f 100644
--- a/Plotting/plotData_payload.m
+++ b/Plotting/plotData_payload.m
@@ -39,7 +39,7 @@ function [fig_num] = plotData_payload(simData,params)
 %     ylim([min(simData.X(:,params.num_vehicles*12 + 3))-eps max(simData.X(:,params.num_vehicles*12 + 3))+eps]); 
    
 
-    fig_num = save_current_figure(fig_prefix, fig_num, save_figs_bool);
+    fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
     
     movegui(gcf,'northwest');
     movegui(gcf,'onscreen');
@@ -77,7 +77,7 @@ function [fig_num] = plotData_payload(simData,params)
 %     minYLims(ylim,-1,1);
    
 
-    fig_num = save_current_figure(fig_prefix, fig_num, save_figs_bool);
+    fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
     
     movegui(gcf,'north');
     movegui(gcf,'onscreen');
@@ -117,7 +117,7 @@ function [fig_num] = plotData_payload(simData,params)
     ylabel('\psi (rad)');
 %     ylim([-pi pi]);
 
-    fig_num = save_current_figure(fig_prefix, fig_num, save_figs_bool);
+    fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
     
 %     movegui(gcf,[1,30]);
     movegui(gcf,'southwest');
@@ -138,7 +138,7 @@ function [fig_num] = plotData_payload(simData,params)
     title('Payload Net Force');
     legend('F_x','F_y','F_z');
     
-    fig_num = save_current_figure(fig_prefix, fig_num, save_figs_bool);
+    fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
     
     movegui(gcf,'northeast');
     movegui(gcf,'onscreen');
@@ -158,10 +158,50 @@ function [fig_num] = plotData_payload(simData,params)
     title('Payload Net Torque');
     legend('\tau_x','\tau_y','\tau_z');
     
-    fig_num = save_current_figure(fig_prefix, fig_num, save_figs_bool);
+    fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
     
     movegui(gcf,'southeast');
     movegui(gcf,'onscreen');
     
+    %% Plot X, Y, Z
+    cubeSideLength = 3;
+    
+    % X Graph Limits
+    xUlim = cubeSideLength;
+    xLlim = -cubeSideLength;
+
+    % Y Graph Limtis
+    yUlim = cubeSideLength;
+    yLlim = -cubeSideLength;
+    
+    % Z Graph Limits
+    zUlim = cubeSideLength;
+    zLlim = -cubeSideLength;
+    
+    figure(fig_num);
+    
+    plot3(simData.X(params.num_vehicles*12 + 1,:), simData.X(params.num_vehicles*12 + 2,:), simData.X(params.num_vehicles*12 + 3,:));
+    hold on;
+    plot3(simData.X_des(params.num_vehicles*12 + 1,:), simData.X_des(params.num_vehicles*12 + 2,:), simData.X_des(params.num_vehicles*12 + 3,:), 'r--');
+    hold off;
+    
+    axis([xLlim xUlim yLlim yUlim zLlim zUlim]);
+    ax = gca;
+    
+    xlabel('x (m)');
+    ylabel('y (m)');
+    zlabel('z (m)');
+    legend('Actual','Desired');
+    title('Load Position (in terms of X, Y, Z)');
+    set(gca, 'Zdir', 'reverse');
+    set(gca, 'Ydir', 'reverse');
+    grid on;
+
+    fig_num = save_current_figure(fig_prefix, fig_num, params.curr_run, save_figs_bool);
+    
+    movegui(gcf,'south');
+    movegui(gcf,'onscreen');
+    
+   
 end
 
diff --git a/Plotting/saveData.m b/Plotting/saveData.m
index 434b9b9bff1429ed65f8ccbda9522ad952b4144a..0df54a9a2b031bf10771b0ed4fba55080c0c10bb 100644
--- a/Plotting/saveData.m
+++ b/Plotting/saveData.m
@@ -1,15 +1,17 @@
-function [] = saveData(simData,params)
+function [] = saveData(simData,params, tag)
 %saveData Saves simData and params into new file by checking existence
 
+    savdir = 'SimData&Params'; 
+    
     %% Find First New Filename
-    filename = 'data_0.mat';
-    count = 0;
-    while (isfile(filename))
-        filename = ['data_' num2str(count) '.mat'];
+    count = 1;
+    filename = [tag num2str(count) '.mat'];
+    while (isfile(fullfile(savdir, filename)))
         count = count + 1;
+        filename = [tag num2str(count) '.mat'];
     end
 
     %% Save Data to this file
-    save(filename,'simData','params');
+    save(fullfile(savdir, filename),'simData','params');
 end
 
diff --git a/coopPayloadSim.m b/coopPayloadSim.m
index 476e9a34f21e28fea9738f56c295e19bd53ab7e4..149817e5a7a0f56bb0b136793ef01b0d81275869 100644
--- a/coopPayloadSim.m
+++ b/coopPayloadSim.m
@@ -7,17 +7,85 @@
 % the parameters files (called by setUpParameters() ).
 
 %% Add Necessary Folders to MATLAB Path (add everything)
-addpath(genpath(pwd))
+addpath(genpath(pwd));
 
-%% Set Up Parameters
-params = setUpParameters();
+%% Get user input
+answer = input('Would you like to load from a matlab file? (Y/N) ', 's');
 
-%% Simulate
-simData = runSimulation(params);
+%% Create data
+if strcmp(answer, 'N') == 1
+    %% Set Up Parameters
+    fprintf('Select desired trajectory: \n')
+    menu = {'(1) Hover             ';
+            '(2) Square            ';
+            '(3) Rise and fall     ';
+            '(4) Rectangle         ';
+            '(5) Linear Function   ';
+            '(6) Quadratic Function';
+            '(7) Sine Curve        ';
+            '(8) Circle            ';
+            '(9) Vertical Circle   ';
+            '(10) Helix            ';
+            '(11) Cardioid         ';
+            '(12) 3D Limaçon       ';
+            '(13) Circles          ';
+            '(14) Ellipse          ';
+            '(15) Cycloid          ';
+            '(16) Staircase        ';
+            '(17) Triangle         ';
+            '(18) Hourglass        ';
+            '(19) Star             ';
+            '(20) Spiral           ';
+            '(21) Lissajous        ';
+            '(22) Simple Flight    ';
+            '(23) Pyramid          '
+            };
+    for index = 1:length(menu) 
+        fprintf(1, '%s\n', menu{index});
+    end
+    traj = input('Enter trajectory number: ', 's');
+    
+    params = setUpParameters(str2num(traj));
+    
+    %% Simulate
+    simData = runSimulation(params);
+    
+    %% Save Params & simData
+    saveData(simData,params, 'run');
 
-%% Plot
-plotData(simData,params);
+%% Load Data
+elseif strcmp(answer, 'Y') == 1
+    file = input('Enter file name: ', 's');
+    load(file);
+    params.curr_run = int8(str2double(extractBetween(file, (strfind(file, 'n')+1), (strfind(file, '.')-1))));
+    
+    %% Plot data
+    plotData(simData,params);
 
-%% Animate
-animate(simData,params);
+    %% Save current figures
+    save_current_figure("WPs_run", 1, params.curr_run, 1);
+
+     %% Transition to animation
+    pause(10);
+    %close all;
+    close(figure(1));
+    close(figure(2));
+    close(figure(3));
+    close(figure(4));
+    close(figure(5));
+
+    %% Animate
+    if strcmp(answer, 'Y') == 1
+        %save_video(['Run_' int2str(params.curr_run)], simData,params);
+        animate(['Run_' int2str(params.curr_run)], simData,params);
+    else
+        animate('none', simData,params);
+    end
+    
+%% Display Error Message  
+else
+    msg = 'Error. Invalid input.';
+    error(msg);
+end
+