fst.m

非常有用的自抗扰pid控制器程序。搞控制的可以看看。

%fst.m
function[f]=fst(v0,v2,r,h)
d=r*h;
d0=h*d;
y=v0+v2*h;
w=d^2+8*r*abs(y);
a0=sqrt(w);
if abs(y)>d0
    a=v2+[(a0-d)/2]*sign(y);
else
    a=v2+y/h;
end
if abs(a)>d;
    f=-r*sign(a);
else
    f=-r*a/d;
end