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&#