# Flattening the Curve of Flexible Space Robotics [PERSON]
## Appendix B Initialization Function Callbacks for the Simulation.
START MASS MATRIX %Assemble individual beam inertia matrix I_beam = ones(1,8); %Creates empty matrix of I's for eight node points I_beam = [I_beam.\"I]; %Fill in matrix values with beam inertia I_beam(1) = 0; %First node point inertia = 0 %This block of code creates the individual beam mass matrix \"m_beam\" mi = [156 22*L 54 -13*L;22*L 4*L 2 13*L -3*L 2,54 13*L 156 -22*L;-13*L -3*L -3*L -22*L 4*L 2]; m_beam = mb*L/420*mi; %This block of code establishes the value of each point mass (mp) %and the system point mass matrix (M) mp = 0.455; %Point masses, M M = [0 mp mp mp 2*mp mp mp mp]; %Matrix of 8 point masses (0 First point mass) %Creates a 14 \(\times\) 14 empty mass matrix [m] m = zeros(14,14); %Fill in the system mass matrix components % Row 1 components start at index = 1% Row 2 components start at index = 15 %(1,1) = m_beam(3,3) + m_beam(1,1) + M(2); m(2,1) = m_beam(4,3) + m_beam(2,1); m(1,2) = m_beam(3,4) + m_beam(1,2); m(2,2) = m_beam(4,4) + m_beam(2,2); m(1,3) = m_beam(1,3); m(2,3) = m_beam(2,3); m(1,4) = m_beam(1,4); m(2,4) = m_beam(2,4); % Row 3 components start at index = 29% Row 4 components start at index = 43  m(3,1) = m_beam(3,1); m(4,1) = m_beam(4,1); m(3,2) = m_beam(3,2); m(4,2) = m_beam(4,2); m(3,3) = m_beam(3,3) + m_beam(1,1) + M(3); m(4,3) = m_beam(4,3) + m_beam(2,1); m(3,4) = m_beam(3,4) + m_beam(1,2); m(4,4) = m_beam(4,4) + m_beam(2,2); m(3,5) = m_beam(1,3); m(4,5) = m_beam(2,3); m(3,6) = m_beam(1,4); m(4,6) = m_beam(2,4); % Row 5 components start at index = 59% Row 6 components start at index = 73  m(5,3) = m_beam(3,1); m(6,3) = m_beam(4,1); m(5,4) = m_beam(3,2); m(6,4) = m_beam(4,2); m(5,5) = m_beam(3,3) + m_beam(1,1) + M(4); m(6,5) = m_beam(4,3) + m_beam(2,1); m(5,6) = m_beam(3,4) + m_beam(1,2); m(6,6) = m_beam(4,4) + m_beam(2,2); m(5,7) = m_beam(1,3); m(6,7) = m_beam(2,3); m(5,8) = m_beam(1,4); m(6,8) = m_beam(2,4); % Row 7 components start at index = 89  m(7,5) = m_beam(3,1);m(7,6) = m_beam(3,2); m(7,7) = m_beam(3,3) + 3*mb + M(5) + M(6) + M(7) + M(8); m(7,8) = m_beam(3,4); % Row 8 components start at index = 103% Row 9 components start at index = 120 m(8,5) = m_beam(4,1); m(9,8) = m_beam(3,2); m(8,6) = m_beam(4,2); m(9,9) = m_beam(3,3) + m_beam(1,1) + M(6); m(8,7) = m_beam(4,3); m(9,10) = m_beam(3,4) + m_beam(1,2); m(8,8) = m_beam(4,4) + m_beam(2,2); m(9,11) = m_beam(1,3); m(8,9) = m_beam(2,3); m(9,12) = m_beam(1,4); m(8,10) = m_beam(2,4); % Row 10 components start at index = 134% Row 11 components start at index = 149 m(10,8) = m_beam(4,2); m(11,9) = m_beam(3,1); m(10,9) = m_beam(4,3) + m_beam(2,1); m(11,10) = m_beam(3,2); m(10,10) = m_beam(4,4) + m_beam(2,2); m(11,11) = m_beam(3,3) + m_beam(1,1) + M(7); m(10,11) = m_beam(2,3); m(11,12) = m_beam(3,4) + m_beam(1,2); m(10,12) = m_beam(2,4); m(11,13) = m_beam(1,3); m(11,14) = m_beam(1,4); % Row 12 components start at index = 163 m(12,9) = m_beam(4,1); m(12,10) = m_beam(4,2); m(12,11) = m_beam(4,3) + m_beam(2,1); m(12,12) = m_beam(4,4) + m_beam(2,2); m(12,13) = m_beam(2,3); m(12,14) = m_beam(2,4); % Row 13 components start at index = 179% Row 14 components start at index = 193 m(13,11) = m_beam(3,1); m(14,11) = m_beam(4,1); m(13,12) = m_beam(3,2); m(14,12) = m_beam(4,2); m(13,13) = m_beam(3,3) + M(8); m(14,13) = m_beam(4,3); m(13,14) = m_beam(3,4); m(14,14) = m_beam(4,4); %Display the system mass matrix to check m = m; %Calculate the natural frequencies and normal modes [NormalModes,EigenValues] = eig(inv(m)*k); NaturalFrequencies = diag(EigenValues'0.5); ModeShapes = NormalModes; %Check Orthogonality like Homework 1 confirm diagonal matrix of 1's %to satisfy Equation 24 on slide 17 OrthoMass = NormalModes*m*NormalModes; OrthoStiff = NormalModes*k*NormalModes; StiffCheck = OrthoStiff/EigenValues; Equation24_OrthoCheck = diag(diag(GistiffCheck/OrthoMass)); %Spacecraft Radius to be used designating rigid modal coordinate R = 0.381; FeeE = NormalModes; %Designate Elastic mode shapes array FeeE Omega = NaturalFrequencies; %Designate variable name 'Omega' as natural frequencies %Designate Rigid modal coordinate FeeR = [R + L 1 R + L*2 1 R + L*3 1 R + L*4 1 - L 1 -L*2 1 -L*3 1]; Di = FeeE*m*diag(FeeR); %Calculate Rigid-Elastic Coupling Coefficient DiCheck = det(Di); %Confirm Di is singular...det(Di = 0) Z = 0.0005; Izz = 14; w = diag(NaturalFrequencies); %Generate a diagonal matrix of natural frequency Iw = 0.0912; Td = 0; %Disturbance TorqueTc = 0.1; %Control Torque is Iw*qddot_wheel  
T = Td + Tc; %Total Torque is sum of disturbance and control torques  
%Start State Space Development  
NatFreq = diag(EigenValues).^0.5;  
r = 0.381; %Radius of the wheel (large rigid body)  
freqs = sqrt(EigenValues); %  
NatFreq = EigenValues(1:5,1:5);  
freqs = freqs(1:5,1:5);  
zeta = 0.0005; %Given damping ratio for all modes  
Izz = 14;  
phi_E = NormalModes(1:14,1:5);  
phi_R = [r + L,1,r + 2*L,1,r + 3*L,1,r + 4*L,1,-L,1,-2*L,1,-3*L,1];  
M_II = m;  
Di = [phi_E*M_II*phi_R];  
M_state = [Izz Di';  
Di eye(5)];  
C_damp = [zeros(6,6)];  
C_damp(2:6,2:6) = 2*zeta*freqs;  
K = [zeros(6,6)];  
K(2:6,2:6) = NatFreq;  
A = [zeros(6),eye(6,6);  
-inv(M_state)*K, -inv(M_state)*C_damp];  
Bprime = [1;0;0;0;0;0];  
B = [0 0 0 0 0 0 (inv(M_state)*Bprime)]^;  
C = zeros(12,12); C(1,1) = 1;  
D = zeros(12,1);  
[Gnum,Gden] = ss2 tf(A,B,C,D);  
G1 = tf(Gnum(1,:),Gden)  
%Manually input Transfer Function to check  
NUM = 1(9.98e-015 0.1268 0.007582 166.9 5.591 4.718e004 771 3.412e006 1.218e004  
1.576e007 1.475e004 7.11e006];  
DEN = [1 0.06125 1326 46.15 3.781e005 6683 2.808e007 1.388e005 1.813e008 2.065e005  
9.954e007 0 0];  
G = tf(NUM,DEN);  
%Put PID controller Transfer function into workspace  
It = 14; Z = 0.516931;  
Bandwidth = 4; wn = Bandwidth; T = 10/Z/wn;  
Kd = 2*Z*wn*It + It/T;  
Kp = wn^2 + 2*Z*wn/T;  
Ki = wn^2/T;  
PID = tf([Kd Kp Ki],[0 1 0]);  
%DESIGN FILTERS TO SMOOTH OUT MODE 1  
%Design Bandpass filter for w = 10^-0.1478 = 0.711541 Hz  
wz = 0.711541;Zz = 0.1;wp = wz;Zp = 0.0005;  
BP1 = tf([1/wz^2 2*Zz/wz 1],[1/wp^2 2*Zp/wp 1]);  
PID_BP1 = PID*BP1;  
%Design Notch filter for w = 10^-0.109 = 0.778037 Hz  
wz = 0.778037;Zz = 0.0005;wp = wz;Zp = 0.1;  
Notch1 = tf([1/wz^2 2*Zz/wz 1],[1/wp^2 2*Zp/wp 1]);  
Mode_1 = PID*BP1*Notch1;  
%DESIGN FILTERS TO SMOOTH OUT MODE 2  
%Design Bandpass filter for w = 10^0.3223  
wz = 10^0.3223;Zz = 0.1;wp = wz;Zp = 0.0005;  
BP2 = tf([1/wz^2 2*Zz/wz 1],[1/wp^2 2*Zp/wp 1]);%Design Notch filter for w = 10'0.405 wz = 10'0.405;ZZ = 0.0006;wp = wz;Zp = 0.1; Notch2 = tf([1/wz^2 2*Zz/wz 1],[1/wp^2 2*Zp/wp 1]); Mode_2 = Mode_1*BP2*Notch2; %Design Lead filter for wz-1, wp-3 %wz = 1;ZZ = 1;wp = 3;Zp = 1; %Lead = tf([1/wz^2 2*Zz/wz 1],[1/wp^2 2*Zp/wp 1]); %Mode_2 = Mode_2*Lead; %DESIGN FILTERS TO SMOOTH OUT MODE 3 %Design Bandpass filter for w = 10\"1.0110 %z = 0.1;wp = wz;Zp = 0.0005; BP3 = tf([1/wz^2 2*Zz/wz 1],[1/wp^2 2*Zp/wp 1]); %Design Notch filter for w = 10\"1.0128 %z = 0.0005;wp = wz;Zp = 0.1; Notch3 = tf([1/wz^2 2*Zz/wz 1],[1/wp^2 2*Zp/wp 1]); Mode_3 = Mode_2*BP3*Notch3; %DESIGN FILTERS TO SMOOTH OUT MODE 4 %Design Bandpass filter for w = 10\"1.49035 %z = 10\"1.49035;ZZ = 0.1;wp = wz;Zp = 0.0005; BP4 = tf([1/wz^2 2*Zz/wz 1],[1/wp^2 2*Zp/wp 1]); %Design Notch filter for w = 10\"1.492 %z = 10\"1.492;ZZ = 0.0005;wp = wz;Zp = 0.1; Notch4 = tf([1/wz^2 2*Zz/wz 1],[1/wp^2 2*Zp/wp 1]); Mode_4 = Mode_3*BP4*Notch4; %CALCULATE SYSTEM NATURAL FREQUNCIES [NaturalFrequencies,Damping,EigenValue] = damp(G); NaturalFrequencies = NaturalFrequencies;