Appendix B. Wrapper MATLAB Line Code
% Clear all variables, figures, and outputs
clear all; clc; close all;
% GLOBAL VARIABLE DECLARATIONS/////////////////////////
% This section is where global variables are defined
% NONE
% END GLOBAL VARIABLES\\\\\\\\\\\\\\\\\\\\\\\\\
% FORMATTING BEHAVIOR/////////////////////////
% This section is where formatting behavior is defined
setgraphics()
% END FORMATTING BEHAVIOR\\\\\\\\\\\\\\\\\\\\\\\\\
% MAIN/////////////////////////
% This section is main function
disp ‘Main -------------------------’;
% J = [50.5 0.1 0.1; 0.1 75.2 0.1; 0.1 0.1 one-hundred.4];
% %J = [2.0 0.1 0.1; 0.1 2.0 0.1; 0.1 0.1 2.0]; %For an uglier J
% J_Inv = J\eye(length(J));
%Load Simulink files
load_system(‘AE3818_HW6_v3_simulink.slx’);
model = ‘AE3818_HW6_v3_simulink’;
%Run Simulink models:
%Set the input parameters
wait_time = 5; % Seconds
maneuver_time = 20; % Seconds
post_maneuver_time = thirty; % Seconds
orbital_Gain_Switch = 0; % 0 = Orbital disturbances off, 1 = on
aero_torque_Switch = 0; % 0 = aero torque disturbances off, 1 = on
gravity_gradient_Switch = 0; % 0 = gravity disturbances off, 1 = on
dist_correct_switch = 1; %1 = off, 2 = on
Rotation_Var = 1; %Select the DCM to Euler Angle Rotation scheme
Inv_case = 1; % Inverse calculated via pseudoinv
spud_case = 1; % 1 = SPUD off, 0 = SPUD on
case_Var = [0,0,thirty]; %The commanded Euler Angle
PID_case = 1; % Feedback controller method 1 = proportional, derivative, integral, 2 = Matlab PID
kp = one-hundred0; % Kp gain for controller
kd = 10; % Kd gain for controller
ki = 0.1; % Ki gain for controller
observer_case = 2; % 1 = observer off, 2 = observer on
kp2 = one-hundred000; % Kp gain for controller
kd2 = 500; % Kd gain for controller
ki2 = 0.1; % Ki gain for controller
% I. Scenario cases
% A. Case 1—large timestep ffd+fb
disp(‘Case 1’)
timestep = 0.01; %Seconds
set_param(model,’FixedStep’,num2str(timestep),’StopTime’,…
num2str(wait_time+maneuver_time+post_maneuver_time));
ffd_case = 1; % 1 = ffd control on, 0 = ffd control off
fb_case = 1; % 1 = fb control on, 0 = fb control off
tic;
[case1.time,~,~] = sim(model);
case1.runtime = toc; %Get timing data
%Retrieve data from Simulink
case1.q_dot = q_dot;
case1.q = q;
case1.q_norm = q_norm;
case1.DMC = DCM;
case1.H = H;
case1.w_B = w_B;
case1.Euler_Angles = Euler_Angles;
case1.Body_Angles = Body_Angles;
case1.T_cmd = T_cmd;
case1.T_act = T_act;
case1.Theta_dot = Theta_dot;
case1.cond_num = cond_num;
case1.detA = det_A;
case1.ffd = ffd;
case1.fb = fb;
case1.ffdfb = ffdfb;
case1.theta_error = theta_error;
case1.omega_error = omega_error;
case1.delta_u = delta_u;
case1.Theta_hat = Theta_hat;
% B. Case 2—small timestep ffd+fb
disp(‘Case 2’)
timestep = 0.001; %Seconds
set_param(model,’FixedStep’,num2str(timestep),’StopTime’,…
num2str(wait_time+maneuver_time+post_maneuver_time));
ffd_case = 1; % 1 = ffd control on, 0 = ffd control off
fb_case = 1; % 1 = fb control on, 0 = fb control off
tic;
[case2.time,~,~] = sim(model);
case2.runtime = toc; %Get timing data
%Retrieve data from Simulink
case2.q_dot = q_dot;
case2.q = q;
case2.q_norm = q_norm;
case2.DMC = DCM;
case2.H = H;
case2.w_B = w_B;
case2.Euler_Angles = Euler_Angles;
case2.Body_Angles = Body_Angles;
case2.T_cmd = T_cmd;
case2.T_act = T_act;
case2.Theta_dot = Theta_dot;
case2.cond_num = cond_num;
case2.detA = det_A;
case2.ffd = ffd;
case2.fb = fb;
case2.ffdfb = ffdfb;
case2.theta_error = theta_error;
case2.omega_error = omega_error;
case2.delta_u = delta_u;
case2.Theta_hat = Theta_hat;
% C. Case 3—large timestep, ffd on, fb off
disp(‘Case 3’)
timestep = 0.001; %Seconds
set_param(model,’FixedStep’,num2str(timestep),’StopTime’,…
num2str(wait_time+maneuver_time+post_maneuver_time));
ffd_case = 1; % 1 = ffd control on, 0 = ffd control off
fb_case = 0; % 1 = fb control on, 0 = fb control off
tic;
[case3.time,~,~] = sim(model);
case3.runtime = toc; %Get timing data
%Retrieve data from Simulink
case3.q_dot = q_dot;
case3.q = q;
case3.q_norm = q_norm;
case3.DMC = DCM;
case3.H = H;
case3.w_B = w_B;
case3.Euler_Angles = Euler_Angles;
case3.Body_Angles = Body_Angles;
case3.T_cmd = T_cmd;
case3.T_act = T_act;
case3.Theta_dot = Theta_dot;
case3.cond_num = cond_num;
case3.detA = det_A;
case3.ffd = ffd;
case3.fb = fb;
case3.ffdfb = ffdfb;
case3.theta_error = theta_error;
case3.omega_error = omega_error;
case3.delta_u = delta_u;
case3.Theta_hat = Theta_hat;
% D. Case 4—large timestep, ffd off, fb on
disp(‘Case 3’)
timestep = 0.001; %Seconds
set_param(model,’FixedStep’,num2str(timestep),’StopTime’,…
num2str(wait_time+maneuver_time+post_maneuver_time));
ffd_case = 0; % 1 = ffd control on, 0 = ffd control off
fb_case = 1; % 1 = fb control on, 0 = fb control off
tic;
[case4.time,~,~] = sim(model);
case4.runtime = toc; %Get timing data
%Retrieve data from Simulink
case4.q_dot = q_dot;
case4.q = q;
case4.q_norm = q_norm;
case4.DMC = DCM;
case4.H = H;
case4.w_B = w_B;
case4.Euler_Angles = Euler_Angles;
case4.Body_Angles = Body_Angles;
case4.T_cmd = T_cmd;
case4.T_act = T_act;
case4.Theta_dot = Theta_dot;
case4.cond_num = cond_num;
case4.detA = det_A;
case4.ffd = ffd;
case4.fb = fb;
case4.ffdfb = ffdfb;
case4.theta_error = theta_error;
case4.omega_error = omega_error;
case4.delta_u = delta_u;
case4.Theta_hat = Theta_hat;
% II. Analysis and Plots
% Timestep analysis plots
figure(1);
subplot(1,3,1);
plot(case1.time, case1.Euler_Angles(:,1),’-’,…
case2.time, case2.Euler_Angles(:,1),’:’);
title(‘$\phi$ Time-step Analysis ffd+fb’);
legend(‘0.01 Time-step’,’0.001 Time-step’,’location’,’best’)
subplot(1,3,2);
plot(case1.time, case1.Euler_Angles(:,2),’-’,…
case2.time, case2.Euler_Angles(:,2),’:’);
title(‘$\theta$ Time-step Analysis ffd+fb’);
legend(‘0.01 Time-step’,’0.001 Time-step’,’location’,’best’)
subplot(1,3,3);
plot(case1.time, case1.Euler_Angles(:,3),’-’,…
case2.time, case2.Euler_Angles(:,3),’:’);
title(‘$\psi$ Time-step Analysis ffd+fb’);
legend(‘0.01 Time-step’,’0.001 Time-step’,’location’,’best’)
figure(20);
subplot(2,3,1);
plot(case1.time, case1.theta_error(:,1),’-’,…
case2.time, case2.theta_error(:,1),’:’);
title(‘$\theta_x$ error to feedback controller’);
legend(‘0.01 Time-step’,’0.001 Time-step’,’location’,’best’)
subplot(2,3,2);
plot(case1.time, case1.theta_error(:,2),’-’,…
case2.time, case2.theta_error(:,2),’:’);
title(‘$\theta_y$ error to feedback controller’);
legend(‘0.01 Time-step’,’0.001 Time-step’,’location’,’best’)
subplot(2,3,3);
plot(case1.time, case1.theta_error(:,3),’-’,…
case2.time, case2.theta_error(:,3),’:’);
title(‘$\theta_z$ error to feedback controller’);
legend(‘0.01 Time-step’,’0.001 Time-step’,’location’,’best’)
subplot(2,3,4);
plot(case1.time, case1.omega_error(:,1),’-’,…
case2.time, case2.omega_error(:,1),’:’);
title(‘$\omega_x$ error to feedback controller’);
legend(‘0.01 Time-step’,’0.001 Time-step’,’location’,’best’)
subplot(2,3,5);
plot(case1.time, case1.omega_error(:,2),’-’,…
case2.time, case2.omega_error(:,2),’:’);
title(‘$\omega_y$ error to feedback controller’);
legend(‘0.01 Time-step’,’0.001 Time-step’,’location’,’best’)
subplot(2,3,6);
plot(case1.time, case1.omega_error(:,3),’-’,…
case2.time, case2.omega_error(:,3),’:’);
title(‘$\omega_z$ error to feedback controller’);
legend(‘0.01 Time-step’,’0.001 Time-step’,’location’,’best’)
% Show ffd,fb,ffd+fb control inputs on a 1x3 plot
figure(2);
subplot(1,3,1);
plot(case2.time, case2.ffd(:,1),…
case2.time, case2.ffd(:,2),…
case2.time, case2.ffd(:,3));
title(‘feedforward control’);
legend(‘$\phi$’,’$\theta$’,’$\psi$’,’location’,’best’)
subplot(1,3,2);
plot(case2.time, case2.fb(:,1),…
case2.time, case2.fb(:,2),…
case2.time, case2.fb(:,3));
title(‘feedback control’);
legend(‘$\phi$’,’$\theta$’,’$\psi$’,’location’,’best’)
subplot(1,3,3);
plot(case2.time, case2.ffdfb(:,1),…
case2.time, case2.ffdfb(:,2),…
case2.time, case2.ffdfb(:,3));
title(‘feedforward plus feedback control’);
legend(‘$\phi$’,’$\theta$’,’$\psi$’,’location’,’best’)
% Show ffd vs fb euler angle analysis
% case3 = ffd only, case4 = fb only, case 1 = ffd+fb
figure(3);
subplot(2,3,1);
plot(case3.time, case3.Euler_Angles(:,1),’:’,…
case3.time, case3.Euler_Angles(:,2),’--’,…
case3.time, case3.Euler_Angles(:,3));
title(‘feedforward control Euler Angles’);
legend(‘$\phi$’,’$\theta$’,’$\psi$’,’location’,’best’)
subplot(2,3,2);
plot(case4.time, case4.Euler_Angles(:,1),’:’,…
case4.time, case4.Euler_Angles(:,2),’--’,…
case4.time, case4.Euler_Angles(:,3));
title(‘feedback control Euler Angles’);
legend(‘$\phi$’,’$\theta$’,’$\psi$’,’location’,’best’)
subplot(2,3,3);
plot(case1.time, case1.Euler_Angles(:,1),’:’,…
case1.time, case1.Euler_Angles(:,2),’--’,…
case1.time, case1.Euler_Angles(:,3));
title(‘feedforward plus feedback control Euler Angles’);
legend(‘$\phi$’,’$\theta$’,’$\psi$’,’location’,’best’)
subplot(2,3,4);
plot(case3.time, case3.Euler_Angles(:,1),…
case3.time, case3.Euler_Angles(:,2),…
case3.time, case3.Euler_Angles(:,3));
title(‘feedforward control Euler Angles (zoomed)’);
legend(‘$\phi$’,’$\theta$’,’$\psi$’,’location’,’best’)
ylim([29.95 thirty.05])
xlim([8 15])
subplot(2,3,5);
plot(case4.time, case4.Euler_Angles(:,1),…
case4.time, case4.Euler_Angles(:,2),…
case4.time, case4.Euler_Angles(:,3));
title(‘feedback control Euler Angles (zoomed)’);
legend(‘$\phi$’,’$\theta$’,’$\psi$’,’location’,’best’)
ylim([29.95 thirty.05])
xlim([8 15])
subplot(2,3,6);
plot(case1.time, case1.Euler_Angles(:,1),…
case1.time, case1.Euler_Angles(:,2),…
case1.time, case1.Euler_Angles(:,3));
title(‘feedforward plus feedback control Euler Angles (zoomed)’);
legend(‘$\phi$’,’$\theta$’,’$\psi$’,’location’,’best’)
ylim([29.95 thirty.05])
xlim([8 15])
% Plot the calculated vs desired Euler angle error over time
figure(4);
subplot(1,4,1);
plot(case3.time, case3.Euler_Angles(:,1)-0,…
case4.time, case4.Euler_Angles(:,1)-0,’:’,…
case1.time, case1.Euler_Angles(:,1)-0);
title(‘$\phi_{act}$-$\phi_{des}$ error’);
legend(‘ffd’,’fb’,’ffd+fb’,’location’,’best’)
subplot(1,4,2);
plot(case3.time, case3.Euler_Angles(:,2)-0,…
case4.time, case4.Euler_Angles(:,2)-0,’:’,…
case1.time, case1.Euler_Angles(:,2)-0);
title(‘$\theta_{act}$-$\theta_{des}$ error’);
legend(‘ffd’,’fb’,’ffd+fb’,’location’,’best’)
subplot(1,4,3);
plot(case3.time, case3.Euler_Angles(:,3)-thirty,…
case4.time, case4.Euler_Angles(:,3)-thirty,’:’,…
case1.time, case1.Euler_Angles(:,3)-thirty);
title(‘$\psi_{act}$-$\psi_{des}$ error’);
legend(‘ffd’,’fb’,’ffd+fb’,’location’,’best’)
subplot(1,4,4);
plot(case3.time, case3.Euler_Angles(:,3)-thirty,…
case4.time, case4.Euler_Angles(:,3)-thirty,’:’,…
case1.time, case1.Euler_Angles(:,3)-thirty);
title(‘$\psi_{act}$-$\psi_{des}$ error (zoomed)’);
legend(‘ffd’,’fb’,’ffd+fb’,’location’,’best’)
ylim([-0.02 0.02])
xlim([8 15])
% Boundary condition satisfaction
feedforward_BC = [0-case3.Euler_Angles(end,1),…
0-case3.Euler_Angles(end,2),…
thirty-case3.Euler_Angles(end,3)];
feedback_BC = [0-case4.Euler_Angles(end,1),…
0-case4.Euler_Angles(end,2),…
thirty-case4.Euler_Angles(end,3)];
feedforwardfeedback_BC = [0-case1.Euler_Angles(end,1),…
0-case1.Euler_Angles(end,2),…
thirty-case1.Euler_Angles(end,3)];
fprintf(‘feedforward BC@tf: phi=%2.2e, theta=%2.2e, psi=%2.2e\n’, …
feedforward_BC(1),feedforward_BC(2),feedforward_BC(3));
fprintf(‘feedback BC@tf: phi=%2.2e, theta=%2.2e, psi=%2.2e\n’, …
feedback_BC(1),feedback_BC(2),feedback_BC(3));
fprintf(‘feedforwardfeedback BC@tf: phi=%2.2e, theta=%2.2e, psi=%2.2e\n’, …
feedforwardfeedback_BC(1),feedforwardfeedback_BC(2),feedforwardfeedback_BC(3));
% Runtime analysis
fprintf(‘Case3 ffd runtime for 0.001 timestep: %2.2e sec\n’, …
case3.runtime);
fprintf(‘Case4 fb runtime for 0.001 timestep: %2.2e sec\n’, …
case4.runtime);
fprintf(‘Case1 ffd+fb runtime for 0.001 timestep: %2.2e sec\n’, …
case1.runtime);
% delta_u analysis plots
figure(5);
subplot(1,3,1);
plot(case1.time, case1.delta_u(:,1)-0);
title(‘$\delta_{ux}$’);
legend(‘ffd+fb’,’location’,’best’)
subplot(1,3,2);
plot(case1.time, case1.delta_u(:,2)-0);
title(‘$\delta_{uy}$’);
legend(‘ffd+fb’,’location’,’best’)
subplot(1,3,3);
plot(case1.time, case1.delta_u(:,3)-thirty);
title(‘$\delta_{uz}$’);
legend(‘ffd+fb’,’location’,’best’)
% Theta_hat analysis plots
figure(6);
subplot(2,3,1);
plot(case1.time, case1.Theta_hat(:,1));
title(‘$\Delta\Theta_{xx}$’);
legend(‘ffd+fb’,’location’,’best’)
subplot(2,3,2);
plot(case1.time, case1.Theta_hat(:,2));
title(‘$\Delta\Theta_{xy}$’);
legend(‘ffd+fb’,’location’,’best’)
subplot(2,3,3);
plot(case1.time, case1.Theta_hat(:,3));
title(‘$\Delta\Theta_{xz}$’);
legend(‘ffd+fb’,’location’,’best’)
subplot(2,3,4);
plot(case1.time, case1.Theta_hat(:,4));
title(‘$\Delta\Theta_{yy}$’);
legend(‘ffd+fb’,’location’,’best’)
subplot(2,3,5);
plot(case1.time, case1.Theta_hat(:,5));
title(‘$\Delta\Theta_{yz}$’);
legend(‘ffd+fb’,’location’,’best’)
subplot(2,3,6);
plot(case1.time, case1.Theta_hat(:,6));
title(‘$\Delta\Theta_{zz}$’);
legend(‘ffd+fb’,’location’,’best’)
disp ‘EOF -------------------------’;
% END MAIN\\\\\\\\\\\\\\\\\\\\\\\\\
% FUNCTION DECLARATIONS/////////////////////////
% This section is where functions are defined
function [] = setgraphics()
% Set the graphics parameters for plotting
set(groot, ‘defaultAxesFontSize’, 18, …
‘defaultAxesLineWidth’, 0.7, …
‘defaultLineLineWidth’, 2, …
‘defaultPatchLinewidth’, 0.7, …
‘defaultTextFontSize’, 18);
set(groot, ‘defaultTextInterpreter’, …
‘latex’);
set(groot, ‘defaultAxesTickLabelInterpreter’, …
‘latex’);
set(groot, ‘defaultLegendInterpreter’, …
‘latex’);
fprintf(‘Graphics paremeters set.\n’)
end
% END FUNCTION DECLARATIONS\\\\\\\\\\\\\\\\\\\\\\\\\\
% EOF **************************************************