写一个扩张状态观测器eso的fal函数的matlab函数 扩展状态观测器

状态观测器是根据系统的输入输出来确定系统内部状态变量的装置，它的示意图如下：

在自抗扰控制器的设计过程中，我们通常把未知的干扰都用$f$来表示，如果利用状态观测器可以将未知的干扰观测出来，就可以提前在控制器中采取补偿措施，使得控制器的设计更加简单，这也是自抗扰控制器的核心思想。

对一个线性控制系统

$\left\{ \begin{array}{lr} \dot{X} = AX+BU & \\ Y = CX \end{array} \right.$

来说，以对象的输出量$Y$和输入量$U$作为其输入，可以构造出如下的新系统

$\dot{Z} = AZ-L(CZ-Y)+BU = (A-LC)Z+LY+BU$ ，$L$为要适当选取的矩阵，这是用对象的输出量$Y$和输入量$U$来设计出的新系统，令这两个系统状态变量的误差记为$e= Z-X$，则上面两个方程组相减，得误差变量$e$所满足的方程组为$\dot{e} = (A-LC)e$，这里只要取矩阵$L$使得$A-LC$稳定，就会有$e\Rightarrow0$，从而$Z \Rightarrow X$。新设计的系统的状态$Z$就能近似的估计出原系统的所有状态变量$X$，这个状态观测器也可以写成

$\left\{ \begin{array}{lr} e=CZ-Y & \\ \dot{Z} = AZ-Le+BU \end{array} \right.$

下面来看一个具体的例子

设对象为如下有扰动作用的非定常系统

$\left\{ \begin{array}{lr} \dot{x_1} = x_2, x_1(0) = 0& \\ \dot{x_2} = f(x_1,x_2,t) +w(t), x_2(0) = 0 & \\ y = x_1 \end{array} \right.$

其中，

$f(x_1,x_2,t) = -(1+\frac{cos(t)}{2})x_1-(1+sin(\frac{t}{3}))x_2$，

$w(t) = sign(sin(\frac{3t}{2}))$，

取状态观测器为

$\left\{ \begin{array}{lr} e = z_1-y & \\ \dot{z_1} = z_2-100e & \\ \dot{z_2} = -200fal(e, 0.5, 0.01) \end{array} \right.$

其中

$fal(e, 0.5, 0.01) = \left\{ \begin{array}{lr} |e|^{0.5}sign(e), |e|>0.01 & \\ \frac{e}{0.01^{0.5}}, |e| \leq 0.01 \end{array} \right.$

用这个观测器对对象进行状态估计时，可以用简单的欧拉积分法来计算，即令状态观测器的初始值为$z_1(0)=0$，$z_2(0) = 0$，然后把系统离散化成如下的递推公式来计算

$\left\{ \begin{array}{lr} e = z_1-y, fe = fal(e, 0.5, 0.01) & \\ z_1= z_1+h(z_2-100e) & \\ z_2 = z_2 -h200fe \end{array} \right.$

用这个递推公式，取采样周期$h=0.01$，来进行状态估计的结果如下图所示。

上面那张图实际上有两条曲线，是状态估计的比较好，所以两条曲线基本重合到一起了，下面有一张局部的细节图。

以下是实现的代码：

clear all;
h=0.01;
T=0.01;
time = 20;
N = time/T;
n=0:N-1;
% x = sin(n*T);

for k=1:1:N
    %%% original state
    x1(1) = 0;
    x2(1) = 0;
    x1(k+1) = x1(k) + h*x2(k);
    x2(k+1) = x2(k) + h*(-(1+0.5*cos(k*T))*x1(k)-(1+sin(k*T/3))*x2(k)+sign(sin(1.5*k*T)));
    y(k) = x1(k);
    %%%% state observer
    z1(1) = 0;
    z2(1) = 0;
    e(k) = z1(k) - y(k);
    fe = fal(e(k), 0.5, 0.01);
    z1(k+1) = z1(k) + h*(z2(k)-100*e(k));
    z2(k+1) = z2(k) -h*200*fe;
end
plot(n*T,x1(1,1:2000),'b',n*T,z1(1,1:2000),'r');

fal函数如下：

function fe = fal(e,tau1,tau2)
%UNTITLED2 此处显示有关此函数的摘要
%   此处显示详细说明
if abs(e)>tau2
    fe = power(abs(e),tau1)*sign(e);
else
    fe = e/(power(tau2,tau1));
end

end

上面设计过程最关键的一步是状态观测器的设计，韩京清先生的设计过程考虑的参数比较多，这方面可以看一下高志强的线性ADRC的设计过程。