0001 function [sys,VERSION] = model_LPF_Dynamics
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025 VERSION = '$Id:$';
0026 utils.helper.msg(utils.const.msg.MNAME, ['running ', mfilename]);
0027
0028
0029 sys = ssm;
0030
0031
0032 sys.paramnames = ...
0033 {...
0034 'p_rH1x_B' 'p_rH1y_B' 'p_rH1z_B' 'p_thetaH1' 'p_etaH1' 'p_phiH1' ...
0035 'p_rH2x_B' 'p_rH2y_B' 'p_rH2z_B' 'p_thetaH2' 'p_etaH2' 'p_phiH2' ...
0036 ...
0037 'p_thetaB' 'p_etaB' 'p_phiB' ...
0038 'p_omegaBx' 'p_omegaBy' 'p_omegaBz' ...
0039 'p_r1x_H1' 'p_r1y_H1' 'p_r1z_H1' 'p_theta1' 'p_eta1' 'p_phi1' ...
0040 'p_r2x_H2' 'p_r2y_H2' 'p_r2z_H2' 'p_theta2' 'p_eta2' 'p_phi2' ...
0041 'p_omega1x' 'p_omega1y' 'p_omega1z' ...
0042 'p_omega2x' 'p_omega2y' 'p_omega2z' ...
0043 ...
0044 'p_m_SC' 'p_Ixx_SC' 'p_Iyy_SC' 'p_Izz_SC' 'p_Ixy_SC' 'p_Ixz_SC' 'p_Iyz_SC' ...
0045 'p_m_TM1' 'p_Ixx_TM1' 'p_Iyy_TM1' 'p_Izz_TM1' 'p_Ixy_TM1' 'p_Ixz_TM1' 'p_Iyz_TM1' ...
0046 'p_m_TM2' 'p_Ixx_TM2' 'p_Iyy_TM2' 'p_Izz_TM2' 'p_Ixy_TM2' 'p_Ixz_TM2' 'p_Iyz_TM2' ...
0047 ...
0048 'p_StTM1_ext11' 'p_StTM1_ext12' 'p_StTM1_ext13' 'p_StTM1_ext14' 'p_StTM1_ext15' 'p_StTM1_ext16' ...
0049 'p_StTM1_ext21' 'p_StTM1_ext22' 'p_StTM1_ext23' 'p_StTM1_ext24' 'p_StTM1_ext25' 'p_StTM1_ext26' ...
0050 'p_StTM1_ext31' 'p_StTM1_ext32' 'p_StTM1_ext33' 'p_StTM1_ext34' 'p_StTM1_ext35' 'p_StTM1_ext36' ...
0051 'p_StTM1_ext41' 'p_StTM1_ext42' 'p_StTM1_ext43' 'p_StTM1_ext44' 'p_StTM1_ext45' 'p_StTM1_ext46' ...
0052 'p_StTM1_ext51' 'p_StTM1_ext52' 'p_StTM1_ext53' 'p_StTM1_ext54' 'p_StTM1_ext55' 'p_StTM1_ext56' ...
0053 'p_StTM1_ext61' 'p_StTM1_ext62' 'p_StTM1_ext63' 'p_StTM1_ext64' 'p_StTM1_ext65' 'p_StTM1_ext66' ...
0054 ...
0055 'p_StTM1_act11' 'p_StTM1_act12' 'p_StTM1_act13' 'p_StTM1_act14' 'p_StTM1_act15' 'p_StTM1_act16' ...
0056 'p_StTM1_act21' 'p_StTM1_act22' 'p_StTM1_act23' 'p_StTM1_act24' 'p_StTM1_act25' 'p_StTM1_act26' ...
0057 'p_StTM1_act31' 'p_StTM1_act32' 'p_StTM1_act33' 'p_StTM1_act34' 'p_StTM1_act35' 'p_StTM1_act36' ...
0058 'p_StTM1_act41' 'p_StTM1_act42' 'p_StTM1_act43' 'p_StTM1_act44' 'p_StTM1_act45' 'p_StTM1_act46' ...
0059 'p_StTM1_act51' 'p_StTM1_act52' 'p_StTM1_act53' 'p_StTM1_act54' 'p_StTM1_act55' 'p_StTM1_act56' ...
0060 'p_StTM1_act61' 'p_StTM1_act62' 'p_StTM1_act63' 'p_StTM1_act64' 'p_StTM1_act65' 'p_StTM1_act66' ...
0061 ...
0062 'p_StTM2_ext11' 'p_StTM2_ext12' 'p_StTM2_ext13' 'p_StTM2_ext14' 'p_StTM2_ext15' 'p_StTM2_ext16' ...
0063 'p_StTM2_ext21' 'p_StTM2_ext22' 'p_StTM2_ext23' 'p_StTM2_ext24' 'p_StTM2_ext25' 'p_StTM2_ext26' ...
0064 'p_StTM2_ext31' 'p_StTM2_ext32' 'p_StTM2_ext33' 'p_StTM2_ext34' 'p_StTM2_ext35' 'p_StTM2_ext36' ...
0065 'p_StTM2_ext41' 'p_StTM2_ext42' 'p_StTM2_ext43' 'p_StTM2_ext44' 'p_StTM2_ext45' 'p_StTM2_ext46' ...
0066 'p_StTM2_ext51' 'p_StTM2_ext52' 'p_StTM2_ext53' 'p_StTM2_ext54' 'p_StTM2_ext55' 'p_StTM2_ext56' ...
0067 'p_StTM2_ext61' 'p_StTM2_ext62' 'p_StTM2_ext63' 'p_StTM2_ext64' 'p_StTM2_ext65' 'p_StTM2_ext66' ...
0068 ...
0069 'p_StTM2_act11' 'p_StTM2_act12' 'p_StTM2_act13' 'p_StTM2_act14' 'p_StTM2_act15' 'p_StTM2_act16' ...
0070 'p_StTM2_act21' 'p_StTM2_act22' 'p_StTM2_act23' 'p_StTM2_act24' 'p_StTM2_act25' 'p_StTM2_act26' ...
0071 'p_StTM2_act31' 'p_StTM2_act32' 'p_StTM2_act33' 'p_StTM2_act34' 'p_StTM2_act35' 'p_StTM2_act36' ...
0072 'p_StTM2_act41' 'p_StTM2_act42' 'p_StTM2_act43' 'p_StTM2_act44' 'p_StTM2_act45' 'p_StTM2_act46' ...
0073 'p_StTM2_act51' 'p_StTM2_act52' 'p_StTM2_act53' 'p_StTM2_act54' 'p_StTM2_act55' 'p_StTM2_act56' ...
0074 'p_StTM2_act61' 'p_StTM2_act62' 'p_StTM2_act63' 'p_StTM2_act64' 'p_StTM2_act65' 'p_StTM2_act66' ...
0075 };
0076
0077 sys.paramvalues = ...
0078 [ ...
0079 0 0 0 0 0 0 ...
0080 0 0 0 0 0 0 ...
0081 ...
0082 0 0 0 ...
0083 0 0 0 ...
0084 0 0 0 0 0 0 ...
0085 0 0 0 0 0 0 ...
0086 0 0 0 ...
0087 0 0 0 ...
0088 ...
0089 0 0 0 0 0 0 0 ...
0090 0 0 0 0 0 0 0 ...
0091 0 0 0 0 0 0 0 ...
0092 ...
0093 0 0 0 0 0 0 ...
0094 0 0 0 0 0 0 ...
0095 0 0 0 0 0 0 ...
0096 0 0 0 0 0 0 ...
0097 0 0 0 0 0 0 ...
0098 0 0 0 0 0 0 ...
0099 ...
0100 0 0 0 0 0 0 ...
0101 0 0 0 0 0 0 ...
0102 0 0 0 0 0 0 ...
0103 0 0 0 0 0 0 ...
0104 0 0 0 0 0 0 ...
0105 0 0 0 0 0 0 ...
0106 ...
0107 0 0 0 0 0 0 ...
0108 0 0 0 0 0 0 ...
0109 0 0 0 0 0 0 ...
0110 0 0 0 0 0 0 ...
0111 0 0 0 0 0 0 ...
0112 0 0 0 0 0 0 ...
0113 ...
0114 0 0 0 0 0 0 ...
0115 0 0 0 0 0 0 ...
0116 0 0 0 0 0 0 ...
0117 0 0 0 0 0 0 ...
0118 0 0 0 0 0 0 ...
0119 0 0 0 0 0 0 ...
0120 ];
0121
0122
0123 sys.paramsigmas = ...
0124 [ ...
0125 0 0 0 0 0 0 ...
0126 0 0 0 0 0 0 ...
0127 ...
0128 0 0 0 ...
0129 0 0 0 ...
0130 0 0 0 0 0 0 ...
0131 0 0 0 0 0 0 ...
0132 0 0 0 ...
0133 0 0 0 ...
0134 ...
0135 0 0 0 0 0 0 0 ...
0136 0 0 0 0 0 0 0 ...
0137 0 0 0 0 0 0 0 ...
0138 ...
0139 0 0 0 0 0 0 ...
0140 0 0 0 0 0 0 ...
0141 0 0 0 0 0 0 ...
0142 0 0 0 0 0 0 ...
0143 0 0 0 0 0 0 ...
0144 0 0 0 0 0 0 ...
0145 ...
0146 0 0 0 0 0 0 ...
0147 0 0 0 0 0 0 ...
0148 0 0 0 0 0 0 ...
0149 0 0 0 0 0 0 ...
0150 0 0 0 0 0 0 ...
0151 0 0 0 0 0 0 ...
0152 ...
0153 0 0 0 0 0 0 ...
0154 0 0 0 0 0 0 ...
0155 0 0 0 0 0 0 ...
0156 0 0 0 0 0 0 ...
0157 0 0 0 0 0 0 ...
0158 0 0 0 0 0 0 ...
0159 ...
0160 0 0 0 0 0 0 ...
0161 0 0 0 0 0 0 ...
0162 0 0 0 0 0 0 ...
0163 0 0 0 0 0 0 ...
0164 0 0 0 0 0 0 ...
0165 0 0 0 0 0 0 ...
0166 ];
0167
0168
0169
0170
0171 for i_params=1:length(sys.paramnames)
0172 cmd = [sys.paramnames{i_params}, '=sym(''', sys.paramnames{i_params},''');'];
0173 eval(cmd);
0174 end
0175
0176
0177
0178
0179
0180 L_TM1 = 0.046;
0181 L_TM2 = 0.046;
0182
0183 rB_M = [9.4e-3; 16.4e-3; 478.9e-3];
0184 rH1_M = [0.376/2; 0; 0.6093];
0185 rH2_M = [-0.376/2; 0; 0.6093];
0186 alphaH1_0 = [0; 0; 0];
0187 alphaH2_0 = [0; 0; 0];
0188
0189
0190 alphaB_0 = [0; 0; 0];
0191 omegaB_0 = [0; 0; 0];
0192
0193
0194 r1_H1_0 = [0; 0; 0];
0195 alpha1_0 = [0; 0; 0];
0196 r2_H2_0 = [0; 0; 0];
0197 alpha2_0 = [0; 0; 0];
0198 omega1_0 = [0; 0; 0];
0199 omega2_0 = [0; 0; 0];
0200
0201
0202 m_SC_0 = 415.2;
0203 I_SC_M_0 = [ 212.9 0 0 ;...
0204 0 214.2 0 ;...
0205 0 0 197.0 ];
0206 m_TM1_0 = 1.96;
0207 I_TM1_0 = [6.912e-04 0 0 ;...
0208 0 6.912e-04 0 ;...
0209 0 0 6.912e-04 ];
0210 m_TM2_0 = 1.96;
0211 I_TM2_0 = [ 6.912e-04 0 0 ;...
0212 0 6.912e-04 0 ;...
0213 0 0 6.912e-04 ];
0214
0215
0216 Stiff_TM1_ext_0 =1.e-07 * [ 13.5-0.65 0.135 0.135 0.37 0.37 0.37 ; ...
0217 0.135 13.5 0.135 0.37 0.37 0.37 ; ...
0218 0.135 0.135 27. 0.37 0.37 0.37 ; ...
0219 0.6 0.6 0.6 53. 0.6 0.6 ; ...
0220 0.6 0.6 0.6 0.6 53. 0.6 ; ...
0221 0.6 0.6 0.6 0.6 0.6 40. ] ;
0222 Stiff_TM2_ext_0 = 1.e-07 * [ 13.5 0.135 0.135 0.37 0.37 0.37 ; ...
0223 0.135 13.5 0.135 0.37 0.37 0.37 ; ...
0224 0.135 0.135 27. 0.37 0.37 0.37 ; ...
0225 0.6 0.6 0.6 53. 0.6 0.6 ; ...
0226 0.6 0.6 0.6 0.6 53. 0.6 ; ...
0227 0.6 0.6 0.6 0.6 0.6 40. ] ;
0228 Stiff_TM1_act_0 = 1.e-07 * [ 6.5 0.065 0.065 0.13 0.13 0.13 ; ...
0229 0.065 15.2 0.065 0.13 0.13 0.13 ; ...
0230 0.065 0.065 21.2 0.13 0.13 0.13 ; ...
0231 1.5 1.5 1.5 3.38 1.0 1.0 ; ...
0232 1.5 1.5 1.5 1.0 2.41 1.0 ; ...
0233 1.5 1.5 1.5 1.0 1.0 1.37 ] ;
0234 Stiff_TM2_act_0 = 1.e-07 * [ 6.5 0.065 0.065 0.13 0.13 0.13 ; ...
0235 0.065 15.2 0.065 0.13 0.13 0.13 ; ...
0236 0.065 0.065 21.2 0.13 0.13 0.13 ; ...
0237 1.5 1.5 1.5 3.38 1.0 1.0 ; ...
0238 1.5 1.5 1.5 1.0 2.41 1.0 ; ...
0239 1.5 1.5 1.5 1.0 1.0 1.37 ] ;
0240
0241
0242
0243
0244 alphaB = alphaB_0 + [p_thetaB ; p_etaB; p_phiB] ;
0245 omegaB = omegaB_0 + [p_omegaBx; p_omegaBy; p_omegaBz] ;
0246
0247
0248 r1_H1 = r1_H1_0 + [p_r1x_H1; p_r1y_H1; p_r1z_H1] ;
0249 alpha1 = alpha1_0 + [p_theta1; p_eta1; p_phi1] ;
0250 r2_H2 = r2_H2_0 + [p_r2x_H2; p_r2y_H2; p_r2z_H2] ;
0251 alpha2 = alpha2_0 + [p_theta2; p_eta2; p_phi2] ;
0252 omega1 = omega1_0 + [p_omega1x; p_omega1y; p_omega1z] ;
0253 omega2 = omega2_0 + [p_omega2x; p_omega2y; p_omega2z] ;
0254
0255
0256 rH1_B = -rB_M + rH1_M + [p_rH1x_B; p_rH1y_B; p_rH1z_B];
0257 alphaH1 = alphaH1_0 + [p_thetaH1; p_etaH1; p_phiH1];
0258 rH2_B = -rB_M + rH2_M + [p_rH2x_B; p_rH2y_B; p_rH2z_B];
0259 alphaH2 = alphaH2_0 + [p_thetaH2; p_etaH2; p_phiH2];
0260
0261
0262
0263 T_B_J = ssm.T_ZYX_rot(alphaB); T_J_B = T_B_J.';
0264 T_H1_B = ssm.T_ZYX_rot(alphaH1); T_B_H1 = T_H1_B.';
0265 T_H2_B = ssm.T_ZYX_rot(alphaH2); T_B_H2 = T_H2_B.';
0266 T_T1_H1 = ssm.T_ZYX_rot(alpha1); T_H1_T1 = T_T1_H1.';
0267 T_T2_H2 = ssm.T_ZYX_rot(alpha2); T_H2_T2 = T_T2_H2.';
0268
0269
0270 T_J_T1 = T_J_B*T_B_H1*T_H1_T1; T_T1_J = T_J_T1.';
0271 T_J_T2 = T_J_B*T_B_H2*T_H2_T2; T_T2_J = T_J_T2.';
0272 T_B_T1 = T_B_H1*T_H1_T1; T_T1_B = T_B_T1.';
0273 T_B_T2 = T_B_H2*T_H2_T2; T_T2_B = T_B_T2.';
0274 T_J_H1 = T_J_B*T_B_H1; T_H1_J = T_J_H1.';
0275 T_J_H2 = T_J_B*T_B_H2; T_H2_J = T_J_H2.';
0276
0277
0278
0279
0280
0281
0282 T_ftH1_2_fJtB = -[T_J_H1 zeros(3,3);...
0283 ssm.skew(rH1_B)*T_B_H1+T_B_H1*ssm.skew(r1_H1) T_B_H1 ];
0284
0285 T_ftH2_2_fJtB = -[T_J_H2 zeros(3,3);...
0286 ssm.skew(rH2_B)*T_B_H2+T_B_H2*ssm.skew(r2_H2) T_B_H2 ];
0287
0288
0289
0290
0291 T_ftT1_2_fJtB = -[T_J_T1 zeros(3,3);...
0292 (ssm.skew(rH1_B)*T_B_H1+T_B_H1*ssm.skew(r1_H1))*T_H1_T1 T_B_T1 ];
0293
0294 T_ftT2_2_fJtB = -[T_J_T2 zeros(3,3);...
0295 (ssm.skew(rH2_B)*T_B_H2+T_B_H2*ssm.skew(r2_H2))*T_H2_T2 T_B_T2 ];
0296
0297 T_ftB_2_fJtB = [ T_J_B zeros(3,3) ;...
0298 zeros(3,3) eye(3,3) ];
0299
0300 T_ftJ_2_fJtB = [ eye(3,3) zeros(3,3) ;...
0301 zeros(3,3) T_B_J ];
0302
0303
0304 T_ftH1_2_fH1tT1 = [eye(3,3) zeros(3,3) ;
0305 zeros(3,3) T_T1_H1 ];
0306
0307 T_ftH2_2_fH2tT2 = [eye(3,3) zeros(3,3) ;...
0308 zeros(3,3) T_T2_H2 ];
0309
0310
0311 T_ftT1_2_fH1tT1 = [T_H1_T1 zeros(3,3) ;
0312 zeros(3,3) eye(3,3) ];
0313
0314 T_ftT2_2_fH2tT2 = [T_H2_T2 zeros(3,3) ;...
0315 zeros(3,3) eye(3,3) ];
0316
0317
0318 T_ftJ_2_fH1tT1 = [T_H1_J zeros(3);...
0319 zeros(3) T_T1_J ];
0320
0321 T_ftJ_2_fH2tT2 = [T_H2_J zeros(3);...
0322 zeros(3) T_T2_J ];
0323
0324
0325
0326
0327
0328
0329 m_SC = m_SC_0 + p_m_SC ;
0330 M_SC = m_SC*eye(3);
0331
0332
0333 I_SC_0 = I_SC_M_0 - m_SC_0 * ssm.skew(rB_M)*(ssm.skew(rB_M)).' ;
0334
0335 p_I_SC = [ p_Ixx_SC p_Ixy_SC p_Ixz_SC ;...
0336 p_Ixy_SC p_Iyy_SC p_Iyz_SC ;...
0337 p_Ixz_SC p_Iyz_SC p_Izz_SC ];
0338
0339 I_SC = I_SC_0 + p_I_SC;
0340
0341
0342 m_TM1 = m_TM1_0 + p_m_TM1;
0343 m_TM2 = m_TM2_0 + p_m_TM2;
0344 M_TM1 = m_TM1*eye(3);
0345 M_TM2 = m_TM2*eye(3);
0346
0347 p_I_TM1 = [p_Ixx_TM1 p_Ixy_TM1 p_Ixz_TM1 ;...
0348 p_Ixy_TM1 p_Iyy_TM1 p_Iyz_TM1 ;...
0349 p_Ixz_TM1 p_Iyz_TM1 p_Izz_TM1 ];
0350 I_TM1 = I_TM1_0 + p_I_TM1;
0351
0352 p_I_TM2 = [p_Ixx_TM2 p_Ixy_TM2 p_Ixz_TM2 ;...
0353 p_Ixy_TM2 p_Iyy_TM2 p_Iyz_TM2 ;...
0354 p_Ixz_TM2 p_Iyz_TM2 p_Izz_TM2 ];
0355 I_TM2 = I_TM2_0 + p_I_TM2;
0356
0357
0358
0359 M11 = [eye(3) zeros(3);...
0360 zeros(3) ssm.B_a2w(alphaB)];
0361
0362 M21 = [zeros(3,3) zeros(3,3) ;...
0363 zeros(3,3) ssm.skew(omegaB)*I_SC*ssm.B_a2w(alphaB)];
0364
0365 M22= [M_SC zeros(3);...
0366 zeros(3) I_SC ];
0367
0368 M33 = [eye(3) zeros(3);...
0369 zeros(3) ssm.B_a2w(alpha1)];
0370
0371 M44 = [eye(3) zeros(3);...
0372 zeros(3) ssm.B_a2w(alpha2)];
0373
0374 M51 = [zeros(3) -m_TM1*T_H1_B*ssm.skew(omegaB)*ssm.skew(rH1_B+ T_B_H1*r1_H1)*ssm.B_a2w(alphaB);...
0375 zeros(3) ssm.skew(T_T1_B*omegaB+omega1)*I_TM1*T_T1_B*ssm.B_a2w(alphaB) ];
0376
0377 M52 = [m_TM1*T_H1_J -m_TM1*T_H1_B*ssm.skew(rH1_B+ T_B_H1*r1_H1) ;...
0378 zeros(3) I_TM1*T_T1_B ];
0379
0380 M53 = [2*m_TM1*ssm.skew(T_H1_B*omegaB) zeros(3) ;...
0381 zeros(3) (I_TM1*ssm.skew(T_T1_B*omegaB)+ssm.skew(T_T1_B*omegaB+omega1)*I_TM1)*ssm.B_a2w(alpha1)];
0382
0383 M55 = [M_TM1 zeros(3) ;...
0384 zeros(3) I_TM1 ];
0385
0386 M61 = [zeros(3) -m_TM2*T_H2_B*ssm.skew(omegaB)*ssm.skew(rH2_B+ T_B_H2*r2_H2)*ssm.B_a2w(alphaB) ;...
0387 zeros(3) ssm.skew(T_T2_B*omegaB+omega2)*I_TM2*T_T2_B*ssm.B_a2w(alphaB) ];
0388
0389 M62 = [m_TM2*T_H2_J -m_TM2*T_H2_B*ssm.skew(rH2_B+ T_B_H2*r2_H2) ;...
0390 zeros(3) I_TM2*T_T2_B ];
0391
0392 M64 = [2*m_TM2*ssm.skew(T_H2_B*omegaB) zeros(3) ;...
0393 zeros(3) (I_TM2*ssm.skew(T_T2_B*omegaB)+ssm.skew(T_T2_B*omegaB+omega2)*I_TM2)*ssm.B_a2w(alpha2) ] ;
0394
0395 M66 = [M_TM2 zeros(3) ;...
0396 zeros(3) I_TM2 ];
0397
0398 M = [ M11 zeros(6) zeros(6) zeros(6) zeros(6) zeros(6);...
0399 M21 M22 zeros(6) zeros(6) zeros(6) zeros(6);...
0400 zeros(6) zeros(6) M33 zeros(6) zeros(6) zeros(6);...
0401 zeros(6) zeros(6) zeros(6) M44 zeros(6) zeros(6);...
0402 M51 M52 M53 zeros(6) M55 zeros(6);...
0403 M61 M62 zeros(6) M64 zeros(6) M66 ];
0404
0405
0406
0407
0408
0409
0410 Stiff_Dim_TM1 = [m_TM1_0 m_TM1_0 m_TM1_0 0.5*L_TM1*m_TM1_0 0.5*L_TM1*m_TM1_0 0.5*L_TM1*m_TM1_0 ; ...
0411 m_TM1_0 m_TM1_0 m_TM1_0 0.5*L_TM1*m_TM1_0 0.5*L_TM1*m_TM1_0 0.5*L_TM1*m_TM1_0 ; ...
0412 m_TM1_0 m_TM1_0 m_TM1_0 0.5*L_TM1*m_TM1_0 0.5*L_TM1*m_TM1_0 0.5*L_TM1*m_TM1_0 ; ...
0413 2/L_TM1*I_TM1_0(1,1) 2/L_TM1*I_TM1_0(1,1) 2/L_TM1*I_TM1_0(1,1) I_TM1_0(1,1) I_TM1_0(1,1) I_TM1_0(1,1) ; ...
0414 2/L_TM1*I_TM1_0(1,1) 2/L_TM1*I_TM1_0(1,1) 2/L_TM1*I_TM1_0(1,1) I_TM1_0(1,1) I_TM1_0(1,1) I_TM1_0(1,1) ; ...
0415 2/L_TM1*I_TM1_0(1,1) 2/L_TM1*I_TM1_0(1,1) 2/L_TM1*I_TM1_0(1,1) I_TM1_0(1,1) I_TM1_0(1,1) I_TM1_0(1,1) ];
0416
0417 Stiff_Dim_TM2 = [m_TM2_0 m_TM2_0 m_TM2_0 0.5*L_TM2*m_TM2_0 0.5*L_TM2*m_TM2_0 0.5*L_TM2*m_TM2_0 ; ...
0418 m_TM2_0 m_TM2_0 m_TM2_0 0.5*L_TM2*m_TM2_0 0.5*L_TM2*m_TM2_0 0.5*L_TM2*m_TM2_0 ; ...
0419 m_TM2_0 m_TM2_0 m_TM2_0 0.5*L_TM2*m_TM2_0 0.5*L_TM2*m_TM2_0 0.5*L_TM2*m_TM2_0 ; ...
0420 2/L_TM2*I_TM2_0(1,1) 2/L_TM2*I_TM2_0(1,1) 2/L_TM2*I_TM2_0(1,1) I_TM2_0(1,1) I_TM2_0(1,1) I_TM2_0(1,1) ; ...
0421 2/L_TM2*I_TM2_0(1,1) 2/L_TM2*I_TM2_0(1,1) 2/L_TM2*I_TM2_0(1,1) I_TM2_0(1,1) I_TM2_0(1,1) I_TM2_0(1,1) ; ...
0422 2/L_TM2*I_TM2_0(1,1) 2/L_TM2*I_TM2_0(1,1) 2/L_TM2*I_TM2_0(1,1) I_TM2_0(1,1) I_TM2_0(1,1) I_TM2_0(1,1) ];
0423
0424
0425
0426
0427
0428
0429
0430
0431
0432
0433
0434 p_Stiff_TM1_ext = [ p_StTM1_ext11 p_StTM1_ext12 p_StTM1_ext13 p_StTM1_ext14 p_StTM1_ext15 p_StTM1_ext16 ; ...
0435 p_StTM1_ext21 p_StTM1_ext22 p_StTM1_ext23 p_StTM1_ext24 p_StTM1_ext25 p_StTM1_ext26 ; ...
0436 p_StTM1_ext31 p_StTM1_ext32 p_StTM1_ext33 p_StTM1_ext34 p_StTM1_ext35 p_StTM1_ext36 ; ...
0437 p_StTM1_ext41 p_StTM1_ext42 p_StTM1_ext43 p_StTM1_ext44 p_StTM1_ext45 p_StTM1_ext46 ; ...
0438 p_StTM1_ext51 p_StTM1_ext52 p_StTM1_ext53 p_StTM1_ext54 p_StTM1_ext55 p_StTM1_ext56 ; ...
0439 p_StTM1_ext61 p_StTM1_ext62 p_StTM1_ext63 p_StTM1_ext64 p_StTM1_ext65 p_StTM1_ext66 ] ;
0440
0441 p_Stiff_TM2_ext = [ p_StTM2_ext11 p_StTM2_ext12 p_StTM2_ext13 p_StTM2_ext14 p_StTM2_ext15 p_StTM2_ext16 ; ...
0442 p_StTM2_ext21 p_StTM2_ext22 p_StTM2_ext23 p_StTM2_ext24 p_StTM2_ext25 p_StTM2_ext26 ; ...
0443 p_StTM2_ext31 p_StTM2_ext32 p_StTM2_ext33 p_StTM2_ext34 p_StTM2_ext35 p_StTM2_ext36 ; ...
0444 p_StTM2_ext41 p_StTM2_ext42 p_StTM2_ext43 p_StTM2_ext44 p_StTM2_ext45 p_StTM2_ext46 ; ...
0445 p_StTM2_ext51 p_StTM2_ext52 p_StTM2_ext53 p_StTM2_ext54 p_StTM2_ext55 p_StTM2_ext56 ; ...
0446 p_StTM2_ext61 p_StTM2_ext62 p_StTM2_ext63 p_StTM2_ext64 p_StTM2_ext65 p_StTM2_ext66 ] ;
0447
0448 Stiff_TM1_ext = (Stiff_TM1_ext_0 + p_Stiff_TM1_ext) .* Stiff_Dim_TM1 ;
0449 Stiff_TM2_ext = (Stiff_TM2_ext_0 + p_Stiff_TM2_ext) .* Stiff_Dim_TM2 ;
0450
0451
0452
0453
0454
0455
0456
0457
0458 p_Stiff_TM1_act = [ p_StTM1_act11 p_StTM1_act12 p_StTM1_act13 p_StTM1_act14 p_StTM1_act15 p_StTM1_act16 ; ...
0459 p_StTM1_act21 p_StTM1_act22 p_StTM1_act23 p_StTM1_act24 p_StTM1_act25 p_StTM1_act26 ; ...
0460 p_StTM1_act31 p_StTM1_act32 p_StTM1_act33 p_StTM1_act34 p_StTM1_act35 p_StTM1_act36 ; ...
0461 p_StTM1_act41 p_StTM1_act42 p_StTM1_act43 p_StTM1_act44 p_StTM1_act45 p_StTM1_act46 ; ...
0462 p_StTM1_act51 p_StTM1_act52 p_StTM1_act53 p_StTM1_act54 p_StTM1_act55 p_StTM1_act56 ; ...
0463 p_StTM1_act61 p_StTM1_act62 p_StTM1_act63 p_StTM1_act64 p_StTM1_act65 p_StTM1_act66 ] ;
0464
0465 p_Stiff_TM2_act = [ p_StTM2_act11 p_StTM2_act12 p_StTM2_act13 p_StTM2_act14 p_StTM2_act15 p_StTM2_act16 ; ...
0466 p_StTM2_act21 p_StTM2_act22 p_StTM2_act23 p_StTM2_act24 p_StTM2_act25 p_StTM2_act26 ; ...
0467 p_StTM2_act31 p_StTM2_act32 p_StTM2_act33 p_StTM2_act34 p_StTM2_act35 p_StTM2_act36 ; ...
0468 p_StTM2_act41 p_StTM2_act42 p_StTM2_act43 p_StTM2_act44 p_StTM2_act45 p_StTM2_act46 ; ...
0469 p_StTM2_act51 p_StTM2_act52 p_StTM2_act53 p_StTM2_act54 p_StTM2_act55 p_StTM2_act56 ; ...
0470 p_StTM2_act61 p_StTM2_act62 p_StTM2_act63 p_StTM2_act64 p_StTM2_act65 p_StTM2_act66 ] ;
0471
0472 Stiff_TM1_act = (Stiff_TM1_act_0 + p_Stiff_TM1_act) .* Stiff_Dim_TM1 ;
0473 Stiff_TM2_act = (Stiff_TM2_act_0 + p_Stiff_TM2_act) .* Stiff_Dim_TM2 ;
0474
0475
0476
0477
0478
0479
0480 Ak23 = T_ftH1_2_fJtB * (Stiff_TM1_ext+ Stiff_TM1_act);
0481 Ak24 = T_ftH2_2_fJtB * (Stiff_TM2_ext+ Stiff_TM2_act);
0482 Ak53 = T_ftH1_2_fH1tT1 * (Stiff_TM1_ext+ Stiff_TM1_act);
0483 Ak64 = T_ftH2_2_fH2tT2 * (Stiff_TM2_ext+ Stiff_TM2_act);
0484
0485
0486 Ak = [zeros(6) eye(6) zeros(6) zeros(6) zeros(6) zeros(6) ;...
0487 zeros(6) zeros(6) Ak23 Ak24 zeros(6) zeros(6) ;...
0488 zeros(6) zeros(6) zeros(6) zeros(6) eye(6) zeros(6) ;...
0489 zeros(6) zeros(6) zeros(6) zeros(6) zeros(6) eye(6) ;...
0490 zeros(6) zeros(6) Ak53 zeros(6) zeros(6) zeros(6) ;...
0491 zeros(6) zeros(6) zeros(6) Ak64 zeros(6) zeros(6) ];...
0492
0493
0494
0495
0496
0497
0498
0499
0500
0501
0502
0503 Bku_mps = [ zeros(6) ;...
0504 T_ftB_2_fJtB ;...
0505 zeros(6) ;...
0506 zeros(6) ;...
0507 zeros(6) ;...
0508 zeros(6) ];
0509
0510
0511 Bku_susp = [zeros(6) zeros(6) ;...
0512 T_ftT1_2_fJtB T_ftT2_2_fJtB ;...
0513 zeros(6) zeros(6) ;...
0514 zeros(6) zeros(6) ;...
0515 T_ftT1_2_fH1tT1 zeros(6) ;...
0516 zeros(6) T_ftT2_2_fH2tT2 ];
0517
0518
0519
0520
0521
0522
0523
0524
0525
0526
0527
0528
0529
0530
0531 Bkn_sol = [zeros(6) ;...
0532 T_ftJ_2_fJtB ;...
0533 zeros(6) ;...
0534 zeros(6) ;...
0535 zeros(6) ;...
0536 zeros(6) ];
0537
0538
0539 Bkn_ifr = [zeros(6) ;...
0540 T_ftJ_2_fJtB ;...
0541 zeros(6) ;...
0542 zeros(6) ;...
0543 zeros(6) ;...
0544 zeros(6) ];
0545
0546
0547 Bkn_grav = [zeros(6) ;...
0548 T_ftJ_2_fJtB ;...
0549 zeros(6) ;...
0550 zeros(6) ;...
0551 T_ftJ_2_fH1tT1 ;...
0552 T_ftJ_2_fH2tT2 ];
0553
0554
0555 Bkn_TM = [zeros(6) zeros(6) ;...
0556 zeros(6) zeros(6) ;...
0557 zeros(6) zeros(6) ;...
0558 zeros(6) zeros(6) ;...
0559 T_ftH1_2_fH1tT1 zeros(6) ;...
0560 zeros(6) T_ftH2_2_fH2tT2];
0561
0562
0563 Bkn_TM_SC = [zeros(6) zeros(6) ;...
0564 T_ftH1_2_fJtB T_ftH2_2_fJtB ;...
0565 zeros(6) zeros(6) ;...
0566 zeros(6) zeros(6) ;...
0567 T_ftH1_2_fH1tT1 zeros(6) ;...
0568 zeros(6) T_ftH2_2_fH2tT2];
0569
0570
0571
0572
0573
0574
0575
0576
0577
0578
0579
0580
0581 C = [ zeros(3) eye(3) zeros(3,6) zeros(3,12) zeros(3,12) ; ...
0582 zeros(12,3) zeros(12,3) zeros(12,6) eye(12,12) zeros(12,12)];
0583
0584
0585
0586
0587
0588
0589
0590
0591
0592
0593
0594
0595 Du_mps = [ zeros(3,6) ;...
0596 zeros(12,6) ];
0597
0598
0599 Du_susp = [zeros(3,12) ;...
0600 zeros(12,12)];
0601
0602
0603
0604
0605
0606
0607
0608
0609
0610
0611
0612
0613
0614
0615
0616 Dn_sol = [zeros(3,6) ;...
0617 zeros(12,6) ];
0618
0619
0620
0621 Dn_ifr = [zeros(3,6) ;...
0622 zeros(12,6) ] ;
0623
0624
0625
0626 Dn_grav = [zeros(3,6) ;...
0627 zeros(12,6) ];
0628
0629
0630
0631 Dn_TM = [zeros(3,12) ;...
0632 zeros(12,12) ];
0633
0634
0635
0636 Dn_TM_SC = [zeros(3,12) ;...
0637 zeros(12,12)] ;
0638
0639
0640
0641
0642
0643
0644
0645
0646
0647 sys.name = 'LPF_Dynamics';
0648
0649 sys.amats = {Ak};
0650 sys.bmats = {Bku_mps Bku_susp Bkn_sol Bkn_ifr Bkn_TM Bkn_TM_SC Bkn_grav };
0651 sys.cmats = {C};
0652 sys.dmats = cell(1,7);
0653 sys.mmats = {M};
0654 sys.timestep = 0;
0655
0656 sys.ssnames = {'LPF_Dynamics'};
0657 sys.ssvarnames = ...
0658 {{'rBx_J' 'rBy_J' 'rBz_J' 'thetaB' 'etaB' 'phiB' 'rBx_J_dot' 'rBy_J_dot' 'rBz_J_dot' 'omegaBx' 'omegaBy' 'omegaBz' ...
0659 'r1x_H1' 'r1y_H1' 'r1z_H1' 'theta1' 'eta1' 'phi1' ...
0660 'r2x_H2' 'r2y_H2' 'r2z_H2' 'theta2' 'eta2' 'phi2' ...
0661 'r1x_H1_dot' 'r1y_H1_dot' 'r1z_H1_dot' 'omega1x' 'omega1y' 'omega1z'...
0662 'r2x_H2_dot' 'r2y_H2_dot' 'r2z_H2_dot' 'omega2x' 'omega2y' 'omega2z'}};
0663 sys.inputnames = {'F_mps' 'F_TMAct' 'Dist_F_solar' 'Dist_F_infrared' 'Dist_F_TM' 'Dist_F_TM_SC' 'Dist_F_gravity' };
0664
0665 sys.inputnames = {'F_mps' 'F_TMAct' 'Dist_F_solar' 'Dist_F_infrared' 'Dist_F_TM' 'Dist_F_TM_SC' 'Dist_F_gravity' };
0666 sys.inputvarnames = {...
0667 {'fx_mps' 'fy_mps' 'fz_mps' 'tx_mps' 'ty_mps' 'tz_mps'}...
0668 {'fx1_TMAct' 'fy1_TMAct' 'fz1_TMAct' 'tx1_TMAct' 'ty1_TMAct' 'tz1_TMAct'...
0669 'fx2_TMAct' 'fy2_TMAct' 'fz2_TMAct' 'tx2_TMAct' 'ty2_TMAct' 'tz2_TMAct'}...
0670 {'Dist_fx_solar' 'Dist_fy_solar' 'Dist_fz_solar' 'Dist_tx_solar' 'Dist_ty_solar' 'Dist_tz_solar'}...
0671 {'Dist_fx_infrared' 'Dist_fy_infrared' 'Dist_fz_infrared' 'Dist_tx_infrared' 'Dist_ty_infrared' 'Dist_tz_infrared'}...
0672 {'Dist_fx1_TM' 'Dist_fy1_TM' 'Dist_fz1_TM' 'Dist_tx1_TM' 'Dist_ty1_TM' 'Dist_tz1_TM' ...
0673 'Dist_fx2_TM' 'Dist_fy2_TM' 'Dist_fz2_TM' 'Dist_tx2_TM' 'Dist_ty2_TM' 'Dist_tz2_TM'}...
0674 {'Dist_fx1_TM_SC' 'Dist_fy1_TM_SC' 'Dist_fz1_TM_SC' 'Dist_tx1_TM_SC' 'Dist_ty1_TM_SC' 'Dist_tz1_TM_SC' ...
0675 'Dist_fx2_TM_SC' 'Dist_fy2_TM_SC' 'Dist_fz2_TM_SC' 'Dist_tx2_TM_SC' 'Dist_ty2_TM_SC' 'Dist_tz2_TM_SC'}...
0676 {'Dist_fx_gravity' 'Dist_fy_gravity' 'Dist_fz_gravity' 'Dist_tx_gravity' 'Dist_ty_gravity' 'Dist_tz_gravity'}...
0677 };
0678
0679 sys.outputnames = {'SC_TM_attitude'};
0680 sys.outputvarnames = {...
0681 {'thetaB' 'etaB' 'phiB'...
0682 'rx1_H1' 'ry1_H1' 'rz1_H1' 'theta1' 'eta1' 'phi1' ...
0683 'rx2_H2' 'ry2_H2' 'rz2_H2' 'theta2' 'eta2' 'phi2'} ...
0684 };
0685 end