Minor changes. Added plots and disabling of linear motion to focus on attitude tuning.

Parameters/setUpParameters_OneQuad.m

@@ -10,7 +10,7 @@ function [params] = setUpParameters_OneQuad(traj_num)
     params.save_figs_bool = true;
     
     %% Debugging Parameters (true / false)
-    params.disable_linear_vehicle_motion  = false;
+    params.disable_linear_vehicle_motion  = true;
     params.disable_angular_vehicle_motion = false;
 
     %% Global Parameters
@@ -30,20 +30,7 @@ function [params] = setUpParameters_OneQuad(traj_num)
 %                         ones(3,1).* 0;
 %                         ones(3,1).* 0;
 %                         ones(3,1).* 0];
-                                    
-    %%
-    params.dist_F_power = 0 .* [1; 1; 1];   % 0-mean Band-limited, White Gaussian Dist (Ext Force)
-    params.dist_F_seed = [1 2 3];
-    params.dist_M_power = 0 .* [1; 1; 1];   % 0-mean Band-limited, White Gaussian Dist (Ext Force)
-    params.dist_M_seed = [4 5 6];
-    
-    params.multirotors{1}.dist_F_power = 0;
-    params.multirotors{1}.dist_M_power = 0;
-    params.multirotors{1}.noise_power = 0;
-    
-    %% Noise
-    params.noise_power = 0.1 .* ones(12,1);
-    params.noise_seed = 101:112;
+                                 
     
     %% Load Multirotors
     params.num_vehicles = 1;
@@ -85,12 +72,26 @@ function [params] = setUpParameters_OneQuad(traj_num)
 %                                 0;...      % q (rad/s)
 %                                 0];        % r (rad/s)
     
+    %% Offset in position
+%     params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.01;0.01;0.01];
+
+    %% Offset in orientation
+    params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.2;0.0;0.0];
     
     %% Choose Additional Parameters
     params.anim_FPS = 30;                % Animation FPS
-
-    %% Set Guidamce
-    params.traj_num = traj_num;
+    
+    %%
+    params.multirotors{1}.dist_F_power = 0 .* [1; 1; 1];   % 0-mean Band-limited, White Gaussian Dist (Ext Force)
+    params.multirotors{1}.dist_F_seed = [1 2 3];
+    params.multirotors{1}.dist_M_power= 0 .* [1; 1; 1];   % 0-mean Band-limited, White Gaussian Dist (Ext Force)
+    params.multirotors{1}.dist_M_seed = [4 5 6];
+    
+    %% Noise
+    params.multirotors{1}.noise_power = 0.0 .* ones(12,1);
+    params.multirotors{1}.noise_seed = 101:112;
+    
+    
     
 end
 

Plotting/helper_functions/plot_orientation.m

@@ -11,7 +11,7 @@ function [] = plot_orientation(T,X,Xd)
     xlabel('Time (s)');
     title('Attitude');
     legend('Actual','Desired');
-    ylim([-pi pi]);   
+    ylim([-pi/2 pi/2]);   
     
     subplot(3,1,2);
     plot(T,X(:,8));
@@ -20,7 +20,7 @@ function [] = plot_orientation(T,X,Xd)
     hold off;
     ylabel('\theta (rad)');
     xlabel('Time (s)');
-    ylim([-pi pi]);
+    ylim([-pi/2 pi/2]);
     legend('Actual','Desired');
 
     subplot(3,1,3);
@@ -30,7 +30,7 @@ function [] = plot_orientation(T,X,Xd)
     hold off;
     ylabel('\psi (rad)');
     xlabel('Time (s)');
-    ylim([-pi pi]);
+    ylim([-pi/2 pi/2]);
     legend('Actual','Desired');
 
 end

Plotting/helper_functions/plot_orientation_rates.m

+function [] = plot_orientation_rates(T,X,Xd)
+
+    figure();
+
+    subplot(3,1,1);
+    plot(T,X(:,10));
+    hold on;
+    plot(T,Xd(:,10),'r--');
+    hold off;
+    ylabel('p (rad/s)');
+    xlabel('Time (s)');
+    title('Attitude Rates');
+    legend('Actual','Desired');   
+    
+    subplot(3,1,2);
+    plot(T,X(:,11));
+    hold on;
+    plot(T,Xd(:,11),'r--');
+    hold off;
+    ylabel('q (rad/s)');
+    xlabel('Time (s)');
+    legend('Actual','Desired');
+
+    subplot(3,1,3);
+    plot(T,X(:,12));
+    hold on;
+    plot(T,Xd(:,12),'r--');
+    hold off;
+    ylabel('r (rad/s)');
+    xlabel('Time (s)');
+    legend('Actual','Desired');
+
+end
+

singleVehicleSim.m

@@ -8,17 +8,13 @@
 addpath(genpath(pwd));
 
 %% Set up Parameters
-traj_num = 2;
+traj_num = 1;
 params = setUpParameters_OneQuad(traj_num);
 params.num_vehicles = 1;
 
 %% Load Top Level Simulink Model (helps Simulink load ahead of time)
 open('singlequadrotor');
 
-params.multirotors{1}.dist_F_power = 0;
-params.multirotors{1}.dist_M_power = 0;
-params.multirotors{1}.noise_power = 0;
-params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.01;0.01;0.01];
 %% Call Simulink and Run the Simulation
 simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal));
 %simData = sim('singlequadrotor','StartTime','0','StopTime','10');
@@ -27,13 +23,15 @@ simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal
 %% Plot
 % TODO plot quadrotor stuff, not load stuff
 
-plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
+% plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
+% plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd);
 plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd);
-plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd);
+plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd);
+
 
 
 %% Animate
-animate_simulink_data(simData, params, false);
+% animate_simulink_data(simData, params, false);