ADRC自抗扰控制，有手就行「建议收藏」

/******************ADRC program with C******************** // write by suitcalaw // copy by the high hand net world // https://blog.csdn.net/b457738242 **********************2020-12-07*************************/

//参数区，这11个就是需要用户整定的参数
/****************TD**********/
float r = 0,//快速跟踪因子
      h = 0;//滤波因子,系统调用步长

/**************ESO**********/
float b       = 0,//系统系数
      delta   = 0,//delta为fal（e，alpha，delta）函数的线性区间宽度
      belta01 = 0,//扩张状态观测器反馈增益1
	  belta02 = 0,//扩张状态观测器反馈增益2
	  belta03 = 0;//扩张状态观测器反馈增益3
	  
/**************NLSEF*******/
float alpha1 = 0,//
      alpha2 = 0,//
      belta1 = 0,//跟踪输入信号增益
      belta2 = 0;//跟踪微分信号增益
	  
	  
/*****************************fhan函数*********************************/
float fhan(float x1,float x2,float r,float h)
{ 
   
 /*****************************第一套************************/
// float d = 0,
// a = 0,
// a0 = 0,
// a1 = 0,
// a2 = 0,
// y = 0,
// fhan = 0;
// 
// d = r*h*h;
// a0 = h*x2;
// y = x1+a0;
// a1 = sqrtf(d*(d+8*fabsf(y)));
// a2 = a0 + sign(y)*(a1 - d)/2.0;
// a = (a0+y)*(sign(y+d)-sign(y-d))/2.0 + a2*(1-(sign(y+d)-sign(y-d))/2.0);
// fhan = -r*(a/d)*(sign(y+d)-sign(y-d))/2.0 - r*sign(a)*(1-(sign(a+d)-sign(a-d))/2.0);
 /******************************第二套**********************/
	float deltaa  =0,
		  deltaa0 =0,
	      y       =0,
	      a0      =0,
	      a       =0,
	      fhan    =0;
	
	deltaa = r*h;
	deltaa0 = deltaa*h;
	y=x1+x2*h;
	a0 = sqrtf(deltaa*deltaa+8*r*fabsf(y));
	if(fabsf(y)<=deltaa0)
		a=x2+y/h;
	else
		a=x2+0.5*(a0-deltaa)*sign(y);
	if(fabsf(a)<=deltaa)
		fhan = -r*a/deltaa;
	else
		fhan = -r*sign(a);
	
  return fhan;
}
/************************************sign函数***************************/
float sign(float x)
{ 
   
	if(x>0)
		return 1;
	else if(x<0)
		return -1;
	else
		return 0;
}
/*******************************fal函数**********************************/
float fal(float e,float alpha,float delta)
{ 
   
  float result = 0,fabsf_e = 0;
  
  fabsf_e = fabsf(e);
  
  if(delta>=fabsf_e)
    result = e/powf(delta,1.0-alpha);
  else //if(delta<fabsf_e)
    result = powf(fabsf_e,alpha)*sign(e);
 
 return result;     
}
//中间变量区，不需要用户管理以及赋值
/****************TD*******************/
float x1 = 0,//跟踪输入
      x2 = 0,//跟踪输入的微分
/****************ESO******************/
      e  = 0,//误差
	  z1 = 0,//跟踪反馈值
	  z2 = 0,//跟踪反馈值的而微分
	  z3 = 0,//跟踪系统的扰动（总扰动）
/**************NLSEF******************/
      u = 0;//输出值
/********************************ADRC************************************/
float ADRC(float v,float y)  // 参数：v：输入的目标值； y：反馈值
{ 
   
  float u0 = 0,
        e1 = 0,
		e2 = 0;
/******************************TD****************************************/
  x1 = x1 + h*x2;
  x2 = x2 + h*fhan(x1-v,x2,r,h);
/******************************ESO***************************************/
  e = z1 - y;
  z1 = z1 + h*(z2-belta01*e);
  z2 = z2 + h*(z3-belta02*fal(e,0.5,delta)+b*u);
  z3 = z3 + h*(-belta03*fal(e,0.25,delta));
/******************限幅，ADRC正常的话不会达到限幅条件********************/
  if(z1>=30000) z1=30000;
  if(z1<=-30000) z1 = -30000;
  if(z2>=30000) z2=30000;
  if(z2<=-30000) z2 = -30000;
  if(z3>=30000) z3=30000;
  if(z3<=-30000) z3 = -30000;
/******************************NLSEF*************************************/
  e1 = x1 - z1;
  e2 = x2 - z2;
  
  u0 = belta1*fal(e1,alpha1,delta) + belta2*fal(e2,alpha2,delta);//其中0<alpha1<1<alpha2
  
  u = u0 - z3/b;
  
  return u;
}