|
|
%--------------------------------------------------------------%
|
|
|
%---------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;
|
|
|
initialsuccesshintbox=msgbox('卫星参数初始化数据完毕，可以进行数学仿真了！','卫星参数初始化','custom',Data,summer(64)); |