（源代码）PWPF调制

1、仿真来源：《应用时标分离和动态逆方法设计飞行器的姿态控制系统》，武立军，贺有智，现代防御技术，2007

clear
clc
[row,av,g]=OnAir(50000);  %计算大气密度，当地声速，重力加速度
m=44; %质量
 
Tay=60*g;  %姿控发动机推力
L=1.945;   %抛盖后长度
DD=0.37;%弹体直径
Jx=0.3;   %绕x轴转动惯量
Jy=3.9;  %绕y轴转动惯量
Jz=3.9;  %绕z轴转动惯量
xcg=0.47*L;   %质心位置，单位m
 
vartheta=0.1;
fea=0.1;
gamma=0.1;
omega_x=0;
omega_y=0;
omega_z=0;
 
%控制器参数
sum1=0;
sum2=0;
sum3=0;
k1=5;
k2=5;
k3=5;
m=40;
n=400;
 
%PWPF调制器参数
%滚转通道
Km1=1;
Tm1=1;
U1=0;
Uon1=0.1;
Uoff1=0.01;
u_out1=0;  %取值为0 1 -1
%偏航通道
Km2=1;
Tm2=1;
U2=0;
Uon2=0.8;
Uoff2=0.3;
u_out2=0;  %取值为0 1 -1
%俯仰通道
Km3=1;
Tm3=0.85;
U3=0;
Uon3=1.4;
Uoff3=1.35;
u_out3=0;  %取值为0 1 -1
 
 
Dt=0.001;
n=1;
t=0;
for i=1:5000
    %期望姿态角
    vartheta_c=50*pi/180;
    fea_c=40*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&#