clear;
clc;
d2r=pi/180;%%degree2radian

%%%轉動慣量IB
IB=[1.250.00.0
????0.09.650.0
????0.00.09.65];

%%%轉動慣量IB的逆invIB
invIB=inv（IB）;

%%%初始角速度Wwo
Ww0=[000]‘*d2r;

%%%初始姿態角（按照ZYX轉動得到的Euler姿態角）fei0=[Phi?Theta?Psi]
fei0=[101010]‘*d2r;

%%%初始姿態四元數[Q00?Q0]
Q00?=?cos（fei0（1）/2）*cos（fei0（2）/2）*cos（fei0（3）/2）+sin（fei0（1）/2）*sin（fei0（2）/2）*sin（fei0（3）/2）;
Q1?=?sin（fei0（1）/2）*cos（fei0（2）/2）*cos（fei0（3）/2）-cos（fei0（1）/2）*sin（fei0（2）/2）*sin（fei0（3）/2）;
Q2?=?cos（fei0（1）/2）*sin（fei0（2）/2）*cos（fei0（3）/2）+sin（fei0（1）/2）*cos（fei0（2）/2）*sin（fei0（3）/2）;
Q3?=?-sin（fei0（1）/2）*sin（fei0（2）/2）*cos（fei0（3）/2）+cos（fei0（1）/2）*cos（fei0（2）/2）*sin（fei0（3）/2）;
Q0=[Q1?Q2?Q3];

%%%目標姿態角（體坐標系下，按照ZYX轉動得到的Euler姿態角）fei0=[Phi?Theta?Psi]
fei1=[000]‘*d2r;

%%%目標姿態四元數Qt=[qt0?qt]
qt0?=?cos（fei1（1）/2）*cos（fei1（2）/2）*cos（fei1（3）/2）+sin（fei1（1）/2）*sin（fei1（2）/2）*sin（fei1（3）/2）;
qt1?=?sin（fei1（1）/2）*cos（fei1（2）/2）*cos（fei1（3）/2）-cos（fei1（1）/2）*sin（fei1（2）/2）*sin（fei1（3）/2）;
qt2?=?cos（fei1（1）/2）*sin（fei1（2）/2）*cos（fei1（3）/2）+sin（fei1（1）/2）*cos（fei1（2）/2）*sin（fei1（3）/2）;
qt3?=?-sin（fei1（1）/2）*sin（fei1（2）/2）*cos（fei1（3）/2）+cos（fei1（1）/2）*cos（fei1（2）/2）*sin（fei1（3）/2）;
Qt=[qt0?qt1?qt2?qt3];

%%%初始角動量HB0
HB0=[IB*Ww0]‘;%HB0設置為行向量以便與Simulink中的積分器模塊的輸入值對應

%%%軌道角速度（Height=300KM）
Wio=[0?-2*pi/5431.184?0]‘;

%%%角速度陀螺：常值漂移cong，白噪聲sample時間Tscontrol和增益Noig，精度Gyquan
cong=[111]*0.5/3600*d2r;%%每3600s漂移0.5?度
Noig=[111]*1/3*5*10^（-5）*d2r;
Gyquan=[111]*1.e-6*d2r;
Tscontrol=0.05;

%%%?PD?控制的控制參數Kp，Kd?均為三維
wnx=5;
wny=5;
wnz=5;
Kp=[1.25/0.515*wnx^2*0.69.65/3.18*wny^2*0.69.65/1.59*wnz^2*0.6]*2;
Kd=[2*wnx*1.25/0.515*22*wny*9.65/3.182*wnz*9.65/1.59*2]*sqrt（0.6）;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件????6453231??2018-07-03?15:37??Simulink仿真衛星姿態控制.pdf

?????文件??????26627??2018-09-12?09:40??weixingpid\attitudecontrolpid.slx

?????文件???????1833??2018-09-12?09:55??weixingpid\attitudepid.m

?????目錄??????????0??2018-09-13?15:35??weixingpid

-----------?---------??----------?-----??----

??????????????6481691????????????????????4