线性系统大作业——1.一阶倒立摆建模与控制系统设计

0.简介

本文是《线性系统理论》大作业的一部分，内容是一阶和二阶倒立摆的分析与控制，本文是 线性系统大作业——0.一阶和二阶倒立摆建模与控制系统设计 的一部分。

最后，本文中使用的 MATLAB 代码和 Simulink 仿真模型已经上传到 GitHub 上。
Code: https://github.com/Cc19245/Inverted-pendulum.git

1.建立数学模型

如图所示，假设$u$为外部作用力，$M$为小车质量，$m$为摆杆质量，摆杆长度为$2l$且摆杆质量均匀分布。设$x$为小车位置，$\theta$为摆杆与竖直方向的夹角。系统不考虑空气阻力的影响。

本文给出这些参数如表所示，假设杆的质量是均匀分布的，并且不考虑系统的摩擦。

物理量	数值
小车质量($M$)	$2kg$
摆杆质量($m$)	$0.1kg$
摆杆长度($2l$)	$1m$

1.1.牛顿运动定律分析

先对小车进行受力分析，水平方向有 $$\begin{array}{r}M\ddot{x}+H=u\end{array}$$

再对摆进行受力分析，水平方向有 $$\begin{array}{r}H=m\frac{d^2}{d{t}^2}(x+l\sin \theta )\end{array}$$

即 $$\begin{array}{r}H=m\ddot{x}+ml(\ddot{\theta }\cos \theta -{\dot{\theta }}^2\sin \theta )\end{array}$$

摆的竖直方向有 $$\begin{array}{r}V-mg=m\frac{d^2}{d{t}^2}(l\cos \theta )\end{array}$$

即 $$\begin{array}{r}V=mg-ml(\ddot{\theta }\sin \theta +{\dot{\theta }}^2\cos \theta )\end{array}$$

对摆应用动量矩定理，以$\theta$顺时针方向旋转为正，设摆绕着质心的转动惯量为$I$，则有
$$\begin{array}{r}Vl\sin \theta -Hl\cos \theta =I\ddot{\theta }\end{array}$$

整理可得 $$\begin{array}{r}\begin{array}{l}(M+m)\ddot{x}+ml\ddot{\theta }\cos \theta -ml{\dot{\theta }}^2\sin \theta =u\\ ml\ddot{x}\cos \theta +(I+m{l}^2)\ddot{\theta }-mgl\sin \theta =0\end{array}\end{array}$$

写成矩阵形式如下： $$\begin{array}{r}\left[\begin{array}{cc}M+m& ml\cos \theta \\ ml\cos \theta & I+m{l}^2\end{array}\right]\left[\begin{array}{c}\ddot{x}\\ \ddot{\theta }\end{array}\right]+\left[\begin{array}{cc}0& -ml\dot{\theta }\sin \theta \\ 0& 0\end{array}\right]\left[\begin{array}{l}\dot{x}\\ \dot{\theta }\end{array}\right]=\left(\begin{array}{c}u\\ mgl\sin \theta \end{array}\right)\end{array}$$

1.2.欧拉-拉格朗日方程分析

欧拉-拉格朗日方程可以描述为 $$\begin{array}{r}\frac{d}{dt}\left(\frac{\partial T}{\partial \dot{q}}\right)-\frac{\partial T}{\partial q}+\frac{\partial V}{\partial q}=F\end{array}$$ 其中 $q$为系统的广义坐标系，$F$为系统的广义力, $T$为系统动能，$V$ 为系统势能。

则对于一阶倒立摆系统来说，选择小车位移$x$和倒立摆的角度$\theta$作为广义坐标，小车推力$u$为广义力，则有
$$\begin{array}{r}\begin{array}{l}T=\frac{1}{2}M{\dot{x}}^2+\frac{1}{2}I{\dot{\theta }}^2+\frac{1}{2}m\left\{{\left[\frac{d}{dt}(x+l\sin \theta )\right]}^2+\left[\frac{d}{dt}(l\cos \theta )\right]\right\}\\ V=mgl\cos \theta \end{array}\end{array}$$

计算欧拉-拉格朗日方程中的各项，有 $$\begin{array}{r}\begin{array}{l}\frac{d}{dt}\left(\frac{\partial T}{\partial \dot{x}}\right)=(M+m)\ddot{x}+ml\cos \theta \ddot{\theta }-ml\sin \theta {\dot{\theta }}^2\\ \frac{\partial T}{\partial x}=0,\frac{\partial V}{\partial x}=0\\ \frac{d}{dt}\left(\frac{\partial T}{\partial \dot{\theta }}\right)=I\ddot{\theta }+ml(\ddot{x}\cos \theta -\dot{x}\sin \theta \dot{\theta }+l\ddot{\theta })\\ \frac{\partial T}{\partial \theta }=-ml\dot{x}\dot{\theta }\sin \theta ,\frac{\partial V}{\partial \theta }=-mgl\sin \theta ,\end{array}\end{array}$$

带入欧拉-拉格朗日方程整理可得 $$\begin{array}{r}\begin{array}{l}(M+m)\ddot{x}+ml\ddot{\theta }\cos \theta -ml{\dot{\theta }}^2\sin \theta =u\\ ml\ddot{x}\cos \theta +(I+m{l}^2)\ddot{\theta }+mgl\sin \theta =0\end{array}\end{array}$$

即 $$\begin{array}{r}\left[\begin{array}{cc}M+m& ml\cos \theta \\ ml\cos \theta & I+m{l}^2\end{array}\right]\left[\begin{array}{c}\ddot{x}\\ \ddot{\theta }\end{array}\right]+\left[\begin{array}{cc}0& -ml\sin \theta \dot{\theta }\\ 0& 0\end{array}\right]\left[\begin{array}{c}\dot{x}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{c}u\\ mgl\sin \theta \end{array}\right]\end{array}$$

可见使用欧拉-拉格朗日方程建立的系统动力学方程与使用牛顿运动定律分析得到的系统动力学方程相同，因此初步验证了所建立的系统动态方程的正确性。

2.Simulink仿真

利用上面建立的一阶倒立摆的系统动力学方程，可以在Simulink中搭建系统的仿真模型，如图所示。

假设系统的初始状态为$x=0,\theta =\frac{\pi }{6}$，且系统无输入，则此后小车位置$x$和倒立摆的摆角$\theta$的变化如图所示。

3.使用SimMechancis仿真

使用SimMechanics可以直接搭建物理模型，只需要对物理模型进行参数赋值和关节的连接就可以完成一个仿真物理模型的搭建。因此这里可以使用SimMechanics建立物理模型进行仿真，并和上面动力学建模的仿真结果进行对比，从而验证上面推导的动力学方程的正确性。如图所示，是使用SimMechanicalcs建立的物理模型。其中增益模块的增益值-1是由于SimMechanicalcs中的$\theta$方向和上面推导的方向相反。如图所示，是使用SimMechanicalcs的仿真结果。

由图可见这个结果和上面动力学建模并使用Simulink仿真的结果完全相同，从而证明了之前动力学建模的正确性。

4.在平衡点附近模型线性化

为了使用线性系统理论的知识对系统进行分析和控制，需要对上述的非线性系统在在平衡点附近进行线性化。系统平衡点附近时，$\theta$很小，并且假设其角速度$\dot{\theta }$也很小，则可进行近似处理：$\cos \theta \approx 1,\sin \theta \approx \theta ,\sin \theta \dot{\theta }\approx 0$。从而得到一阶倒立摆系统在平衡点附近的线性化模型为
$$\begin{array}{r}\left[\begin{array}{cc}M+m& ml\\ ml& I+m{l}^2\end{array}\right]\left[\begin{array}{c}\ddot{x}\\ \ddot{\theta }\end{array}\right]=\left[\begin{array}{c}u\\ mgl\theta \end{array}\right]\end{array}$$

定义系统的状态变量为$(x_1,x_2,x_3,x_4)=(x,\dot{x},\theta ,\dot{\theta })$，系统的输入量为小车外力$u$，系统输出为小车的位移$x$，则可得系统的状态空间方程为
$$\begin{array}{r}\begin{array}{c}\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\theta }\\ \ddot{\theta }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& \frac{-m^{2}g{l}^2}{I(M+m)+Mm{l}^2}& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{mgl(M+m)}{I(M+m)+Mm{l}^2}& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]+\left[\begin{array}{c}0\\ \frac{I+m{l}^2}{I(M+m)+Mm{l}^2}\\ 0\\ \frac{-ml}{I(M+m)+Mm{l}^2}\end{array}\right]u\\ y=x=\left[\begin{array}{llll}1& 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]\end{array}\end{array}$$

带入系统参数，可得一阶倒立摆系统的状态空间方程为 $$\begin{array}{r}\begin{array}{c}\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\theta }\\ \ddot{\theta }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& -0.3630& \\ 0& 0& 0& 1\\ 0& 0& 15.2444& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]+\left[\begin{array}{c}0\\ 0.4938\\ 0\\ -0.7407\end{array}\right]u\\ y=\left[\begin{array}{llll}1& 0& 0& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right]\end{array}\end{array}$$

5.系统能控性、能观性和稳定性分析

5.1.能控性分析

对于一个连续的时间系统： $$\begin{array}{r}\begin{array}{c}\dot{x}=Ax+Bu\\ y=Cx+Du\end{array}\end{array}$$

系统能控的充要条件是系统的能控性矩阵为行满秩阵，即 $$\begin{array}{r}rank(S_c)=rank(\left[\begin{array}{ccccc}B& AB& A^{2}B& \dots & A^{n-1}B\end{array}\right])=n\end{array}$$

根据上面可控性的原理，对系统进行可控性分析，将式中的$A,B$代入判据中，编写MATLAB代码如下：

M = 2;
m = 0.1;
g = 9.8;
l =0.5;
I = 1/3*m*l^2;

a23 = -m*m*g*l*l/(I*(m+M)+M*m*l*l);
a43 = m*g*l*(M+m)/(I*(m+M)+M*m*l*l);
b2 = (I+m*l*l)/(I*(m+M)+M*m*l*l);
b4 = -m*l/(I*(m+M)+M*m*l*l);

A = [0 1 0 0;
    0 0 a23 0;
    0 0 0 1;
    0 0 a43 0];
B = [0;
    b2;
    0;
    b4];
C =[1 0 0 0]; 

S_c = [B A*B A^2*B A^3*B]
fprintf('rank(S_c) = %d\n', rank(S_c));

程序运行结果为： $$\begin{array}{r}rank(S_c)=4\end{array}$$

因此系统是状态完全可控的。

5.2.能观性分析

系统能观的充要条件是系统的能观性矩阵为列满秩阵，即 $$\begin{array}{r}rank(Q_o)=rank(\left[\begin{array}{c}C\\ CA\\ C{A}^2\\ ⋮\\ C{A}^{n-1}\end{array}\right])=n\end{array}$$

根据上面能观性原理，继续编写MATLAB代码判断系统能观性

Q_o = [C;
    C*A;
    C*A^2;
    C*A^3];
fprintf('rank(Q_o) = %d\n', rank(Q_o));

程序运行结果为： $$\begin{array}{r}rank(Q_o)=4\end{array}$$

因此系统是能观的。

5.3.稳定性分析

5.3.1.Routh-Hurwitz判据

劳斯–赫尔维茨稳定性判据给出线性定常系统$\varsigma (A,B,C)$输出稳定的充要条件是其传递函数
$$\begin{array}{r}\Phi =C(sI-A{)}^{-1}B\end{array}$$
的所有极点都位于复平面S的左半平面，即系统特征方程$\mid \lambda I-A\mid =0$的特征根具有负实部。
使用Matlab计算矩阵A的特征值：

disp('eig(A)=');
eig(A)

程序输出结果为$\lambda =\left[0,0,3.9044,-3.9044\right]$，存在复平面右半平面的特征根，因此系统是不稳定的。

5.3.2.Lyapunov函数

Lyapunov定理给出线性时不变系统在平衡点$x_e=0$处渐进稳定的充分必要条件是对任意给定的对称正定矩阵$Q$，存在一个对称正定矩阵$P$，满足李雅普诺夫方程 $$\begin{array}{r}{A}^{\top }P+PA=-Q\end{array}$$

使用MATLAB进行Lyapunov方程的求解，程序如下：

P = lyap(A',eye(4))

程序运行结果为$\lambda =1.0e+33\times \left[-0.0000,-0.0000,0.0000,3.0550\right]$。由于存在数值稳定性问题仍然解得$P$，但是$P$不是正定矩阵，因此系统不稳定。（这里应该有点问题）

6.基于极点配置方法的控制器设计

根据上述分析可知系统存在复平面右半平面的极点导致系统不稳定，因此需要设计一个状态反馈控制器$u=-Kx$，配置系统闭环极点全部到复平面左半平面。

假设希望将系统的闭环极点配置到${\mu }_1=-1,{\mu }_2=-2,{\mu }_3=-1+j\sqrt{3},{\mu }_4=-1-j\sqrt{3}$。则可以通过调用MATLAB中的acker和place命令来方便地计算反馈矩阵K，代码如下：

% 调节器的极点配置问题
J = [-1 -2 -1+j*sqrt(3) -1-j*sqrt(3)];
K1 = acker(A,B,J); 
K2 = place(A,B,J); 
disp('K1 = ');
disp(K1);
disp('K2 = ');
disp(K2);

程序输出结果为$K1=K2=(-1.1020,-2.2041,-37.5147,-8.2194)$，可见对于单输入系统来说使用acker和place命令得到的结果是一样的。

为了检验上述极点配置的闭环系统的性能，使用在平衡点线性化的系统配置的极点K对非线性系统进行极点配置，并在Simulink中搭建如图所示，其中math-model模块是一阶倒立摆的非线性数学模型。

假设系统初始状态为$\left[x_1,x_2,x_3,x_4\right]=\left[x,\dot{x},\theta ,\dot{\theta }\right]=\left[0,0,5^{\circ },0\right]$，运行Simulink仿真模型，得到系统的闭环响应应如图所示，由图可见系统在微小扰动的情况下仍然能够保持稳定，说明所设计的状态反馈控制器是有效的。

7.状态观测器设计

7.1.全阶观测器

定义线性系统的观测器的数学模型为
$$\begin{array}{r}\dot{x}=(A-K_{e}C)x+Bu+K_{e}y\end{array}$$
其中，$x$为估计状态，$Cx$为估计输出。对于观测器的极点配置，一般来说其响应速度至少要比所考虑的闭环系统快2至5倍。因此假设所配置的观测器极点为${\mu }_1=-2,{\mu }_2=-6,{\mu }_3=-2-j2\sqrt{3},{\mu }_4=-2+j2\sqrt{3}$。

前文已经验证，取系统的输出变量为$x$，系统是能观的。因此这里可以直接进行观测器的配置。考虑原系统的对偶系统，可以证明原系统的对偶系统和全解观测器的极点配置问题是等价的，因此可以使用状态反馈极点配置的方法进行极点配置，MATLAB代码如下：

A_ = A';
B_ = C';
C_ = B';
J_ = [-2 -6 -2-j*2*sqrt(3) -2+j*2*sqrt(3)];
Ke = (acker(A_, B_, J_))';
disp('Ke = ');
disp(Ke);

程序输出结果为$Ke=(12.0,75.2,-988.9,-3689.2)$。

7.2.最小阶观测器

最小阶观测器由全阶观测器中经过变量替换得到，可由下式描述：
$$\begin{array}{r}\dot{\tilde{\eta }}=\hat{\mathbf{A}}\tilde{\mathbf{\eta }}+\hat{\mathbf{B}}y+\hat{\mathbf{F}}\mathbf{u}\end{array}$$
其中， $$\begin{array}{r}\begin{array}{l}\mathbf{\eta }={\mathbf{x}}_{\mathrm{b}}-{\mathbf{K}}_{\mathbf{e}}y\\ \tilde{\mathbf{\eta }}={\tilde{\mathbf{x}}}_{\mathrm{b}}-{\mathbf{K}}_{\mathbf{e}}y\\ \hat{\mathbf{A}}={\mathbf{A}}_{\mathrm{b}\mathrm{b}}-{\mathbf{K}}_{\mathbf{e}}{\mathbf{A}}_{\mathrm{a}\mathrm{b}}\\ \hat{\mathbf{B}}=\hat{\mathbf{A}}{\mathbf{K}}_{\mathbf{e}}+{\mathbf{A}}_{\mathrm{b}\mathrm{a}}-{\mathbf{K}}_{\mathbf{e}}{A}_{\mathrm{a}\mathrm{a}}\\ \hat{\mathbf{F}}={\mathbf{B}}_{\mathrm{b}}-{\mathbf{K}}_{\mathbf{e}}{\mathbf{B}}_{\mathrm{a}}\end{array}\end{array}$$
则针对$\hat{\mathbf{A}}$进行极点配置即可。

对于一阶倒立摆系统来说，状态变量$x$等于输出$y$，因此可以直接测量，剩余的状态变量这是不可测量的部分。对状态空间模型划分如下：
$$\begin{array}{r}x=\left[\begin{array}{c}{x}_a\\ {\mathbf{x}}_{\mathbf{b}}\end{array}\right]=\left[\begin{array}{c}x\\ \dot{x}\\ \theta \\ \dot{\theta }\end{array}\right],A=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& -0.3630& 0\\ 0& 0& 0& 1\\ 0& 0& 15.2444& 0\end{array}\right],B=\left[\begin{array}{c}0\\ 0.4938\\ 0\\ -0.7407\end{array}\right]\end{array}$$

划分后的降阶观测器是3阶系统，假设期望配置的极点位置为${\mu }_1=-6,{\mu }_2=-2-j2\sqrt{3},{\mu }_3=-2+j2\sqrt{3}$，则可编写MATLAB代码如下：
编写代码：

Aaa = A(1, 1);
Aab = A(1, 2:end);
Aba = A(2:end, 1);
Abb = A(2:end, 2:end);
J_j = [-6 -2+j*2*sqrt(3) -2-j*2*sqrt(3)];
Ke_j = (acker(Abb',Aab',J_j))';
disp('Ke_j = ');
disp(Ke_j);

程序输出为$K{e}_j=(10,-152.2041,-684.4898)$。

8.基于观测器的状态反馈控制

8.1.基于全阶观测器的状态反馈控制

对于线性系统来说，基于全阶观测器的状态反馈控制系统的状态方程为
$$\begin{array}{r}\left[\begin{array}{c}\dot{x}\\ \dot{e}\end{array}\right]=\left[\begin{array}{cc}A& -BK\\ 0& A-K_{e}C\end{array}\right]\left[\begin{array}{c}x\\ e\end{array}\right]\end{array}$$

但是这里一阶倒立摆系统是非线性系统，上述观测器和极点配置都是使用线性化的模型得到的，因此式并不能完全真实反映系统状态。

为了检验基于全阶观测器的状态反馈控制效果，搭建具有全阶观测器的状态反馈控制器的非线性系统Simulink仿真模型如图所示。

Simulink仿真模型中，全阶状态观测器由于设计变量较多，使用Simulink模块搭建较为复杂，因此编写s-function来实现，代码如下：

% 一阶倒立摆系统的全阶观测器s-function建模
function [sys,x0,str,ts,simStateCompliance]=
order1_all_observer_sfun(t,x,u,flag, x_0, th_0)
switch flag,
    case 0,
        [sys,x0,str,ts,simStateCompliance]=
        mdlInitializeSizes(t,x,u, x_0, th_0);
    case 1,
        sys=mdlDerivatives(t,x,u);
    case 2,
        sys=mdlUpdate(t,x,u);
    case 3,
        sys=mdlOutputs(t,x,u);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,x,u);
    case 9,
        sys=mdlTerminate(t,x,u);
    otherwise
        DAStudio.error('Simulink:blocks:unhandledFlag',
        num2str(flag));
end
% 主函数结束

% ---------------------------------------------
function [sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u,x_0, th_0)
% 初始化
sizes = simsizes;% 生成sizes数据结构

sizes.NumContStates  = 4;% 连续状态数, 分别是x', theta1', theta2', x, theta1, theta2
sizes.NumDiscStates  = 0;% 离散状态数,缺省为 0
sizes.NumOutputs     = 4;% 输出量个数,缺省为 0, 
sizes.NumInputs      = 1;% 输入量个数，缺省为 0, 这里的输入取为e
sizes.DirFeedthrough = 1;% 是否存在直接馈通。1：存在；0：不存在，缺省为 1 
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);
x0  = [x_0; 0; th_0; 0];% 设置初始状态
str = [];% 保留变量置空
ts  = [0 0]; % 连续系统
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes

% ---------------------------------------------
function sys=mdlDerivatives(t, x, u)
%  计算导数例程子函数
M = 2; m = 0.1; l =0.5; I = 1/3*m*l^2; g = 9.8;
a23 = -m*m*g*l*l/(I*(m+M)+M*m*l*l);
a43 = m*g*l*(M+m)/(I*(m+M)+M*m*l*l);
b2 = (I+m*l*l)/(I*(m+M)+M*m*l*l);
b4 = -m*l/(I*(m+M)+M*m*l*l);
A = [0 1 0 0;
    0 0 a23 0;
    0 0 0 1;
    0 0 a43 0];
b = [0; b2; 0; b4]; 
K = [-1.1020, -2.2041, -37.5147, -8.2194];  
Ke = [12; 75.2; -988.9; -3689.2];
sys = (A-b*K)*x + Ke*u ;  % 注意这里的u就是x(1)的观测误差e(1)

% ---------------------------------------------
function sys=mdlUpdate(t,x,u)
%3. 状态更新例程子函数
sys = [];

% ---------------------------------------------
function sys=mdlOutputs(t,x,u)
%4. 计算输出例程子函数
sys=x;

% ---------------------------------------------
function sys=mdlGetTimeOfNextVarHit(t,x,u)
% 5. 计算下一个采样时间，仅在系统是变采样时间系统时调用
sampleTime = 1;    %  Example, set the next hit to be one second later.
sys = t + sampleTime;

% ---------------------------------------------
function sys=mdlTerminate(t,x,u)
% 6. 仿真结束时要调用的例程函数
sys = [];

假设系统初始状态为$\left[x_1,x_2,x_3,x_4\right]=\left[x,\dot{x},\theta ,\dot{\theta }\right]=\left[0,0,5^{\circ },0\right]$，并且状态观测器的初始观测误差为$\left[e_1,e_2,e_3,e_4\right]=\left[0,0,5^{\circ },0\right]$，运行Simulink仿真模型，系统的动态响应如图所示，全阶观测器的观测误差如图，可见系统可以很好地稳定。

8.2.基于最小观测器的状态反馈控制

对于线性系统来说，基于最小阶观测器的状态反馈控制系统的状态方程为
$$\begin{array}{r}\left[\begin{array}{c}\dot{x}\\ \dot{e}\end{array}\right]=\left[\begin{array}{cc}A-BK& B{K}_b\\ 0& A_{bb}-K_e{A}_{ab}\end{array}\right]\left[\begin{array}{c}x\\ e\end{array}\right]\end{array}$$

同理可知，该方程并不能完全准确的反映一阶倒立摆系统的响应。为了检验基于最小阶观测器的状态反馈控制效果，搭建具有最小阶观测器的状态反馈控制器的非线性系统Simulink仿真模型如图所示。

Simulink仿真模型中，最小阶状态观测器由于设计变量较多，使用Simulink模块搭建较为复杂，因此编写s-function来实现，代码如下：

% 一阶倒立摆系统的最小阶观测器s-function建模
function [sys,x0,str,ts,simStateCompliance] =
order1_min_observer_sfun(t,x,u,flag, x_0, th_0)
switch flag,
    case 0,
        [sys,x0,str,ts,simStateCompliance]=
        mdlInitializeSizes(t,x,u, x_0, th_0);
    case 1,
        sys=mdlDerivatives(t,x,u);
    case 2,
        sys=mdlUpdate(t,x,u);
    case 3,
        sys=mdlOutputs(t,x,u);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,x,u);
    case 9,
        sys=mdlTerminate(t,x,u);
    otherwise
        DAStudio.error('Simulink:blocks:unhandledFlag',
        num2str(flag));
end
% 主函数结束

% ---------------------------------------------
function [sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u,x_0, th_0)
% 初始化
sizes = simsizes;% 生成sizes数据结构

sizes.NumContStates  = 3;% 连续状态数
sizes.NumDiscStates  = 0;% 离散状态数,缺省为 0
sizes.NumOutputs     = 4;% 输出量个数,缺省为 0, 
sizes.NumInputs      = 2;% 输入量个数，缺省为 0, 这里的输入取为输入u和y
sizes.DirFeedthrough = 1;% 是否存在直接馈通。1：存在；0：不存在，缺省为 1 
sizes.NumSampleTimes = 1;   % at least one sample time is needed
sys = simsizes(sizes);       
x0  = [0; th_0; 0] - [10; -152.2041; -684.4898]*x_0;  % 设置初始状态
str = [];% 保留变量置空
ts  = [0 0]; % 连续系统
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes

% ---------------------------------------------
function sys=mdlDerivatives(t, x, u)
%  计算导数例程子函数
M = 2; m = 0.1; l =0.5; I = 1/3*m*l^2; g = 9.8;
a23 = -m*m*g*l*l/(I*(m+M)+M*m*l*l);
a43 = m*g*l*(M+m)/(I*(m+M)+M*m*l*l);
b2 = (I+m*l*l)/(I*(m+M)+M*m*l*l);
b4 = -m*l/(I*(m+M)+M*m*l*l);
A = [0 1 0   0;
     0 0 a23 0;
     0 0 0   1;
     0 0 a43 0];
B = [0; b2; 0; b4]; 
Ke_j = [10;  -152.2041; -684.4898];
A_hat = A(2:end, 2:end) - Ke_j*A(1, 2:end);
B_hat = A_hat*Ke_j + A(2:end, 1) - Ke_j*A(1,1);
F_hat = B(2:end) - Ke_j*B(1);
sys = A_hat*x + B_hat*u(2) + F_hat*u(1) ;  % u1就是输入，u2是原系统输出y

% ---------------------------------------------
function sys=mdlUpdate(t,x,u)
%3. 状态更新例程子函数
sys = [];

% ---------------------------------------------
function sys=mdlOutputs(t,x,u)
%4. 计算输出例程子函数
C_hat = [0 0 0; 1 0 0; 0 1 0; 0 0 1];
D_hat = [1; 10; -152.2041; -684.4898];
sys = C_hat*x + D_hat*u(2);

% ---------------------------------------------
function sys=mdlGetTimeOfNextVarHit(t,x,u)
 % 5. 计算下一个采样时间，仅在系统是变采样时间系统时调用
sampleTime = 1;    %  Example, set the next hit to be one second later.
sys = t + sampleTime;

% ---------------------------------------------
function sys=mdlTerminate(t,x,u)
 % 6. 仿真结束时要调用的例程函数
sys = [];

假设系统初始状态为$\left[x_1,x_2,x_3,x_4\right]=\left[x,\dot{x},\theta ,\dot{\theta }\right]=\left[0,0,5^{\circ },0\right]$，并且最小阶状态观测器的初始观测误差为$\left[e_2,e_3,e_4\right]=\left[0,5^{\circ },0\right]$。运行Simulink仿真模型，系统的动态响应如图所示，最小阶观测器的观测误差如图，其中由于状态变量$x_1$是小车的位移，即系统输出可以直接测量，因此它的误差一直是0。可见基于最小阶观测器的系统动态响应速度比使用全阶观测器效果更好，这是因为有一个状态变量可以进行准确的观测，因此系统收敛速度更快。

9.LQR控制

已知系统的状态空间模型 $$\begin{array}{rl}\dot{x}& =Ax+Bu\end{array}$$
LQR问题就是确定下列最佳控制向量的矩阵$K$： $$\begin{array}{r}u(t)=-Kx(t)\end{array}$$ 使得下列性能指标达到最小值：
$$\begin{array}{r}J={\int }_0^{\infty }[{\mathbf{x}}^{\top }\mathbf{Q}\mathbf{x}+{\mathbf{u}}^{\top }\mathbf{R}\mathbf{u}]dt\end{array}$$

选择$\mathbf{Q}=\mathbf{I},\mathbf{R}=\mathbf{I}$，使用MATLAB求解的代码如下：

Q = eye(4);
R = eye(1);
K_lqr = lqr(A,B,Q,R);
disp('K_lqr = ');
disp(K_lqr);

得到最优状态反馈矩阵为$K_{l}qr=(-1,-2.7964,-53.9967,-13.9239)$。

直接使用这个反馈控制矩阵在Simulink中仿真非线性模型的LQR控制，假设系统初始状态为$\left[x_1,x_2,x_3,x_4\right]=\left[x,\dot{x},\theta ,\dot{\theta }\right]=\left[0,0,5^{\circ },0\right]$，系统的动态响应如图所示，可见系统的控制效果非常好。