Home > classes > @ssm > model_LPF_Dynamics.m

model_LPF_Dynamics

PURPOSE ^

defines the LPF dynamic system

SYNOPSIS ^

function [sys,VERSION] = model_LPF_Dynamics

DESCRIPTION ^

 defines the LPF dynamic system
 ONLY THE CONSTRUCTOR SHOULD CALL THIS PRIVATE FUNCTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 DESCRIPTION: model_lpf_dyn_params defines the LPF
 dynamic system

 CALL: [sys, VERSION] = model_lpf_dyn_params 
                         
 

 VERSION: $Id: $

 HISTORY: 01-04-2008 M Weyrich
 23-04-2008 A Grynagier
 
 geometric params :12
 working point params: 24
 mass and inertia params :21
 stiffness params (same params for both test masses, 36act+36ext: 72
 ==> 129 params
 params are additive, central value 0, not unitless yet!
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 function [sys,VERSION] = model_LPF_Dynamics
0002 % defines the LPF dynamic system
0003 % ONLY THE CONSTRUCTOR SHOULD CALL THIS PRIVATE FUNCTION
0004 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0005 %
0006 % DESCRIPTION: model_lpf_dyn_params defines the LPF
0007 % dynamic system
0008 %
0009 % CALL: [sys, VERSION] = model_lpf_dyn_params
0010 %
0011 %
0012 %
0013 % VERSION: $Id: $
0014 %
0015 % HISTORY: 01-04-2008 M Weyrich
0016 % 23-04-2008 A Grynagier
0017 %
0018 % geometric params :12
0019 % working point params: 24
0020 % mass and inertia params :21
0021 % stiffness params (same params for both test masses, 36act+36ext: 72
0022 % ==> 129 params
0023 % params are additive, central value 0, not unitless yet!
0024 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0025 VERSION = '$Id:$';
0026 utils.helper.msg(utils.const.msg.MNAME, ['running ', mfilename]);
0027 
0028 %% creating ssm
0029 sys = ssm;
0030 
0031 % parameters in subsystem
0032 sys.paramnames = ...
0033     {...%---------------geometric params--------------------------------------------             
0034     'p_rH1x_B'  'p_rH1y_B'  'p_rH1z_B'   'p_thetaH1'  'p_etaH1'  'p_phiH1' ... % B->H1
0035     'p_rH2x_B'  'p_rH2y_B'  'p_rH2z_B'   'p_thetaH2'  'p_etaH2'  'p_phiH2' ... % B->H2                                 
0036     ...%---------------x0 working point params--------------------------------------------- 
0037     'p_thetaB'  'p_etaB'  'p_phiB'                                    ... % J->B(alphaB=alphaM) x0 states SC att, pos never used
0038     'p_omegaBx' 'p_omegaBy' 'p_omegaBz'                               ... %                     x0 states SC rates 
0039     'p_r1x_H1' 'p_r1y_H1' 'p_r1z_H1'  'p_theta1'  'p_eta1'  'p_phi1'  ... % H1->T1              x0 states TM1 pos&att 
0040     'p_r2x_H2' 'p_r2y_H2' 'p_r2z_H2'  'p_theta2'  'p_eta2'  'p_phi2'  ... % H2->T2              x0 states TM2 pos&att
0041     'p_omega1x' 'p_omega1y' 'p_omega1z'                               ... % x0 states TM1 vel&rates
0042     'p_omega2x' 'p_omega2y' 'p_omega2z'                               ... % x0 states TM2 vel&rates
0043     ...%--------------- mass & inertia params----------------------------------------
0044     'p_m_SC'  'p_Ixx_SC'   'p_Iyy_SC'    'p_Izz_SC'   'p_Ixy_SC'   'p_Ixz_SC'   'p_Iyz_SC'   ... %I_SC_M inertia SC in M
0045     'p_m_TM1' 'p_Ixx_TM1'  'p_Iyy_TM1'   'p_Izz_TM1'  'p_Ixy_TM1'  'p_Ixz_TM1'  'p_Iyz_TM1'  ... %I_TM1 inertia TM1
0046     'p_m_TM2' 'p_Ixx_TM2'  'p_Iyy_TM2'   'p_Izz_TM2'  'p_Ixy_TM2'  'p_Ixz_TM2'  'p_Iyz_TM2'  ... %I_TM2 inertia TM2
0047     ...%-----------------ext stiffness params TM1-----------------------------------------------------
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     ...%-----------------act stiffness params TM1-----------------------------------------------------
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     ...%-----------------ext stiffness params TM2-----------------------------------------------------
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     ...%-----------------act stiffness params TM2-----------------------------------------------------
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     [ ...%----------geometric params--------------------------------------------
0079       0  0  0   0  0  0    ... % B->H1
0080       0  0  0   0  0  0    ... % B->H2
0081      ...%---------------x0 working point params---------------------------------------- 
0082       0  0  0              ... % J->B(alphaB=alphaM) x0 states SC att, pos never used      
0083       0  0  0              ... %                     x0 states SC rates
0084       0  0  0   0  0  0    ... % H1->T1              x0 states TM1 pos&att
0085       0  0  0   0  0  0    ... % H2->T2              x0 states TM2 pos&att
0086       0  0  0              ... % x0 states TM1 vel&rates
0087       0  0  0              ... % x0 states TM2 vel&rates
0088      ...%--------------- mass & inertia params------------------------------------
0089       0  0  0  0  0  0  0   ... %I_SC_M inertia SC in M                 
0090       0  0  0  0  0  0  0   ... %I_TM1 inertia TM1                 
0091       0  0  0  0  0  0  0   ... %I_TM2 inertia TM2                       
0092      ...%-----------------ext stiffness params TM1 -----------------------------------------------------
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      ...%-----------------act stiffness params TM1-----------------------------------------------------
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       ...%-----------------ext stiffness params TM2 -----------------------------------------------------
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      ...%-----------------act stiffness params TM2-----------------------------------------------------
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    [ ...%----------geometric params--------------------------------------------
0125       0  0  0   0  0  0    ... % B->H1
0126       0  0  0   0  0  0    ... % B->H2
0127      ...%---------------x0 working point params---------------------------------------- 
0128       0  0  0              ... % J->B(alphaB=alphaM) x0 states SC att, pos never used      
0129       0  0  0              ... %                     x0 states SC rates
0130       0  0  0   0  0  0    ... % H1->T1              x0 states TM1 pos&att
0131       0  0  0   0  0  0    ... % H2->T2              x0 states TM2 pos&att
0132       0  0  0              ... % x0 states TM1 vel&rates
0133       0  0  0              ... % x0 states TM2 vel&rates
0134      ...%--------------- mass & inertia params------------------------------------
0135       0  0  0  0  0  0  0   ... %I_SC_M inertia SC in M                 
0136       0  0  0  0  0  0  0   ... %I_TM1 inertia TM1                 
0137       0  0  0  0  0  0  0   ... %I_TM2 inertia TM2                       
0138      ...%-----------------ext stiffness params TM1 -----------------------------------------------------
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      ...%-----------------act stiffness params TM1-----------------------------------------------------
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       ...%-----------------ext stiffness params TM2 -----------------------------------------------------
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      ...%-----------------act stiffness params TM2-----------------------------------------------------
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 %% declaring symbolic parameters
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 %%%%%%%%%%%%%%%%%%%%%%%%%% START BUILDING PARAMETRIC SUBSYTEM %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0177 %  ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
0178 %% Numerical Definitions
0179 %==============================
0180 L_TM1 = 0.046;   % Edge lengths [m] of test mass cubes
0181 L_TM2 = 0.046;   % Edge lengths [m] of test mass cubes
0182 
0183 rB_M = [9.4e-3; 16.4e-3; 478.9e-3]; %position of COM
0184 rH1_M = [0.376/2; 0; 0.6093]; %nominal position of housing
0185 rH2_M = [-0.376/2; 0; 0.6093];
0186 alphaH1_0 = [0; 0; 0];
0187 alphaH2_0 = [0; 0; 0];
0188 
0189 %working point SC
0190 alphaB_0 = [0; 0; 0]; 
0191 omegaB_0 = [0; 0; 0]; 
0192 
0193 %working point TM
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 % mass & inertia
0202 m_SC_0 = 415.2;
0203 I_SC_M_0 = [ 212.9       0         0       ;...  % Moments and cross products of inertia [kg m^2] of the
0204              0           214.2     0       ;...  % S/C w.r.t. mechanical reference frame (M) !!!
0205              0           0         197.0  ];
0206 m_TM1_0 = 1.96;
0207 I_TM1_0 = [6.912e-04   0             0         ;... % Moment of inertia [kg m^2] of test mass 1
0208            0             6.912e-04   0         ;... % w.r.t. the CoM.
0209            0             0           6.912e-04 ];
0210 m_TM2_0 = 1.96;
0211 I_TM2_0 = [ 6.912e-04   0             0         ;... % Moment of inertia [kg m^2] of test mass 1
0212             0             6.912e-04   0         ;... % w.r.t. the CoM.
0213             0             0           6.912e-04 ];
0214 
0215 % Stiffness
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 %% "Working point"  & geometric parameter vector definitions
0242 %================================================================
0243 %working point SC
0244 alphaB = alphaB_0 + [p_thetaB ; p_etaB; p_phiB] ;
0245 omegaB = omegaB_0 + [p_omegaBx; p_omegaBy; p_omegaBz] ; 
0246 
0247 %working point TM
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 %other geometric vectors (trans&rot. offsets between frames)
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 %% Transformation Matrices for transformation of coordinate vectors between the different systems
0262 %================================================================================================
0263 T_B_J = ssm.T_ZYX_rot(alphaB);   T_J_B = T_B_J.'; % T_M_J is the same
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 %derived tranformations:
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 %% Composed Transformation matrices T of Force&Torque to corresponding frames
0278 %==========================================================================
0279 % Transformation matrix of force & torque on TM1/2 in housing frames to force on SC in inertial frame & torque on SC in body frame: (e.g. stiffness)
0280 % Note: force & torque on housing are of opposite sign to force & torque on TM! (due to recoil)
0281 % Note: force on a TM with offset to nominal point will result in extra torque on H!
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 % Transformation matrix of force & torque on TM1/2 in T- frames to force on SC in inertial frame & torque on SC in body frame: (e.g. F_susp)
0289 % Note: force & torque on housing are of opposite sign to force & torque on TM! (due to recoil)
0290 % Note: force on a TM with offset to nominal point will result in extra torque on H!
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 % Transformation matrix of force & torque on B in B-frame  to force in J-frame & torque in B-frame: (e.g. F_mps)
0297 T_ftB_2_fJtB = [ T_J_B        zeros(3,3) ;...   %force 
0298                  zeros(3,3)   eye(3,3)    ];     %torque
0299 % Transformation matrix of force&torque in J on B to force in J (on B) and torque in B (on B): (e.g. F_dist_B)
0300 T_ftJ_2_fJtB = [  eye(3,3)     zeros(3,3)  ;... %force
0301                   zeros(3,3)   T_B_J     ];   %torque
0302               
0303 % Transformation matrix of forces & torque on TM1/2 in H-frame to forces on TM1/2 in H-frame and torque in TM-frame: (e.g. stiffness)
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 % Transformation matrix of forces & torque on TM1/2 in T-frame to forces on TM1/2 in H-frame and torque in TM-frame: (e.g. F_susp)
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 % Transformation matrix of force&torque in J to f in H, t in T
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 %% Mass Matrix M (36x36)
0325 %============================================================
0326 %Construct Mass Matrix M
0327 %============================================================
0328 % SPACECRAFT
0329 m_SC = m_SC_0 + p_m_SC  ;    % S/C mass [kg]
0330 M_SC = m_SC*eye(3);
0331 
0332 % Apply parallel axis theorem to define MoI w.r.t. B and calculate the inverse
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;  %with respect to B
0340 
0341 % TEST MASSES
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;      %with respect to COM
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;     %with respect to COM
0356 
0357 %--------------------------------------------------------------------------
0358 % ASSEMBLING OF MASS MATRIX
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 %% Stiffness model
0407 %======================================================================================
0408 % Matrices used to dimensionalize the stiffness matrices (multiplication with coordinate vector gives forces and torques):
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 % ******************* EXTERNAL STIFFNESS ****************************
0425 % Stiffness caused by "external" (no electrostatic actuation) effects:
0426 %*****************************************************************************
0427 % Differential stiffness: abs(w2^2-w1^2) < 1.1*6.5e-6, since it is assumed
0428 % that actuation stiffness is negative, acting on x2 only in science mode,
0429 % 0.065e-6 is subtracted from the external stiffness w1.
0430 %
0431 % Note: Negative sign means negative stiffness is modelled!
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 %******************* ACTUATION STIFFNESS ********************************
0452 % Stiffness caused by electrostatic actuation
0453 %**********************************************************************************
0454 % Note: Negative sign means negative stiffness is modelled!
0455 % Note: worst case stiffness matrix is used
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 %% Matrix A (36x36)
0476 %=============================================================
0477 %Construct Matrix Ak, then A_LTP = inv(M)*Ak
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 %     S/Cpos+att S/Cvel+rate TM1 pos+att TM2 pos+att TM1vel+rate TM2vel+rate
0486 Ak = [zeros(6)   eye(6)      zeros(6)    zeros(6)    zeros(6)    zeros(6)   ;... % (S/C pos + att)_dot
0487       zeros(6)   zeros(6)    Ak23        Ak24        zeros(6)    zeros(6)   ;... % (S/C vel + rate)_dot
0488       zeros(6)   zeros(6)    zeros(6)    zeros(6)    eye(6)      zeros(6)   ;... % (TM1 pos + att)_dot
0489       zeros(6)   zeros(6)    zeros(6)    zeros(6)    zeros(6)    eye(6)     ;... % (TM2 pos + att)_dot
0490       zeros(6)   zeros(6)    Ak53        zeros(6)    zeros(6)    zeros(6)   ;... % (TM1 vel + rate)_dot    
0491       zeros(6)   zeros(6)    zeros(6)    Ak64        zeros(6)    zeros(6)   ];...   % (TM2 vel + rate)_dot
0492       
0493 %% Matrix Bu (36x18)
0494 %=============================================================
0495 %Construct Matrix Bk, then B_LTP = inv(M)*Bk
0496 %=============================================================
0497 % Inputs u:
0498 %   1- 6: Forces & Torques from Micropropulsion System on B in B
0499 %   7-18: Forces & Torques from Electrostatic Suspension System on TM in H-frame
0500 %--------------------------------------------------------------------------------------------------------------------------
0501 
0502 %            F_mps
0503 Bku_mps = [ zeros(6)      ;...  % (S/C pos + att)_dot
0504             T_ftB_2_fJtB  ;...  % (S/C vel + rate)_dot
0505             zeros(6)      ;...  % (TM1 pos + att)_dot
0506             zeros(6)      ;...  % (TM2 pos + att)_dot
0507             zeros(6)      ;...  % (TM1 vel + rate)_dot     
0508             zeros(6)      ];    % (TM2 vel + rate)_dot
0509 
0510 %             F_susp
0511 Bku_susp = [zeros(6)         zeros(6)        ;...  % (S/C pos + att)_dot
0512             T_ftT1_2_fJtB    T_ftT2_2_fJtB   ;...  % (S/C vel + rate)_dot
0513             zeros(6)         zeros(6)        ;...  % (TM1 pos + att)_dot
0514             zeros(6)         zeros(6)        ;...  % (TM2 pos + att)_dot
0515             T_ftT1_2_fH1tT1  zeros(6)        ;...  % (TM1 vel + rate)_dot 
0516             zeros(6)         T_ftT2_2_fH2tT2 ]; % (TM2 vel + rate)_dot
0517          
0518 %% Matrices Bn1,Bn2,... (36xn1,36xn2,...)
0519 %===============================================================
0520 % Construct Matrix Ek for process Disturbance/Noise Inputs, then E_LTP = inv(M)*Ek
0521 %===============================================================
0522 % Noise Inputs w:
0523 % n1  solar pressure noise
0524 % n2  infrared noise
0525 % n3  gravity f&t (sun, moon, earth)
0526 % n4  f&t noise on TM 10% from noise filter NOISE_TM (N_TM), only on TM, no reaction force
0527 % n5  f&t noise on TM 90% from noise filter NOISE_TM (N_TM)
0528 %-------------------------------------------------------------------------------------------------
0529 
0530 %          solar pressure from noise filter N_sol
0531 Bkn_sol = [zeros(6)     ;...  % (S/C pos + att)_dot
0532            T_ftJ_2_fJtB ;...  % (S/C vel + rate)_dot 
0533            zeros(6)     ;...  % (TM1 pos + att)_dot
0534            zeros(6)     ;...  % (TM2 pos + att)_dot
0535            zeros(6)     ;...  % (TM1 vel + rate)_dot    
0536            zeros(6)     ];   % (TM2 vel + rate)_dot
0537 
0538 %          infrared noise from noise filter N_ifr
0539 Bkn_ifr = [zeros(6)     ;...  % (S/C pos + att)_dot
0540            T_ftJ_2_fJtB ;...  % (S/C vel + rate)_dot 
0541            zeros(6)     ;...  % (TM1 pos + att)_dot
0542            zeros(6)     ;...  % (TM2 pos + att)_dot
0543            zeros(6)     ;...  % (TM1 vel + rate)_dot    
0544            zeros(6)     ];   % (TM2 vel + rate)_dot
0545 
0546 %           gravity f&t (sun, moon, earth)
0547 Bkn_grav = [zeros(6)     ;...  % (S/C pos + att)_dot
0548             T_ftJ_2_fJtB ;...  % (S/C vel + rate)_dot 
0549             zeros(6)     ;...  % (TM1 pos + att)_dot
0550             zeros(6)     ;...  % (TM2 pos + att)_dot
0551             T_ftJ_2_fH1tT1 ;...  % (TM1 vel + rate)_dot    
0552             T_ftJ_2_fH2tT2 ];   % (TM2 vel + rate)_dot
0553 
0554 %          f&t noise on TM 10% from noise filter NOISE_TM (N_TM), only on TM, no reaction force
0555 Bkn_TM =                [zeros(6)         zeros(6) ;...         % (S/C pos + att)_dot
0556                          zeros(6)         zeros(6) ;...         % (S/C vel + rate)_dot
0557                          zeros(6)         zeros(6) ;...         % (TM1 pos + att)_dot
0558                          zeros(6)         zeros(6) ;...         % (TM2 pos + att)_dot
0559                          T_ftH1_2_fH1tT1  zeros(6) ;...         % (TM1 vel + rate)_dot
0560                          zeros(6)         T_ftH2_2_fH2tT2];  % (TM2 vel + rate)_dot
0561 
0562 %            f&t noise on TM  90% from noise filter NOISE_TM_SC (N_TM)
0563 Bkn_TM_SC =                [zeros(6)         zeros(6)       ;... % (S/C pos + att)_dot
0564                             T_ftH1_2_fJtB    T_ftH2_2_fJtB  ;... % (S/C vel + rate)_dot
0565                             zeros(6)         zeros(6)       ;... % (TM1 pos + att)_dot
0566                             zeros(6)         zeros(6)       ;... % (TM2 pos + att)_dot
0567                             T_ftH1_2_fH1tT1  zeros(6)       ;... % (TM1 vel + rate)_dot
0568                             zeros(6)         T_ftH2_2_fH2tT2]; % (TM2 vel + rate)_dot
0569 
0570  
0571 %% Matrix C (21x36)
0572 %================================================================
0573 % Construct Matrix C
0574 %=================================================================
0575 % Outputs:
0576 %   1- 3: S/C Attitude (Measurements)
0577 %   4-15: TM  Position & Attitude (Measurements)
0578 %-------------------------------------------------------------------------------------
0579 
0580 %     SC_pos       SC_att       SC_vel+rates    TM1_pos+att_TM2   TM1_vel+rate_TM2
0581 C = [ zeros(3)     eye(3)       zeros(3,6)      zeros(3,12)       zeros(3,12) ; ... %  1- 3: S/C att. meas
0582       zeros(12,3)  zeros(12,3)  zeros(12,6)     eye(12,12)        zeros(12,12)];    %  4-15: TM  pos. + att. meas.
0583            
0584 
0585 %% Matrix Du (21x18)
0586 %=================================================================
0587 % Construct Matrix D
0588 %=================================================================
0589 % Inputs u:
0590 %   1- 6: Forces & Torques from Micropropulsion System on B in B
0591 %   7-18: Forces & Torques from Electrostatic Suspension System on TM in H-frame
0592 %--------------------------------------------------------------------------------------------------------------------------
0593 
0594 %           F_mps
0595 Du_mps = [ zeros(3,6)  ;...  %  1- 3: S/C att. meas 
0596            zeros(12,6) ];  %  4-15: TM pos. + att. meas
0597 
0598 %           F_susp
0599 Du_susp = [zeros(3,12) ;... %  1- 3: S/C att. meas 
0600            zeros(12,12)]; %  4-15: TM pos. + att. meas
0601            
0602        
0603 %% Matrices Dn1,Dn2,... (21xn1,21xn2,...)
0604 %===============================================================
0605 % Construct Matrix F for observation Disturbance/Noise Inputs
0606 %===============================================================
0607 % Noise Inputs w:
0608 % n1  solar pressure noise
0609 % n2  infrared noise
0610 % n3  gravity f&t (sun, moon, earth)
0611 % n4  f&t noise on TM 10% from noise filter NOISE_TM (N_TM), only on TM, no reaction force
0612 % n5  f&t noise on TM 90% from noise filter NOISE_TM (N_TM)
0613 %--------------------------------------------------------------------------
0614 
0615 %         solar pressure noise
0616 Dn_sol = [zeros(3,6)    ;... %  1- 3: S/C att. meas.
0617           zeros(12,6)   ]; %  4-15: TM  pos. + att. meas.
0618           
0619                       
0620 %         infrared noise from noise filter N_ifr
0621 Dn_ifr = [zeros(3,6)     ;... %  1- 3: S/C att. meas.
0622           zeros(12,6) ]  ; %  4-15: TM  pos. + att. meas.
0623           
0624             
0625 %           gravity f&t (sun, moon, earth)
0626 Dn_grav = [zeros(3,6)    ;... %  1- 3: S/C att. meas.
0627            zeros(12,6)  ]; %  4-15: TM  pos. + att. meas.
0628            
0629                
0630 %         f&t noise on TM 10% from noise filter NOISE_TM (N_TM), only on TM, no reaction force
0631 Dn_TM =  [zeros(3,12)    ;...%  1- 3: S/C att. meas.
0632           zeros(12,12)  ];%  4-15: TM  pos. + att. meas.
0633          
0634              
0635 %             f&t noise on TM 90% from noise filter NOISE_TM (N_TM)
0636 Dn_TM_SC =  [zeros(3,12)    ;...%  1- 3: S/C att. meas.
0637              zeros(12,12)]  ;%  4-15: TM  pos. + att. meas.
0638              
0639           
0640 %%%%%%%%%%%%%%%%%%%%%%%%%% END BUILDING PARAMETRIC SUBSYTEM %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0641 %  +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
0642 
0643 
0644 %% ADDING MATRICES TO SUBSYSTEM SAVING STRUCTURE
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

Generated on Wed 27-Aug-2008 13:30:29 by m2html © 2003