|
|
%--------------------------------------------------------------%
|
|
|
%---------InitialData File for simulink model "***.mdl"--------%
|
|
|
%
|
|
|
clear all;
|
|
|
disp(' Initial Data for model sat9.mdl ');
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
% 1.卫星的轨道参数及相关的地球参数 %
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
R_s = 6371230 + 705000; %圆轨道，卫星半长轴，单位:米；
|
|
|
Mu = 3.986015e14; %地心引力常数;
|
|
|
T0 = 2 * pi * sqrt(R_s ^ 3 / Mu); %轨道周期；单位:秒;根据公式T0=2*pi*sqrt(R^3/Mu）;
|
|
|
Omega0 = 2 * pi / T0; %轨道角速度，rad/s ；
|
|
|
|
|
|
% load B.dat; %装入地磁场的离线计算值
|
|
|
T = 0.2;
|
|
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
% 2.卫星本体惯量及帆板挠性参数 %
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
%---------------------Satellite parameters----------------%
|
|
|
m_sat = 3100; %整星质量 kg
|
|
|
%-----Inertia tensor of sat------%
|
|
|
Ix = 8235.3;
|
|
|
Iy = 5973.7;
|
|
|
Iz = 11733.8;
|
|
|
Ixy = -59.2;
|
|
|
Iyz = -40.8;
|
|
|
Ixz = -483.7;
|
|
|
I_sas = [Ix, -Ixy, -Ixz; -Ixy, Iy, -Iyz; -Ixz, -Iyz, Iz]; %帆板展开时整星相对于Ob的转动惯量kg.m2(sat affix spread)
|
|
|
I_ben = [2188.4, 59.0, 481.8; 59.0, 5856.0, 39.5; 481.8, 39.5, 5677.0]; % 卫星本体（不含帆板）相对于Ob的惯量kg.m2
|
|
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
% 展开状态 %
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
%-----------------帆板系原点在本体系中位置矢量--------------------%
|
|
|
r_p1 = [2.96; 1037; 0] * 0.001; %+Y帆板(m)
|
|
|
r_p2 = [2.96; -1037; 0] * 0.001; %-Y帆板(m)
|
|
|
Rp_mat1 = vec2mat(r_p1);
|
|
|
Rp_mat2 = vec2mat(r_p2);
|
|
|
|
|
|
Omega_1 = diag([0.0901, 0.5013, 0.5514, 0.5881, 1.5297, 1.8149, 2.9137, 3.2012, 4.6741, 4.8323], 0) * 2 * pi; %附件振动耦合矩阵(10阶)---两个帆板通用
|
|
|
Omega_2 = Omega_1;
|
|
|
xi_1 = diag([0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01]); %挠性模态阻尼系数（阻尼比）---两个帆板通用
|
|
|
xi_2 = xi_1;
|
|
|
|
|
|
B_tran1 = [-2.5591e-11 4.0962e-10 -6.5798; ... % +Y帆板太阳电池阵局部坐标系下的与转角无关的平动耦合系数,挠性附件振动对附件平动的耦合系数，通过有限元模型计算得到
|
|
|
7.2553 -0.0053 5.1308e-8; ...
|
|
|
2.4257e-7 -2.2529e-9 -3.5996; ...
|
|
|
-1.0111e-6 5.5133e-10 -0.4947; ...
|
|
|
-6.5783e-9 -3.6992e-9 1.9825; ...
|
|
|
-1.0444e-7 6.5840e-10 -0.0989; ...
|
|
|
-6.8739e-9 -4.2777e-9 1.1973; ...
|
|
|
-6.6231e-8 9.0168e-10 -0.1045; ...
|
|
|
4.4429e-9 9.7248e-9 -1.6161; ...
|
|
|
-5.5866e-8 1.7895e-8 -0.1009];
|
|
|
B_tran2 = [-2.5591e-11 -4.0962e-10 6.5798; ... % -Y帆板太阳电池阵局部坐标系下的与转角无关的平动耦合系数,挠性附件振动对附件平动的耦合系数，通过有限元模型计算得到
|
|
|
7.2553 0.0053 -5.1308e-8; ...
|
|
|
2.4257e-7 2.2529e-9 3.5996; ...
|
|
|
-1.0111e-6 -5.5133e-10 0.4947; ...
|
|
|
-6.5783e-9 3.6992e-9 -1.9825; ...
|
|
|
-1.0444e-7 -6.5840e-10 0.0989; ...
|
|
|
-6.8739e-9 4.2777e-9 -1.1973; ...
|
|
|
-6.6231e-8 -9.0168e-10 0.1045; ...
|
|
|
4.4429e-9 -9.7248e-9 1.6161; ...
|
|
|
-5.5866e-8 -1.7895e-8 -0.1009]; ;
|
|
|
B_rot1 = [-46.6574 0.0621 2.3823e-10; ... % +Y帆板太阳电池阵局部坐标系下的与转角无关的转动耦合系数,挠性附件振动对附件转动的耦合系数，通过有限元模型计算得到
|
|
|
1.1612e-7 6.9951e-7 -47.6960; ...
|
|
|
-7.8112 -0.6626 -1.5925e-6; ...
|
|
|
-1.0435 4.6264 6.6374e-6; ...
|
|
|
2.6541 0.0963 4.1253e-8; ...
|
|
|
-0.1219 1.7069 6.4940e-7; ...
|
|
|
1.1634 0.1011 3.7618e-8; ...
|
|
|
-0.0970 0.9859 3.5322e-7; ...
|
|
|
-1.2392 -0.0403 -1.5143e-8; ...
|
|
|
-0.0759 0.5447 1.9007e-7];
|
|
|
B_rot2 = [-46.6574 -0.0621 -2.3823e-10; ... % -Y帆板太阳电池阵局部坐标系下的与转角无关的转动耦合系数,挠性附件振动对附件转动的耦合系数，通过有限元模型计算得到
|
|
|
1.1612e-7 -6.9951e-7 47.6960; ...
|
|
|
-7.8112 0.6626 1.5925e-6; ...
|
|
|
-1.0435 -4.6264 -6.6374e-6; ...
|
|
|
2.6541 -0.0963 -4.1253e-8; ...
|
|
|
-0.1219 -1.7069 -6.4940e-7; ...
|
|
|
1.1634 -0.1011 -3.7618e-8; ...
|
|
|
-0.0970 -0.9859 -3.5322e-7; ...
|
|
|
-1.2392 0.0403 1.5143e-8; ...
|
|
|
-0.0759 -0.5447 -1.9007e-7];
|
|
|
I_a1 = B_rot1' * B_rot1; % +Y帆板相对于安装点Oa的转动惯量
|
|
|
I_a2 = B_rot2' * B_rot2; % -Y帆板相对于安装点Oa的转动惯量
|
|
|
P1 = [0; 0; 0]; % +Y帆板对于安装点Oa点的一阶惯量矩（静矩），通过有限元模型计算得到
|
|
|
P2 = [0; 0; 0]; % -Y帆板对于安装点Oa点的一阶惯量矩（静矩），通过有限元模型计算得到
|
|
|
|
|
|
%----------------save parameters in mat form----------------%
|
|
|
save SatPara r_p1 B_rot1 B_tran1 I_a1 Rp_mat1 P1 I_ben ...
|
|
|
r_p2 B_rot2 B_tran2 I_a2 Rp_mat2 P2;
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
% 3.卫星敏感器及姿态确定参数 %
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
%--------------------------------%
|
|
|
% 敏感器性能指标 %
|
|
|
%--------------------------------%
|
|
|
Wgmax = [2; 2; 2] * pi / 180; %速率陀螺最大测量范围,2度/秒
|
|
|
gyro_bias_initial = [1, 1, 1]' * pi / 180/3600; %常值偏移 rad/s
|
|
|
var_gyro = (0.005/3600 * 2 * pi / 360) ^ 2;
|
|
|
sigmav = 0.005/3600 * 2 * pi / 360; % %角速率随机游走系数,等价于输入白噪声的标准差
|
|
|
var_gyro_bias = (0.005/3600 * 2 * pi / 360 * 1e-3) ^ 2;
|
|
|
sigmau = 0.005/3600 * 2 * pi / 360 * 1e-3; % %角度随机游走系数,等价于输入白噪声的标准差
|
|
|
gyro_white_noise_seed = fix(1000 * rand(3, 1)); %陀螺白噪声的随机数种子
|
|
|
star_white_noise_seed = fix(1000 * rand(4, 1)); %星敏感器白噪声的随机数种子
|
|
|
sample_time = 0.5; %采用时间与星载计算机采用时间相同
|
|
|
%--------------------------------%
|
|
|
% Kalman滤波器参数 %
|
|
|
%--------------------------------%
|
|
|
K = [diag([0.0143 0.0143 0.0143]); diag([-0.0008 -0.0008 -0.0008])];
|
|
|
%%Q
|
|
|
Q = [0.25 * 0.6/3600 * 2 * pi / 360 * eye(3), 0 * eye(3); 0 * eye(3), 0.6/3600 * 2 * pi / 360 * 1e-3 * eye(3)];
|
|
|
%%H
|
|
|
H = [eye(3), zeros(3)];
|
|
|
%%G
|
|
|
G = [-0.5 * eye(3), zeros(3, 3); zeros(3, 3), eye(3, 3)];
|
|
|
%%R
|
|
|
R = 35/3600 * 2 * pi / 360 * eye(3);
|
|
|
%%P0
|
|
|
P0 = [0.0004 * eye(3), 0 * eye(3); 0 * eye(3), 0.0009 * eye(3)];
|
|
|
%%s_time
|
|
|
s_time = 0.5;
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
% 4.卫星执行机构及控制器参数 %
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
%--------------------------------%
|
|
|
% 执行机构参数 %
|
|
|
%--------------------------------%
|
|
|
%=============反作用飞轮参数===========
|
|
|
Jwheel = 0.106; %飞轮转动惯量；kg.m^2
|
|
|
Tmotormax = 0.2; % 最大反作用力矩，Nm
|
|
|
Kr = 0.015;
|
|
|
Tr = 0.1;
|
|
|
Usaturate = 42; %饱和电压;
|
|
|
Wsaturate = 3500 * 30 / pi; %飞轮的饱和转速，最大3500rpm；
|
|
|
Hsaturate = 25; %飞轮的饱和角动量，最大25Nms
|
|
|
|
|
|
%===============磁力矩器参数========
|
|
|
% Mags = 100; %磁力矩器的最大磁矩A.m2
|
|
|
|
|
|
%飞轮安装矩阵
|
|
|
Dwheel = [1 0 0 cos(54 * pi / 180) cos(54 * pi / 180); ...
|
|
|
0 1 0 cos(55.1 * pi / 180) -cos(55.1 * pi / 180); ...
|
|
|
0 0 1 cos(55.1 * pi / 180) cos(55.1 * pi / 180)];
|
|
|
|
|
|
%------------飞轮角动量------------%
|
|
|
HmA = 0; %正常稳态运行时X轴A轮的标称角动量;
|
|
|
HmB = 0; %正常稳态运行时Y轴B轮的标称角动量;
|
|
|
HmC = 0; %正常稳态运行时Z轴C轮的标称角动量;
|
|
|
HmD = 0; %正常稳态运行时斜装S1轮的标称角动量;
|
|
|
HmE = 0; %正常稳态运行时斜装S2轮的标称角动量;
|
|
|
|
|
|
%------------磁卸载参数------------%
|
|
|
% kxunload = 4;
|
|
|
% kyunload = 4;%
|
|
|
% kzunload = 4;
|
|
|
|
|
|
Qerror = 1/30;
|
|
|
%---------卫星三轴控制参数---------%
|
|
|
% PID=[900,0,5600,760,0,2000,900,0,6000];
|
|
|
wn = 1 * 2 * pi; %带宽wn
|
|
|
k = wn ^ 2;
|
|
|
c = 2 * 0.9 * wn; %阻尼0.9
|
|
|
Kx = [Ix * k, Ix * k * 0.001, Ix * c]; %[kp;ki;kd]
|
|
|
Ky = [Iy * k, Ix * k * 0.001, Iy * c];
|
|
|
Kz = [Iz * k, Ix * k * 0.001, Iz * c];
|
|
|
Kpx = Kx(1);
|
|
|
Kpix = Kx(2);
|
|
|
Kwx = Kx(3);
|
|
|
Kpy = Ky(1);
|
|
|
Kpiy = Ky(2);
|
|
|
Kwy = Ky(3);
|
|
|
Kpz = Kz(1);
|
|
|
Kpiz = Kz(2);
|
|
|
Kwz = Kz(3);
|
|
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
% 5.卫星初始姿态参数 %
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
%卫星初始的姿态角度，姿态角速度
|
|
|
phi0 = 0.01;
|
|
|
theta0 = 0;
|
|
|
psi0 = -0.01;
|
|
|
dphi0 = 0;
|
|
|
dtheta0 = 0;
|
|
|
dpsi0 = 0;
|
|
|
|
|
|
An = [phi0; theta0; psi0] * pi / 180;
|
|
|
Av = [dphi0; dtheta0; dpsi0] * pi / 180;
|
|
|
W0 = [Av(1) * cos(An(2)) - Av(3) * cos(An(1)) * sin(An(2)); ...
|
|
|
Av(2) + Av(3) * sin(An(1)); ...
|
|
|
Av(3) * cos(An(2)) * cos(An(1)) + Av(1) * sin(An(2))];
|
|
|
W0 = W0 - Omega0 * [cos(An(2)) * sin(An(3)) + sin(An(1)) * sin(An(2)) * cos(An(3)); ...
|
|
|
cos(An(1)) * cos(An(3)); ...
|
|
|
sin(An(2)) * sin(An(3)) - sin(An(1)) * cos(An(2)) * cos(An(3))];
|
|
|
%卫星本体坐标系相对于质心轨道坐标系的姿态四元数初值为qbo
|
|
|
qbo0 = cos(An(1) / 2) * cos(An(2) / 2) * cos(An(3) / 2) - sin(An(1) / 2) * sin(An(2) / 2) * sin(An(3) / 2);
|
|
|
qbo1 = sin(An(1) / 2) * cos(An(2) / 2) * cos(An(3) / 2) - cos(An(1) / 2) * sin(An(2) / 2) * sin(An(3) / 2);
|
|
|
qbo2 = sin(An(1) / 2) * cos(An(2) / 2) * sin(An(3) / 2) + cos(An(1) / 2) * sin(An(2) / 2) * cos(An(3) / 2);
|
|
|
qbo3 = cos(An(1) / 2) * cos(An(2) / 2) * sin(An(3) / 2) + sin(An(1) / 2) * sin(An(2) / 2) * cos(An(3) / 2);
|
|
|
qbo = [qbo0 qbo1 qbo2 qbo3];
|
|
|
|
|
|
%卫星机动的指令欧拉角，相对于轨道系
|
|
|
cphi = 0;
|
|
|
ctheta = 0;
|
|
|
cpsi = 0;
|
|
|
|
|
|
%%%%%%%%%%初始化完成对话框%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
Data = 1:64; Data = (Data' * Data) / 64;
|
|
|
save initial_data.mat
|
|
|
% initialsuccesshintbox=msgbox('卫星参数初始化数据完毕，可以进行数学仿真了！','卫星参数初始化','custom',Data,summer(64));
|