微分---跟踪器（TD）

方法参考自
——微分跟踪器的研究与应用
张海丽
张宏丽
传统的PID 从含有噪音的信号提取具有降将噪音增大或超调量增大的特点。将微分跟踪器应用于PID，可以得到原始信号的最佳逼近。
优点：TD与PID有效地提取了原始信号，克服消极因素，有较强的抗噪声的能力，无超调的跟踪原始信号。

TD的控制公式如下：

float fst(float x1, float x2, float v){    float td_y = 0;    float a0 = 0;    float a = 0;    float fhan = 0;    float d = 0;    float d0 = 0;    d = r*h;    d0 = h*d;    td_y = x1 - v + h*x2;    a0 = sqrt(d*d + 8 * r*fabs(td_y));    if (fabs(td_y)>d0)        a = x2 + 0.5*(a0 - d)*sign(td_y);    else        a = x2 + td_y / h;    if (fabs(a)>d)        fhan = -r*sign(a);    else        fhan = -r*a / d;    return(fhan);}

float ADRC(float v, float y)             // v是控制系统的输入，y是控制系统的输出{    float u0;    float e = 0;    float e1 = 0;    float e2 = 0;    //**********   TD  ************    td_x1 = td_x1 + h*td_x2;                        //td_x1=v1;    td_x2 = td_x2 + h*fst(td_x1, td_x2, v);           //td_x2=v2;        e1 = td_x1 - y;                 e2 = td_x2 ;        u0 = P* e1 + D * e2;        if(u0 > 10) u0 = 10;        if(u0 < -10) u0 = -10;    return(u0);          

经过实际检测！