微分---跟踪器（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);          

经过实际检测！