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