https://blog.csdn.net/weixin_43487974/article/details/127163824?spm=1001.2101.3001.6650.2&utm_medium=distribute.pc_relevant.none-task-blog-2defaultYuanLiJiHuaPosition-2-127163824-blog-122591196.pc_relevant_default&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2defaultYuanLiJiHuaPosition-2-127163824-blog-122591196.pc_relevant_default&utm_relevant_index=5

//最速控制综合函数
FH_Float fhan(FH_Float x1, FH_Float x2, FH_Float ctrlError, int r, FH_Float h)
{
    FH_Float d = r*h;
    FH_Float d0 = d*h;
    FH_Float y = x1 - ctrlError + h*x2;
    FH_Float a0 = sqrt(d*d + 8*r*fabs(y));
    FH_Float a,f;
    if(fabs(y) <= d0)
        a = x2 + y/h;
    else
        a = x2 + 0.5*(a0 -d)*Sign(y);
    if(fabs(a) <= d)
        f = -r*a/d;
    else
        f = -r*Sign(a);
//    LogPrintf("fhan f=%f,a=%f,a0=%f,y=%f,pid=%f,x1=%f,x2=%f./n",f,a,a0,y,ctrlError,x1,x2);
    return f;
}

//TD微分跟踪器
FH_Float TD_differential_track(struct PIDController_t* pidCtrller)
{
//    int r = 10; //调节系数
    struct PIDControllerParams_t* pidParams = (struct PIDControllerParams_t*)pidCtrller->base.params;
    int r = pidParams->td_r;
    FH_Float h = 0.02; //滤波因子 略大于步长(0.01)
    FH_Float x_1k = pidCtrller->td_x1;
    FH_Float x_2k = pidCtrller->td_x2;
    FH_Float rtn = fhan(x_1k,x_2k,pidCtrller->ctrlError,r,h);
    pidCtrller->td_x1 = x_1k + AgvSamplePeriod*x_2k;
    pidCtrller->td_x2 = x_2k + AgvSamplePeriod*rtn;
    pidCtrller->ctrlError = pidCtrller->td_x1;

}