Space Vehicle Control matlab

aerospace engineering question and need support to help me learn.

see the upload file
Requirements:
% —————————————–% ANIMATE BOX FUNCTION% —————————————–% Code is distributed under The MIT License (MIT)% Copyright %% Permission is hereby granted, free of charge, to any person obtaining a copy% of this software and associated documentation files (the “Software”), to deal% in the Software without restriction, including without limitation the rights% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell% copies of the Software, and to permit persons to whom the Software is% furnished to do so, subject to the following conditions:%% The above copyright notice and this permission notice shall be included in all% copies or substantial portions of the Software.%% THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE% SOFTWARE.function [save_H_b,save_H_i] = box_animate_4(q,omega_b,psi,t);global MOI_b Inv_MOI_b M_b_ext D_rotor q_commanded M_tilde_inv K_P K_D show_datafiguref = clf; % clear figureset(f,’Color’,[1 1 1]); % create white background% v = VideoWriter(strcat(‘HW3_P3.avi’));% open(v);for t_ctr = 1 : length(t)clf; hold on[a_0,phi_0,R_bi,q_0] = Q2R(q(t_ctr,:)’);T = 0.5*dot(omega_b(t_ctr,:)’,MOI_b*omega_b(t_ctr,:)’); % needs to be modifiedH_b = MOI_b*omega_b(t_ctr,:)’ + D_rotor*psi(t_ctr,:)’; % compute angularmomentum for this time stepsave_H_b(t_ctr,:) = H_b’;
H_i = R_bi’*H_b; % compute angular momentum in inertial framesave_H_i(t_ctr,:) = H_i’;M_i_ext = R_bi’*M_b_ext;omega_i = R_bi’*omega_b(t_ctr,:)’; % compute angular velocity in inertial frameH_b_hat = H_b/norm(H_b); % compute angular momentum direction for this timestepomega_b_hat = omega_b(t_ctr,:)’/norm(omega_b(t_ctr,:)); % compute angularvelocity direction for this time stepi_2_vector = one_vector(0,-1,0,0,-3,0,0.01,’k’);text(‘Interpreter’,’latex’,’String’,’$-\hat{\iota}_2$’,’Position’,[0,-3.3,0],’FontSize’,14);q_error = M_tilde_inv*[q(t_ctr,:)’];[a_error,phi_error,R_error,q_error] = Q2R(q_error);psi_dot = K_P*q_error(1:3) + K_D*omega_b(t_ctr,:);text_string=strcat(‘Time = ‘,sprintf(‘ %5.1f’, t(t_ctr)),’ s’); % Display thetime on the screen
annotation(‘textbox’,[0.4 0.1 0.80.0001],’FontSize’,14,’FontWeight’,’bold’,’LineStyle’,’none’,’String’,text_string);axis off % turn off axes for a cleaner backgroundbox_satellite([a_0′ phi_0]);text_start_x =0.78; % text starting x locationtext_start_y = 0.05; % text starting y locationline_spacing = 0.04;box_spacing = 0.2;text_string=strcat(‘Pointing Error’);annotation(‘textbox’,[text_start_x-.03 text_start_y+2*line_spacingtext_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘\Phi_e = ‘,sprintf(‘ %8.4f deg’,rad2deg(phi_error)));annotation(‘textbox’,[text_start_x-.03 text_start_y+1*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘\Phi_e = ‘,sprintf(‘ %8.0f arc-sec’,rad2deg(phi_error)*3600));
annotation(‘textbox’,[text_start_x-.03 text_start_y+0*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);if show_data || t_ctr == length(t)text_string=strcat(‘RW Speeds [RPM]’);annotation(‘textbox’,[text_start_x text_start_y+12*line_spacingtext_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘\psi_1 = ‘,sprintf(‘%7.0f’,psi(t_ctr,1)*60/2/3.14));annotation(‘textbox’,[text_start_x text_start_y+11*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘\psi_2 = ‘,sprintf(‘%7.0f’,psi(t_ctr,2)*60/2/3.14));annotation(‘textbox’,[text_start_x text_start_y+10*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘\psi_3 = ‘,sprintf(‘%7.0f’,psi(t_ctr,3)*60/2/3.14));annotation(‘textbox’,[text_start_x text_start_y+9*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘RW Torque [Nm]’);annotation(‘textbox’,[text_start_x text_start_y+7*line_spacingtext_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘D*d/dt(\psi_1)=’,sprintf(‘ %5.3f’,D_rotor*psi_dot(1)));annotation(‘textbox’,[text_start_x text_start_y+6*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);
text_string=strcat(‘D*d/dt(\psi_2)=’,sprintf(‘ %5.3f’,D_rotor*psi_dot(2)));annotation(‘textbox’,[text_start_x text_start_y+5*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘D*d/dt(\psi_3)=’,sprintf(‘ %5.3f’,D_rotor*psi_dot(3)));annotation(‘textbox’,[text_start_x text_start_y+4*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x =0.01; % text starting x locationtext_start_y = 0.5; % text starting y locationline_spacing = 0.04;box_spacing = 0.4;text_string=strcat(‘Initial Condtions’);annotation(‘textbox’,[text_start_x text_start_y text_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘\omega_1_0 = ‘,sprintf(‘ %4.1f deg/s’,rad2deg(omega_b(1,1)) ));annotation(‘textbox’,[text_start_x text_start_y-1*line_spacing
text_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘\omega_2_0 = ‘,sprintf(‘ %4.1f deg/s’,rad2deg(omega_b(1,2))));annotation(‘textbox’,[text_start_x text_start_y-2*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘\omega_3_0 = ‘,sprintf(‘ %4.1f deg/s’,rad2deg(omega_b(1,3))));annotation(‘textbox’,[text_start_x text_start_y-3*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘q_1_0 = ‘,sprintf(‘ %4.3f ‘,q(1,1)));annotation(‘textbox’,[text_start_x text_start_y-4*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘q_2_0 = ‘,sprintf(‘ %4.3f ‘,q(1,2)));annotation(‘textbox’,[text_start_x text_start_y-5*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘q_3_0 = ‘,sprintf(‘ %4.3f ‘,q(1,3)));annotation(‘textbox’,[text_start_x text_start_y-6*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘q_4_0 = ‘,sprintf(‘ %4.3f ‘,q(1,4)));annotation(‘textbox’,[text_start_x text_start_y-7*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x =0.37;
% text starting x locationtext_start_y = 0.5; % text starting y locationline_spacing = 0.04;box_spacing = 0.4;text_string=strcat(‘Euler Axis/Angle’);annotation(‘textbox’,[text_start_x text_start_y text_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘a(1) = ‘,sprintf(‘ %5.3f’, a_0(1)));annotation(‘textbox’,[text_start_x text_start_y-1*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘a(2) = ‘,sprintf(‘ %5.3f’, a_0(2)));annotation(‘textbox’,[text_start_x text_start_y-2*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘a(3) = ‘,sprintf(‘ %5.3f’, a_0(3)));annotation(‘textbox’,[text_start_x text_start_y-3*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘\phi = ‘,sprintf(‘ %5.3f’, rad2deg(phi_0)),’ deg’);annotation(‘textbox’,[text_start_x text_start_y-4*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x =0.63; % text starting x locationtext_start_y = 0.5;
% text starting y locationline_spacing = 0.04;box_spacing = 0;text_string=strcat(‘Direction Cosine Matrix (DCM)’);annotation(‘textbox’,[text_start_x text_start_y text_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘R^b^i = |’,sprintf(‘ %7.3f %7.3f %7.3f |’, R_bi(1,1:3)));annotation(‘textbox’,[text_start_x text_start_y-1*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘ |’,sprintf(‘ %7.3f %7.3f %7.3f |’,R_bi(2,1:3)));annotation(‘textbox’,[text_start_x text_start_y-2*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘ |’,sprintf(‘ %7.3f
%7.3f %7.3f |’,R_bi(3,1:3)));annotation(‘textbox’,[text_start_x text_start_y-3*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x =0.78; % text starting x locationtext_start_y = 0.4; % text starting y locationline_spacing = 0.04;box_spacing = 0;text_string=strcat(‘Quaternions’);annotation(‘textbox’,[text_start_x text_start_y text_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘q(1) = ‘,sprintf(‘ %4.3f’, q_0(1)));annotation(‘textbox’,[text_start_x text_start_y-1*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘q(2) = ‘,sprintf(‘ %4.3f’, q_0(2)));annotation(‘textbox’,[text_start_x text_start_y-2*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘q(3) = ‘,sprintf(‘ %4.3f’, q_0(3)));annotation(‘textbox’,[text_start_x text_start_y-3*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);
text_string=strcat(‘q(4) = ‘,sprintf(‘ %4.3f’, q_0(4)));annotation(‘textbox’,[text_start_x text_start_y-4*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x =0.55; % text starting x locationtext_start_y = 0.08; % text starting y locationline_spacing = 0.04;box_spacing = 0.2;text_string=strcat(‘Ang. Vel. [deg/s]’);annotation(‘textbox’,[text_start_x text_start_y+4*line_spacingtext_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘|| \omega || = ‘,sprintf(‘ %4.1f’,rad2deg(norm(omega_b(t_ctr,:)))));annotation(‘textbox’,[text_start_x text_start_y+3*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘\omega_1= ‘,sprintf(‘[%6.3f]_b’,rad2deg(omega_b(t_ctr,1))));annotation(‘textbox’,[text_start_x text_start_y+2*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘\omega_2= ‘,sprintf(‘
[%6.3f]_b’,rad2deg(omega_b(t_ctr,2))));annotation(‘textbox’,[text_start_x text_start_y+1*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘\omega_3= ‘,sprintf(‘[%6.3f]_b’,rad2deg(omega_b(t_ctr,3))));annotation(‘textbox’,[text_start_x text_start_y+0*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x =0.30; % text starting x locationtext_start_y = 0.08; % text starting y locationline_spacing = 0.04;box_spacing = 0.1;text_string=strcat(‘Ext. Torques [Nm]’);annotation(‘textbox’,[text_start_x text_start_y+4*line_spacingtext_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘|M_b_-_e_x_t|= ‘,sprintf(‘ %3.1f’, norm(M_b_ext)),’ Nm’);annotation(‘textbox’,[text_start_x text_start_y+3*line_spacing
text_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘M_b(1) = ‘,sprintf(‘ [%6.3f]_b’,(M_b_ext(1))));annotation(‘textbox’,[text_start_x text_start_y+2*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘M_b(2) = ‘,sprintf(‘ [%6.3f]_b’,(M_b_ext(2))));annotation(‘textbox’,[text_start_x text_start_y+1*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘M_b(3) = ‘,sprintf(‘ [%6.3f]_b’,(M_b_ext(3))));annotation(‘textbox’,[text_start_x text_start_y+0*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x = 0.01; % text starting x locationtext_start_y = 0.33; % text starting y locationline_spacing = 0.05;box_spacing = 0.3;text_string=strcat(‘MOI_b [kg-m^2]’);annotation(‘textbox’,[text_start_x text_start_y text_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(sprintf(‘|%5.0f %5.0f %5.0f|’,MOI_b(1,:)));annotation(‘textbox’,[text_start_x text_start_y-1*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);
text_string=strcat(sprintf(‘|%5.0f %5.0f %5.0f|’,MOI_b(2,:)));annotation(‘textbox’,[text_start_x text_start_y-2*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(sprintf(‘|%5.0f %5.0f %5.0f|’,MOI_b(3,:)));annotation(‘textbox’,[text_start_x text_start_y-3*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x = 0.01; % text starting x locationtext_start_y = 0.22; % text starting y locationline_spacing = 0.04;box_spacing = 0.3;text_string=strcat(‘Ang. Mom. [kg-m^2/s]’);annotation(‘textbox’,[text_start_x text_start_y-0*line_spacingtext_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘|| H || = ‘,sprintf(‘ %6.3f’,norm(H_i(:))));annotation(‘textbox’,[text_start_x text_start_y-1*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘H_1= ‘,sprintf(‘ [%6.3f]_b’,(H_b(1))),sprintf(‘ [%6.3f]_i’,(H_i(1))));annotation(‘textbox’,[text_start_x text_start_y-2*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);
text_string=strcat(‘H_2= ‘,sprintf(‘ [%6.3f]_b’,(H_b(2))),sprintf(‘ [%6.3f]_i’,(H_i(2))));annotation(‘textbox’,[text_start_x text_start_y-3*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_string=strcat(‘H_3= ‘,sprintf(‘ [%6.3f]_b’,(H_b(3))),sprintf(‘ [%6.3f]_i’,(H_i(3))));annotation(‘textbox’,[text_start_x text_start_y-4*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x = 0.01; % text starting x locationtext_start_y = 0.10; % text starting y locationline_spacing = 0.04;box_spacing = 0.35;text_string=strcat(‘K.E. [kg-m^2/s^2]’);annotation(‘textbox’,[text_start_x text_start_y text_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);text_string=strcat(‘T = ‘,sprintf(‘ %5.3f’, T));
annotation(‘textbox’,[text_start_x text_start_y-1*line_spacingtext_start_x+box_spacing text_start_y],’LineStyle’,’none’,’String’,text_string);text_start_x = 0.15; % text starting x locationtext_start_y = 0.024; % text starting y locationline_spacing = 0.05;box_spacing = 0.7;text_string=sprintf(‘ASSUMPTIONS: rigid body, no energy loss, no noise’);annotation(‘textbox’,[text_start_x text_start_y text_start_x+box_spacingtext_start_y],’LineStyle’,’none’,’String’,text_string,’FontWeight’,’bold’);end%M(t_ctr) = getframe;drawnow;% frame = getframe(gcf);% writeVideo(v,frame);end% close(v);End
% SATELLITE DEMO SCRIPT — USE AT OWN RISK% — Sets initial conditions, sets ode45 options, runs ode45% — runs animate m-file, and plots solutions% — Author: % Code is distributed under The MIT License (MIT)% Copyright %% Permission is hereby granted, free of charge, to any person obtaining a copy% of this software and associated documentation files (the “Software”), to deal% in the Software without restriction, including without limitation the rights% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell% copies of the Software, and to permit persons to whom the Software is% furnished to do so, subject to the following conditions:%% The above copyright notice and this permission notice shall be included in all% copies or substantial portions of the Software.%% THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE% SOFTWARE.close all %close all figuresclear all %clear all variablesclc % clear the screenglobal MOI_b Inv_MOI_b M_b_ext D_rotor q_commanded M_tilde_inv K_P K_D show_dataRW_max_torque%% —————————————-% Inputs%—————————————-show_data = 0; % set equal to 1 if you want to see data at every time stepti=0; % Initial time [s]tf=100; % Final time [s]t_step = 1; % returns ODE45 results at a fixed time step [s] – controls speed ofvideomass = 655; % SV total mass [kg]MOI_b = [ 449 0 00
516 00 0 156]; % Mass moment of inertia matrix in body frame [kg-m2]Inv_MOI_b = inv(MOI_b); % Inverse of Inertia matrix – only compute once [1/(kg-m2)]D_rotor = 0.015277; % RW MOI about spin axisRW_max_RPM = 5000; % reaction max angular velocity [rpm]RW_max_Ang_Momentum = D_rotor*RW_max_RPM/60*2*pi; % RW max angular momentum [Nms]RW_max_torque = 0.25; % RW max torque [Nm]RW_max_ang_accel = RW_max_torque/D_rotor; % maximum angular acceleration [rad/s^2]%% INITIAL ATTITUDE via a 3-2-1 rotation sequencetheta_1 = deg2rad(0); % choose first rotation angle 3-axistheta_2 = deg2rad(0); % choose second rotation angle 2-axistheta_3 = deg2rad(0); % choose third rotation angle 1-axisR_3_1 = [ cos(theta_1) sin(theta_1) 0 ;-sin(theta_1) cos(theta_1) 0 ;
0 0 1]; % R_3 rotation matrixR_2_2 = [ cos(theta_2) 0 -sin(theta_2) ;0 1 0 ;sin(theta_2) 0 cos(theta_2) ]; % R_2 rotation matrixR_1_3 = [ 1 0 0 ;0 cos(theta_3) sin(theta_3) ;0 -sin(theta_3) cos(theta_3) ]; % R_1 rotation matrix
R_bi_0 = R_1_3*R_2_2*R_3_1; % Initial attitude DCM from a 3-2-1 rotation sequence[a,phi,q_0] = R2Q(R_bi_0); % Convert DCM to quaternion%% Initial Stateomega_b_0 = [ deg2rad(2) deg2rad(2) deg2rad(2)]’; %initial angular velocity fordetumble problem [rad/s]psi_0 = 2*pi/60*[ 1000 1000 1000 ]’; % Initial RW spin rate [rad/s]M_b_ext = [ 0 0 0]’; % external torques [Nm]x0=[q_0′ omega_b_0′ psi_0′]’; % Initial state vector [ q1 q2 q3 q4 w1 w2 w3 psi_1psi_2 psi_3]%% FINAL COMMANDED ATTITUDE via a 3-2-1 rotation sequencetheta_1 = deg2rad(30); % choose first rotation angle 3-axistheta_2 = deg2rad(30); % choose second rotation angle 2-axistheta_3 = deg2rad(30); % choose third rotation angle 1-axisR_3_1 = [ cos(theta_1) sin(theta_1) 0 ;
-sin(theta_1) cos(theta_1) 0 ;0 0 1]; % R_3 rotation matrixR_2_2 = [ cos(theta_2) 0 -sin(theta_2) ;0 1 0 ;sin(theta_2) 0 cos(theta_2) ]; % R_2 rotation matrixR_1_3 = [ 1 0 0 ;0 cos(theta_3) sin(theta_3) ;0 -sin(theta_3) cos(theta_3) ]; % R_1 rotation matrixR_bi = R_1_3*R_2_2*R_3_1; % Commanded final attitude DCM from a 3-2-1 rotationsequence
[a,phi,q_commanded] = R2Q(R_bi); % Convert DCM to quaternionK_P=50; % proportional gainK_D=500; % derivative gain% Transmuted Quaternion MatrixM_tilde = [ q_commanded(4) -q_commanded(3) q_commanded(2) q_commanded(1)q_commanded(3) q_commanded(4) -q_commanded(1) q_commanded(2)-q_commanded(2) q_commanded(1) q_commanded(4) q_commanded(3)-q_commanded(1) -q_commanded(2) -q_commanded(3) q_commanded(4) ];M_tilde_inv = inv(M_tilde);%%—————————————-% Computed initial variables%—————————————-H_b_0 = MOI_b * omega_b_0 + D_rotor*psi_0; % Initial SV angular momentum in bodyframe [kg-m^2/s][a,Phi,R_ib_0] = Q2R(q_0); % Convert quaternions to a rotation matrixMOI_i = R_ib_0*MOI_b*R_ib_0′; % Mass moment of inertia matrix in inertial frame in[kg-m2]H_i = R_ib_0*H_b_0; % Angular momentum in inertial frame
omega_i_0 = R_ib_0*omega_b_0; % Angular velocity in inertial frame%%—————————————-% Run simulation — call ODE45%—————————————-options=odeset(‘MaxStep’,0.1); %set ODE45 options[t,x]=ode45(@box_eom,[ti:t_step:tf],x0,options); %call ODE45 to compute solutionq = x(:,1:4); % separate quaternions from state vectoromega_b = x(:,5:7); % separate angular rates from state vectorpsi = x(:,8:10); % separate angular rates from state vectorfor t_ctr = 1 : length(t)xdot(t_ctr,:) = box_eom(t(t_ctr),x(t_ctr,:)’);q_error(t_ctr,:) = M_tilde_inv*q(t_ctr,:)’;[a_e,phi_error(t_ctr),R_e,q_e] = Q2R(q_error(t_ctr,:)’);endq_dot = xdot(:,1:4); % separate quaternions from state vectoromega_b_dot = xdot(:,5:7); % separate angular rates from state vector
psi_dot = xdot(:,8:10); % separate angular rates from state vector%% —————————————-% Animate and plot results%—————————————-[save_H_b,save_H_i] =box_animate_4(q,omega_b,psi,t); % Animate the solution%% —————————————-% Plot solution%—————————————-figuresubplot(2,1,1); plot(t,rad2deg(phi_error)); hold onlegend(‘\Phi_e’)ylabel(‘Error (deg)’)xlabel(‘time [s]’)title(‘Angular error (deg)’)subplot(2,1,2); plot(t,q_error) % plot the quaternion errorlegend(‘q_e_1′,’q_e_2′,’q_e_3′,’q_e_4’); % add a legendtitle(‘SV Quaternion Error’); % add a title to the plotxlabel(‘time (s)’); % add a x-label to the plotylabel(‘Quaternion Error Values’); % add a y-label to the plotfiguresubplot(2,1,1); plot(t,(psi)/2/3.14*60); hold onlegend(‘\psi_1′,’\psi_2′,’\psi_3’)ylabel(‘RW rate (RPM)’)
xlabel(‘time [s]’)title(‘RW speed vs time’)subplot(2,1,2); plot(t,(psi_dot)*D_rotor); hold onlegend(‘\tau_1′,’\tau_2′,’\tau_3’)ylabel(‘RW torque (Nm)’)xlabel(‘time [s]’)title(‘RW torque as a function of time’)% Plot angular ratesfiguresubplot(2,1,1); plot(t,rad2deg(omega_b)); % plot the angular rateslegend(‘\omega_1′,’\omega_2′,’\omega_3’); % add a legendtitle(‘SV Angular Rates’); % add a title to the plotxlabel(‘time (s)’); % add a x-label to the plotylabel(‘Angular Rates (deg/sec)’); % add a y-label to the plot% Plot quaternionssubplot(2,1,2); plot(t,q) % plot the quaternionslegend(‘q_1′,’q_2′,’q_3′,’q_4’); % add a legendtitle(‘SV Quaternions’); % add a title to the plot
xlabel(‘time (s)’); % add a x-label to the plotylabel(‘quaternion values’); % add a y-label to the plot%% XDOT – COMPUTE STATE VECTOR DERIVATIVEfunction xdot=box_eom(t,x)% Returns state derivative “xdot” given current time and state% called from script in main file: usage example: [t,x]=ode45(@eom,[0 10],x0);global MOI_b Inv_MOI_b M_b_ext D_rotor q_commanded M_tilde_inv K_P K_DRW_max_torque%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Students Add Code Herexdot = zeros(10,1); % compute state derivative at this timestep%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%end

Similar Posts

Leave a Reply

Your email address will not be published. Required fields are marked *