YXS
/
fulllink-solver
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
main
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '659b13a46a'
${ noResults }
fulllink-solver/PlatformPerformance/程序初始化参数（公开）.txt

212 lines
9.2 KiB
Plaintext
Raw Normal View History Unescape Escape

1、初始化代码；
11 months ago
%--------------------------------------------------------------%
%---------InitialData File for simulink model "***.mdl"--------%
%
clear all;
disp(' Initial Data for model sat9.mdl ');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 1.<2E><><EFBFBD>ǵĹ<C7B5><C4B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>صĵ<D8B5><C4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R_s = 6371230 + 705000; %Բ<><D4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ볤<C7B0>ᣬ<EFBFBD><E1A3AC>λ:<3A>ף<EFBFBD>
Mu = 3.986015e14; %<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>;
T0 = 2*pi*sqrt(R_s^3/Mu); %<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڣ<EFBFBD><DAA3><EFBFBD>λ:<3A><>;<3B><><EFBFBD>ݹ<EFBFBD>ʽT0=2*pi*sqrt(R^3/Mu<4D><75>;
Omega0 = 2*pi/T0; %<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȣ<D9B6>rad/s <20><>
% load B.dat; %װ<><D7B0><EFBFBD>شų<D8B4><C5B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߼<EFBFBD><DFBC><EFBFBD>ֵ
T = 0.2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 2.<2E><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD> %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------------------Satellite parameters----------------%
m_sat = 3100; %<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 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]; %<25><><EFBFBD><EFBFBD>չ<EFBFBD><D5B9>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ob<4F><62>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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]; % <20><><EFBFBD>DZ<EFBFBD><C7B1>壨<EFBFBD><E5A3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>壩<EFBFBD><E5A3A9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ob<4F>Ĺ<EFBFBD><C4B9><EFBFBD>kg.m2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% չ<><D5B9>״̬ %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------------<2D><><EFBFBD><EFBFBD>ϵԭ<CFB5><D4AD><EFBFBD>ڱ<EFBFBD><DAB1><EFBFBD>ϵ<EFBFBD><CFB5>λ<EFBFBD><CEBB>ʸ<EFBFBD><CAB8>--------------------%
r_p1 = [2.96;1037;0]*0.001; %+Y<><59><EFBFBD><EFBFBD>(m)
r_p2 = [2.96;-1037;0]*0.001; %-Y<><59><EFBFBD><EFBFBD>(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; %<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͼ<EFBFBD><CFBE><EFBFBD>(10<31><30>)---<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8>
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]); %<25><><EFBFBD><EFBFBD>ģ̬<C4A3><CCAC><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȣ<EFBFBD>---<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8>
xi_2 = xi_1;
B_tran1 = [-2.5591e-11 4.0962e-10 -6.5798;... % +Y<><59><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֲ<EFBFBD><D6B2><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>µ<EFBFBD><C2B5><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>޹ص<DEB9>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>,<2C><><EFBFBD>Ը<EFBFBD><D4B8><EFBFBD><EFBFBD>񶯶Ը<F1B6AFB6><D4B8><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫģ<D4AA>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD>õ<EFBFBD>
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<><59><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֲ<EFBFBD><D6B2><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>µ<EFBFBD><C2B5><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>޹ص<DEB9>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>,<2C><><EFBFBD>Ը<EFBFBD><D4B8><EFBFBD><EFBFBD>񶯶Ը<F1B6AFB6><D4B8><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫģ<D4AA>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD>õ<EFBFBD>
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<><59><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֲ<EFBFBD><D6B2><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>µ<EFBFBD><C2B5><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>޹ص<DEB9>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>,<2C><><EFBFBD>Ը<EFBFBD><D4B8><EFBFBD><EFBFBD>񶯶Ը<F1B6AFB6><D4B8><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫģ<D4AA>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD>õ<EFBFBD>
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<><59><EFBFBD><EFBFBD>̫<EFBFBD><CCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֲ<EFBFBD><D6B2><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>µ<EFBFBD><C2B5><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>޹ص<DEB9>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>,<2C><><EFBFBD>Ը<EFBFBD><D4B8><EFBFBD><EFBFBD>񶯶Ը<F1B6AFB6><D4B8><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫģ<D4AA>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD>õ<EFBFBD>
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<><59><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڰ<EFBFBD>װ<EFBFBD><D7B0>Oa<4F><61>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
I_a2 = B_rot2'*B_rot2; % -Y<><59><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڰ<EFBFBD>װ<EFBFBD><D7B0>Oa<4F><61>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
P1 = [0; 0; 0]; % +Y<><59><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڰ<EFBFBD>װ<EFBFBD><D7B0>Oa<4F><61><EFBFBD><EFBFBD>һ<EFBFBD>׹<EFBFBD><D7B9><EFBFBD><EFBFBD>أ<EFBFBD><D8A3><EFBFBD><EFBFBD>أ<EFBFBD><D8A3><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫģ<D4AA>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD>õ<EFBFBD>
P2 = [0; 0; 0]; % -Y<><59><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڰ<EFBFBD>װ<EFBFBD><D7B0>Oa<4F><61><EFBFBD><EFBFBD>һ<EFBFBD>׹<EFBFBD><D7B9><EFBFBD><EFBFBD>أ<EFBFBD><D8A3><EFBFBD><EFBFBD>أ<EFBFBD><D8A3><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫģ<D4AA>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD>õ<EFBFBD>
%----------------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.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬ȷ<CCAC><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD> %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%--------------------------------%
% <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> %
%--------------------------------%
Wgmax =[2;2;2]*pi/180; %<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Χ,2<><32>/<2F><>
gyro_bias_initial = [1,1,1]'*pi/180/3600; %<25><>ֵƫ<D6B5><C6AB> rad/s
var_gyro = (0.005/3600*2*pi/360)^2;
sigmav = 0.005/3600*2*pi/360;% %<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>,<2C>ȼ<EFBFBD><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD>׼<EFBFBD><D7BC>
var_gyro_bias = (0.005/3600*2*pi/360*1e-3)^2;
sigmau = 0.005/3600*2*pi/360*1e-3;% %<25>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>,<2C>ȼ<EFBFBD><C8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD>׼<EFBFBD><D7BC>
gyro_white_noise_seed = fix(1000*rand(3,1)); %<25><><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
star_white_noise_seed = fix(1000*rand(4,1)); %<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
sample_time = 0.5; %<25><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ͬ
%--------------------------------%
% Kalman<61>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> %
%--------------------------------%
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.<2E><><EFBFBD><EFBFBD>ִ<EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%--------------------------------%
% ִ<>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> %
%--------------------------------%
%=============<3D><><EFBFBD><EFBFBD><EFBFBD>÷<EFBFBD><C3B7>ֲ<EFBFBD><D6B2><EFBFBD>===========
Jwheel = 0.106; %<25><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>kg.m^2
Tmotormax = 0.2; % <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD>Nm
Kr = 0.015;
Tr = 0.1;
Usaturate = 42; %<25><><EFBFBD>͵<EFBFBD>ѹ;
Wsaturate = 3500*30/pi; %<25><><EFBFBD>ֵı<D6B5><C4B1><EFBFBD>ת<EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD>3500rpm<70><6D>
Hsaturate = 25; %<25><><EFBFBD>ֵı<D6B5><C4B1>ͽǶ<CDBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>25Nms
%===============<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>========
% Mags = 100; %<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ž<EFBFBD>A.m2
%<25><><EFBFBD>ְ<EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
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)];
%------------<2D><><EFBFBD>ֽǶ<D6BD><C7B6><EFBFBD>------------%
HmA = 0;%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>ʱX<CAB1><58>A<EFBFBD>ֵı<D6B5><C4B1>ƽǶ<C6BD><C7B6><EFBFBD>;
HmB = 0;%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>ʱY<CAB1><59>B<EFBFBD>ֵı<D6B5><C4B1>ƽǶ<C6BD><C7B6><EFBFBD>;
HmC = 0;%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>ʱZ<CAB1><5A>C<EFBFBD>ֵı<D6B5><C4B1>ƽǶ<C6BD><C7B6><EFBFBD>;
HmD = 0;%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>ʱбװS1<53>ֵı<D6B5><C4B1>ƽǶ<C6BD><C7B6><EFBFBD>;
HmE = 0;%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD>ʱбװS2<53>ֵı<D6B5><C4B1>ƽǶ<C6BD><C7B6><EFBFBD>;
%------------<2D><>ж<EFBFBD>ز<EFBFBD><D8B2><EFBFBD>------------%
% kxunload = 4;
% kyunload = 4;%
% kzunload = 4;
Qerror = 1/30;
%---------<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD>---------%
% PID=[900,0,5600,760,0,2000,900,0,6000];
wn = 1*2*pi; %<25><><EFBFBD><EFBFBD>wn
k = wn^2;
c = 2*0.9*wn; %<25><><EFBFBD><EFBFBD>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.<2E><><EFBFBD>dz<EFBFBD>ʼ<EFBFBD><CABC>̬<EFBFBD><CCAC><EFBFBD><EFBFBD> %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%<25><><EFBFBD>dz<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>̬<EFBFBD>Ƕȣ<C7B6><C8A3><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>ٶ<EFBFBD>
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))];
%<25><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>ֵΪ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];
%<25><><EFBFBD>ǻ<EFBFBD><C7BB><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>ŷ<EFBFBD><C5B7><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD><DAB9><EFBFBD>ϵ
cphi = 0;
ctheta = 0;
cpsi = 0;
%%%%%%%%%%<25><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>ɶԻ<C9B6><D4BB><EFBFBD>%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Data=1:64;Data=(Data'*Data)/64;
initialsuccesshintbox=msgbox('<27><><EFBFBD>Dz<EFBFBD><C7B2><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD><EFBFBD><EFBFBD><EFBFBD>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD>ˣ<EFBFBD>','<27><><EFBFBD>Dz<EFBFBD><C7B2><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>','custom',Data,summer(64));