Home > classes > @ssm > model_LPF_Dynamics_noparams.m

model_LPF_Dynamics_noparams

PURPOSE ^

defines the LPF dynamic system without params (substituted with nominal

SYNOPSIS ^

function [sys,VERSION] = model_LPF_Dynamics_noparams

DESCRIPTION ^

 defines the LPF dynamic system without params (substituted with nominal
 value)
 ONLY THE CONSTRUCTOR SHOULD CALL THIS PRIVATE FUNCTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 DESCRIPTION: model_LPF_Dynamics_noparams defines the LPF
 dynamic system

 CALL: [sys, VERSION] = model_LPF_Dynamics_noparams 
                         
 

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

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