最速微分器Matlab实现

ADRC中关于TD最速微分器的Matlab实现及Simulink仿真

个人理解：
x1(k)是一阶值，v(t)是输入目标值，x2(k)是对应的微分值，如果一阶是位置，则微分对应速度，
X1(k+1)对应是下一时刻在最大加速度下的估计的位置，x2(k+1)则是最大加速下的速度
r 对应最大加速度，h0是对应的伺服周期
matlab测试代码

function [NewV1, NewV2] = TDFilterCalc(V1,V2,Aim,Tr,Cycle)
	fh = fFhan(V1 - Aim, V2, Tr, Cycle);
	NewV1 = V1 + V2 * Cycle;
	NewV2 = V2 + fh * Cycle;
end

function [output] = Fsg(x,d)
	temp1 = sign(x + d);
	temp2 = sign(x - d);
	output = 0.5 * (temp1 - temp2);
end

function [output] = fFhan(x1, x2, r, h)
	d = r * h * h;
	a0 = h * x2;
	y = x1 + a0;
	a1 = sqrt(d * (d + 8.0 * abs(y)));
	a2 = a0 + sign(y) * (a1 - d) / 2.0;
	a = (a0 + y) * Fsg(y, d) + a2 * (1.0 - Fsg(y, d));
	output = -r * (a / d) * Fsg(a, d) - r * sign(a) * (1.0 - Fsg(a, d));
end

定步长0.025，加速度500，伺服周期0.025，运行得到上图。