（源代码）《应用时标分离和动态逆方法设计飞行器的姿态控制系统》

1、仿真来源：《应用时标分离和动态逆方法设计飞行器的姿态控制系统》，武立军，贺有智，现代防御技术，2007
2、在发动机推力的处理上，尝试了三种方法：连续推力、通过幅值限制得到的离散推力和通过PWPF调制得到的离散推力。

连续推力很容易实现；后两者与飞行器的参数相关，发动机推力等参数需要仔细设置，由于原文没有给出飞行器的具体参数，发动机推力的大小与原文不同。

一、离散推力

%滚动通道：推力阈值0.5N，大于此值发动机启动；稳态推力30N
%偏航通道：推力阈值0.5N，大于此值发动机启动；稳态推力30N
%俯仰通道：推力阈值0.5N，大于此值发动机启动；稳态推力30N
clear
clc
[row,av,g]=OnAir(50000);  %计算大气密度，当地声速，重力加速度
 
L=1;   %抛盖后长度
DD=0.5;%弹体直径
Jx=0.1;   %绕x轴转动惯量
Jy=1;  %绕y轴转动惯量
Jz=1;  %绕z轴转动惯量
 
xcg=0.5*L;   %质心位置，单位m
 
vartheta=0;
fea=0;
gamma=0;
omega_x=0;
omega_y=0;
omega_z=0;
 
sum1=0;
sum2=0;
sum3=0;
 
k1=5;
k2=5;
k3=50;
m1=20;
n1=100;
m2=20;
n2=100;
m3=40;
n3=400;
 
Dt=0.001;
n=1;
t=0;
for i=1:1000
    %期望姿态角
    vartheta_c=60*pi/180;
    fea_c=45*pi/180;
    gamma_c=30*pi/180;
      
    e1=[vartheta-vartheta_c;
        fea-fea_c;
        gamma-gamma_c];
    k_matrix=[-k1 0 0;
        0 -k2 0;
        0 0 -k3];
    tran1=[0 sin(gamma) cos(gamma);
        0 cos(gamma)/cos(vartheta) -sin(gamma)/cos(vartheta);
        1 -tan(vartheta)*cos(gamma) tan(vartheta)*sin(gamma)];
    omega_c=inv(tran1)*k_matrix*e1;
    
    e2=[omega_x-omega_c(1);
        omega_y-omega_c(2);
        omega_z-omega_c(3);];
    tran2=[1/Jx 0 0;
        0 1/Jy 0;
        0 0 1/Jz];
    sum1=sum1+e2(1)*Dt;
    sum2=sum2+e2(2)*Dt;
    sum3=sum3+e2(3)*Dt;
    m_matrix=[-m1*e2(1)-n1*sum1-(Jy-Jz)/Jx*omega_y*omega_z;
        -m2*e2(2)-n2*sum2-(Jz-Jx)/Jy*omega_x*omega_z;
        -m3*e2(3)-n3*sum3-(Jx-Jy)/Jz*omega_x*omega_y];
    u=inv(tran2)*m_matrix;

    if abs(u(1)/(DD/2)/2)>.5 %阈值为1N
        u(1)=30*2*DD/2*sign(u(1));%滚动发动机稳态推力为5N
    else
        u(1)=0;
    end
    
    if abs(u(2)/(