1.算法仿真效果
matlab2022a仿真结果如下：
2.算法涉及理论知识概要
高超声速飞行器是指飞行马赫数大于5的飞行器，它是一种近空间飞行器。“近空间”可简单理解为：现有飞机飞行的最高高度（约20Km）和卫星运行轨道的最低高度（约100Km）间的空域。近空间飞行器可定性描述为：能持久稳定运行于近空间执行特定任务的各种飞行器。近空间飞行器的发展涉及国家安全与和平利用空间，是目前国际竞相争夺空间技术的焦点之一，是综合国力的体现。近空间飞行器的出现将促生新的作战样式，改写联合作战理论，并对未来技术局部战争产生重大影响。
高超声速飞行器与常规的飞行器相比其整体布局采用机身发动机一体化设计，这使得各个子系统之间具有更强的耦合性和非线性。为了满足高超声速飞行器在复杂的飞行条件下仍然拥有稳定的飞行性能和良好的飞行品质，必须采用全新的控制手段。
高超声速飞行器是指飞行马赫数大于5的飞行器，它是一种近空间飞行器。“近空间”可简单理解为：现有飞机飞行的最高高度（约20Km）和卫星运行轨道的最低高度（约100Km）间的空域。高超声速飞行器与常规的飞行器相比其整体布局采用机身发动机一体化设计，这使得各个子系统之间具有更强的耦合性和非线性。为了满足高超声速飞行器在复杂的飞行条件下仍然拥有稳定的飞行性能和良好的飞行品质，必须采用全新的控制手段。
上面五个式子之间呈强非线性耦合关系，任何变量的改变都将引起其它量的变化，从而导致整个系统的状态发生改变，因此，需要设计非线性控制器来实现解耦协调控制。多平衡点非线性解耦控制方法，基于的是经典反馈控制理论，采用内外环结合的设计思想，具体为：在给定的飞行段内，控制器的结构是相同的，理论上对于某一飞行段，存在着无数个飞行平衡点，应该为每一个平衡点，选取一组控制参数，但实际设计过程中，只需要选取具有典型意义的平衡点来进行研究，得到相应的控制参数，并在飞行段内基于这些参数进行插值计算，即可现实高超声速飞行器多平衡点非线性解耦控制。 
如上图中的结构可以看到，高超声速飞行器控制系统主要包括飞行高度控制和飞行速度控制两部分，飞行高度控制系统是以姿态控制作为控制内环，以达到稳定姿态的目的。而外环采用的是基于高度和高度微分信号的反馈控制；飞行速度控制系统是通过改变发动机推力来控制速度，并在设计过程中结合了姿态信号，以保证速度调整初始阶段的稳定性。
由于高超声速飞行器的飞行和推进系统之间采用一体化设计，该设计方法引起的耦合作用对飞行动态特性和发动机动态特性都有影响，而重点在于飞行控制，故只考虑发动机对飞行动态特性的影响。那么对于一个平衡点，高超声速飞行器纵向线性模型为
高超声速飞行器控制系统主要包括飞行高度控制和飞行速度控制两部分，飞行高度控制系统是以姿态控制作为控制内环，以达到稳定姿态的目的。而外环采用的是基于高度和高度微分信号的反馈控制；飞行速度控制系统是通过改变发动机推力来控制速度，并在设计过程中结合了姿态信号，以保证速度调整初始阶段的稳定性。
3.MATLAB核心程序
% x'= Ax+Bu
% y = Cx+Du
A=[-1.4225e-13 -9.9688 -16.641 0 -5.2784e-22;
9.7346e-7 0 0.086872 0 5.8625e-10;
-9.7346e-7 0 -0.086872 1 -5.8625e-10;
-1.6167e-15 0 0.78031 -0.076265 0;
-1.6645e-16 4525.6 0 0 0];
%--------------------------------------------------------
B=[16.219 0 ;
6.1181e-5 0 ;
-6.1181e-5 0 ;
0 3.6619;
0 0];
%--------------------------------------------------------
C=[1 0 0 0 0;
0 0 0 0 1];
%--------------------------------------------------------
D=0;
%=================================================================
..................................................................
C1 =[1 0 0 0 0;
0 1 0 0 0;
0 0 1 0 0;
0 0 0 1 0;
0 0 0 0 1;
0 0 0 0 0;
0 0 0 0 0];
r = 1.4;
R3 = B*B'-(1/r^2)*B1*B1';
Q3 = C1'*C1
[p31,p32,lamp,perr,wellposed,P3] =aresolv(A,Q3,R3)
K3 = B'*P3;
A1 = A-B*K1 % 原系统
A2 = A-B*K3 % H无穷设计A
y1 = ss(A,B,C,D) % 原系统
y2 = ss(A2,B1,C,D)% H无穷设计后系统
figure;
bode(y1,'b');hold on
bode(y2,'r')
grid on
title('原系统伯德图(蓝) & H无穷设计后原系统伯德图(红)');
xlabel('频率');
ylabel('相位');