无名的ADRC的C语言实现

分为ADRC.h和ADRC.c

确实看头文件有用，有哪些变量都一目了然。

和ACfly一样的是比如都有beta这个参数

ADRC.c

/*----------------------------------------------------------------------------------------------------------------------/
*               本程序只供购买者学习使用，版权著作权属于无名科创团队，无名科创团队将飞控程序源码提供给购买者，
*               购买者要为无名科创团队提供保护，未经作者许可，不得将源代码提供给他人，不得将源代码放到网上供他人免费下载，
*               更不能以此销售牟利，如发现上述行为，无名科创团队将诉之以法律解决！！！
-----------------------------------------------------------------------------------------------------------------------/
*               生命不息、奋斗不止；前人栽树，后人乘凉！！！
*               开源不易，且学且珍惜，祝早日逆袭、进阶成功！！！
*               学习优秀者，简历可推荐到DJI、ZEROTECH、XAG、AEE、GDU、AUTEL、EWATT、HIGH GREAT等公司就业
*               求职简历请发送：15671678205@163.com，需备注求职意向单位、岗位、待遇等
*               无名科创开源飞控QQ群：540707961
*               CSDN博客：http://blog.csdn.net/u011992534
*               优酷ID：NamelessCotrun无名小哥
*               B站教学视频：https://space.bilibili.com/67803559/#/video
*               客户使用心得、改进意见征集贴：http://www.openedv.com/forum.php?mod=viewthread&tid=234214&extra=page=1
*               淘宝店铺：https://shop348646912.taobao.com/?spm=2013.1.1000126.2.5ce78a88ht1sO2
*               百度贴吧:无名科创开源飞控
*               修改日期:2018/8/30
*               版本：慧飞者——V1.0.1
*               版权所有，盗版必究。
*               Copyright(C) 武汉科技大学无名科创团队 2017-2025
*               All rights reserved
----------------------------------------------------------------------------------------------------------------------*/
#include "Headfile.h"
#include "ADRC.h"Fhan_Data ADRC_Pitch_Controller;
Fhan_Data ADRC_Roll_Controller;
const float ADRC_Unit[4][16]=
{/*TD跟踪微分器   改进最速TD,h0=N*h      扩张状态观测器ESO           扰动补偿     非线性组合*//*  r     h      N                  beta_01   beta_02    beta_03     b0     beta_0  beta_1     beta_2     N1     C    alpha1  alpha2  zeta       b*/{1000000 ,0.005 , 5,               300,      10000,      100000,      0.5,    0.85,   1.5,      0.0003,       2,    5,    0.9,   1.2,    0.03,    0.1},{1000000 ,0.005 , 5,               300,      10000,      100000,      0.5,    0.85,   1.5,      0.0003,       2,    5,    0.9,   1.2,    0.03,    0.1},{300000  ,0.005 , 3,               300,      4000,      10000,     100,   0.2,    1.2,      0.0010,       5,      5,    0.8,   1.5,    0.03,    0.05},
};float Constrain_Float(float amt, float low, float high){return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}Butter_Parameter ADRC_Div_LPF_Parameter={//200---20hz1,    -1.14298050254,   0.4128015980962,0.06745527388907,   0.1349105477781,  0.06745527388907
};
float ADRC_LPF(float curr_inputer,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
{/* 加速度计Butterworth滤波 *//* 获取最新x(n) */Buffer->Input_Butter[2]=curr_inputer;/* Butterworth滤波 */Buffer->Output_Butter[2]=Parameter->b[0] * Buffer->Input_Butter[2]+Parameter->b[1] * Buffer->Input_Butter[1]+Parameter->b[2] * Buffer->Input_Butter[0]-Parameter->a[1] * Buffer->Output_Butter[1]-Parameter->a[2] * Buffer->Output_Butter[0];/* x(n) 序列保存 */Buffer->Input_Butter[0]=Buffer->Input_Butter[1];Buffer->Input_Butter[1]=Buffer->Input_Butter[2];/* y(n) 序列保存 */Buffer->Output_Butter[0]=Buffer->Output_Butter[1];Buffer->Output_Butter[1]=Buffer->Output_Butter[2];return (Buffer->Output_Butter[2]);
}int16_t Sign_ADRC(float Input)
{int16_t output=0;if(Input>1E-6) output=1;else if(Input<-1E-6) output=-1;else output=0;return output;
}int16_t Fsg_ADRC(float x,float d)
{int16_t output=0;output=(Sign_ADRC(x+d)-Sign_ADRC(x-d))/2;return output;
}void ADRC_Init(Fhan_Data *fhan_Input1,Fhan_Data *fhan_Input2)
{fhan_Input1->r=ADRC_Unit[0][0];fhan_Input1->h=ADRC_Unit[0][1];fhan_Input1->N0=(uint16)(ADRC_Unit[0][2]);fhan_Input1->beta_01=ADRC_Unit[0][3];fhan_Input1->beta_02=ADRC_Unit[0][4];fhan_Input1->beta_03=ADRC_Unit[0][5];fhan_Input1->b0=ADRC_Unit[0][6];fhan_Input1->beta_0=ADRC_Unit[0][7];fhan_Input1->beta_1=ADRC_Unit[0][8];fhan_Input1->beta_2=ADRC_Unit[0][9];fhan_Input1->N1=(uint16)(ADRC_Unit[0][10]);fhan_Input1->c=ADRC_Unit[0][11];fhan_Input1->alpha1=ADRC_Unit[0][12];fhan_Input1->alpha2=ADRC_Unit[0][13];fhan_Input1->zeta=ADRC_Unit[0][14];fhan_Input1->b=ADRC_Unit[0][15];fhan_Input2->r=ADRC_Unit[1][0];fhan_Input2->h=ADRC_Unit[1][1];fhan_Input2->N0=(uint16)(ADRC_Unit[1][2]);fhan_Input2->beta_01=ADRC_Unit[1][3];fhan_Input2->beta_02=ADRC_Unit[1][4];fhan_Input2->beta_03=ADRC_Unit[1][5];fhan_Input2->b0=ADRC_Unit[1][6];fhan_Input2->beta_0=ADRC_Unit[1][7];fhan_Input2->beta_1=ADRC_Unit[1][8];fhan_Input2->beta_2=ADRC_Unit[1][9];fhan_Input2->N1=(uint16)(ADRC_Unit[1][10]);fhan_Input2->c=ADRC_Unit[1][11];fhan_Input2->alpha1=ADRC_Unit[1][12];fhan_Input2->alpha2=ADRC_Unit[1][13];fhan_Input2->zeta=ADRC_Unit[1][14];fhan_Input2->b=ADRC_Unit[1][15];
}//ADRC最速跟踪微分器TD，改进的算法fhan
void Fhan_ADRC(Fhan_Data *fhan_Input,float expect_ADRC)//安排ADRC过度过程
{float d=0,a0=0,y=0,a1=0,a2=0,a=0;float x1_delta=0;//ADRC状态跟踪误差项x1_delta=fhan_Input->x1-expect_ADRC;//用x1-v(k)替代x1得到离散更新公式fhan_Input->h0=fhan_Input->N0*fhan_Input->h;//用h0替代h，解决最速跟踪微分器速度超调问题d=fhan_Input->r*fhan_Input->h0*fhan_Input->h0;//d=rh^2;a0=fhan_Input->h0*fhan_Input->x2;//a0=h*x2y=x1_delta+a0;//y=x1+a0a1=sqrt(d*(d+8*ABS(y)));//a1=sqrt(d*(d+8*ABS(y))])a2=a0+Sign_ADRC(y)*(a1-d)/2;//a2=a0+sign(y)*(a1-d)/2;a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d));fhan_Input->fh=-fhan_Input->r*(a/d)*Fsg_ADRC(a,d)-fhan_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));//得到最速微分加速度跟踪量fhan_Input->x1+=fhan_Input->h*fhan_Input->x2;//跟新最速跟踪状态量x1fhan_Input->x2+=fhan_Input->h*fhan_Input->fh;//跟新最速跟踪状态量微分x2
}//原点附近有连线性段的连续幂次函数
float Fal_ADRC(float e,float alpha,float zeta)
{int16 s=0;float fal_output=0;s=(Sign_ADRC(e+zeta)-Sign_ADRC(e-zeta))/2;fal_output=e*s/(powf(zeta,1-alpha))+powf(ABS(e),alpha)*Sign_ADRC(e)*(1-s);return fal_output;
}/************扩张状态观测器********************/
//状态观测器参数beta01=1/h  beta02=1/(3*h^2)  beta03=2/(8^2*h^3) ...
void ESO_ADRC(Fhan_Data *fhan_Input)
{fhan_Input->e=fhan_Input->z1-fhan_Input->y;//状态误差fhan_Input->fe=Fal_ADRC(fhan_Input->e,0.5,fhan_Input->N1*fhan_Input->h);//非线性函数，提取跟踪状态与当前状态误差fhan_Input->fe1=Fal_ADRC(fhan_Input->e,0.25,fhan_Input->N1*fhan_Input->h);/*************扩展状态量更新**********/fhan_Input->z1+=fhan_Input->h*(fhan_Input->z2-fhan_Input->beta_01*fhan_Input->e);fhan_Input->z2+=fhan_Input->h*(fhan_Input->z3-fhan_Input->beta_02*fhan_Input->fe+fhan_Input->b0*fhan_Input->u);//ESO估计状态加速度信号，进行扰动补偿，传统MEMS陀螺仪漂移较大，估计会产生漂移fhan_Input->z3+=fhan_Input->h*(-fhan_Input->beta_03*fhan_Input->fe1);
}/************非线性组合****************/
/*
void Nolinear_Conbination_ADRC(Fhan_Data *fhan_Input)
{
float d=0,a0=0,y=0,a1=0,a2=0,a=0;
float Sy=0,Sa=0;//ADRC状态跟踪误差项fhan_Input->h1=fhan_Input->N1*fhan_Input->h;d=fhan_Input->r*fhan_Input->h1*fhan_Input->h1;
a0=fhan_Input->h1*fhan_Input->c*fhan_Input->e2;
y=fhan_Input->e1+a0;
a1=sqrt(d*(d+8*ABS(y)));
a2=a0+Sign_ADRC(y)*(a1-d)/2;Sy=Fsg_ADRC(y,d);
a=(a0+y-a2)*Sy+a2;
Sa=Fsg_ADRC(a,d);
fhan_Input->u0=-fhan_Input->r*((a/d)-Sign_ADRC(a))*Sa-fhan_Input->r*Sign_ADRC(a);//a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d));//fhan_Input->fh=-fhan_Input->r*(a/d)*Fsg_ADRC(a,d)
//                -fhan_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));//得到最速微分加速度跟踪量
}
*/
void Nolinear_Conbination_ADRC(Fhan_Data *fhan_Input)
{float temp_e2=0;//temp_e2=Constrain_Float(fhan_Input->e2,-3000,3000);temp_e2=Constrain_Float(fhan_Input->e2_lpf,-3000,3000);fhan_Input->u0=fhan_Input->beta_1*Fal_ADRC(fhan_Input->e1,fhan_Input->alpha1,fhan_Input->zeta)+fhan_Input->beta_2*Fal_ADRC(temp_e2,fhan_Input->alpha2,fhan_Input->zeta);}void ADRC_Control(Fhan_Data *fhan_Input,float expect_ADRC,float feedback_ADRC)
{fhan_Input->Last_TD_Input=fhan_Input->TD_Input;fhan_Input->TD_Input=expect_ADRC;fhan_Input->TD_Input_Div=(fhan_Input->TD_Input-fhan_Input->Last_TD_Input)/fhan_Input->h;fhan_Input->Last_ESO_Input=fhan_Input->ESO_Input;fhan_Input->ESO_Input=feedback_ADRC;fhan_Input->ESO_Input_Div=(fhan_Input->ESO_Input-fhan_Input->Last_ESO_Input)/fhan_Input->h;/*自抗扰控制器第1步*//**************************//*****安排过度过程，输入为期望给定，由TD跟踪微分器得到：过度期望信号x1，过度期望微分信号x2******/Fhan_ADRC(fhan_Input,expect_ADRC);/*自抗扰控制器第2步*//***********  * *  ***********//************系统输出值为反馈量，状态反馈，ESO扩张状态观测器的输入*********/fhan_Input->y=feedback_ADRC;/*****扩张状态观测器，得到反馈信号的扩张状态：1、状态信号z1；2、状态速度信号z2；3、状态加速度信号z3。其中z1、z2用于作为状态反馈与TD微分跟踪器得到的x1,x2做差后，经过非线性函数映射，乘以beta系数后，组合得到未加入状态加速度估计扰动补偿的原始控制量u*********/ESO_ADRC(fhan_Input);//低成本MEMS会产生漂移，扩展出来的z3此项会漂移，目前暂时未想到办法解决，未用到z3/*自抗扰控制器第3步*//**************************//********状态误差反馈率***/fhan_Input->e0+=fhan_Input->e1*fhan_Input->h;//状态积分项fhan_Input->e1=fhan_Input->x1-fhan_Input->z1;//状态偏差项fhan_Input->e2=fhan_Input->x2-fhan_Input->z2;//状态微分项fhan_Input->e2_lpf=ADRC_LPF(fhan_Input->e2,&fhan_Input->ADRC_LPF_Buffer,&ADRC_Div_LPF_Parameter);//巴特沃斯低通后得到的微分项,20hz/********线性组合*******//*fhan_Input->u0=//fhan_Input->beta_0*fhan_Input->e0+fhan_Input->beta_1*fhan_Input->e1+fhan_Input->beta_2*fhan_Input->e2;*/Nolinear_Conbination_ADRC(fhan_Input);/**********扰动补偿*******/fhan_Input->u=fhan_Input->u0//-fhan_Input->z3/fhan_Input->b;-fhan_Input->z3/fhan_Input->b0;//fhan_Input->u+=Constrain_Float(fhan_Input->beta_0*fhan_Input->e0,-150,150);//由于MEMS传感器漂移比较严重，当beta_03取值比较大时，长时间z3漂移比较大，目前不加入扰动补偿控制量fhan_Input->u=Constrain_Float(fhan_Input->u0,-450,450);//不加入扰动补偿//fhan_Input->u=Constrain_Float(fhan_Input->u,-450,450);//加入扰动补偿
}void ADRC_Integrate_Reset(Fhan_Data *fhan_Input)  {fhan_Input->e0=0.0f;}

=============================================

下面是网上的比较早的版本

ADRC.h

#ifndef _ADRC_H_
#define _ADRC_H_
/*----------------------------------------------------------------------------------------------------------------------/*               本程序只供购买者学习使用，版权著作权属于无名科创团队，*               无名科创团队将飞控程序源码提供给购买者，*               购买者要为无名科创团队提供保护，*               未经作者许可，不得将源代码提供给他人*               不得将源代码放到网上供他人免费下载，*               更不能以此销售牟利，如发现上述行为，*               无名科创团队将诉之以法律解决！！！
-----------------------------------------------------------------------------------------------------------------------/*               生命不息、奋斗不止；前人栽树，后人乘凉！！！*               开源不易，且学且珍惜，祝早日逆袭、进阶成功！！！
-----------------------------------------------------------------------------------------------------------------------/*       无名科创开源飞控 V1.1   武汉科技大学  By.YuYi*        CSDN博客: http://blog.csdn.net/u011992534*               优酷ID：NamelessCotrun无名小哥*               无名科创开源飞控QQ群：540707961*               https://shop348646912.taobao.com/?spm=2013.1.1000126.2.5ce78a88ht1sO2*               百度贴吧:无名科创开源飞控*               修改日期:2017/10/30*               版本：V1.1*               版权所有，盗版必究。*               Copyright(C) 武汉科技大学无名科创团队 2017-2019*               All rights reserved
----------------------------------------------------------------------------------------------------------------------*/
typedef struct
{
/*****安排过度过程*******/
float x1;//跟踪微分期状态量
float x2;//跟踪微分期状态量微分项
float r;//时间尺度
float h;//ADRC系统积分时间
uint16 N0;//跟踪微分器解决速度超调h0=N*hfloat h0;
float fh;//最速微分加速度跟踪量
/*****扩张状态观测器*******/
/******已系统输出y和输入u来跟踪估计系统状态和扰动*****/
float z1;
float z2;
float z3;//根据控制对象输入与输出，提取的扰动信息
float e;//系统状态误差
float y;//系统输出量
float fe;
float fe1;
float beta_01;
float beta_02;
float beta_03;/**********系统状态误差反馈率*********/
float e0;//状态误差积分项
float e1;//状态偏差
float e2;//状态量微分项
float u0;//非线性组合系统输出
float u;//带扰动补偿后的输出/*********第一种组合形式*********/
float beta_0;//线性
float beta_1;//非线性组合参数
float beta_2;//u0=beta_1*e1+beta_2*e2+(beta_0*e0);
/*********第二种组合形式*********/
float alpha1;//u0=beta_1*fal(e1,alpha1,zeta)+beta_2*fal(e2,alpha2,zeta)
float alpha2;//0<alpha1<1<alpha2
float zeta;//线性段的区间长度
/*********第三种组合形式*********/
float h1;//u0=-fhan(e1,e2,r,h1);
uint16 N1;//跟踪微分器解决速度超调h0=N*h
/*********第四种组合形式*********/
float c;//u0=-fhan(e1,c*e2*e2,r,h1);
float b0;//扰动补偿
}Fhan_Data;void ADRC_Init(Fhan_Data *fhan_Input1,Fhan_Data *fhan_Input2);
void Fhan_ADRC(Fhan_Data *fhan_Input,float expect_ADRC);
void ADRC_Control(Fhan_Data *fhan_Input,float expect_ADRC,float feedback);extern Fhan_Data ADRC_Pitch_Controller,ADRC_Roll_Controller;
#endif

ADRC.c

#include "Headfile.h"
#include "ADRC.h"
/*----------------------------------------------------------------------------------------------------------------------/*               本程序只供购买者学习使用，版权著作权属于无名科创团队，*               无名科创团队将飞控程序源码提供给购买者，*               购买者要为无名科创团队提供保护，*               未经作者许可，不得将源代码提供给他人*               不得将源代码放到网上供他人免费下载，*               更不能以此销售牟利，如发现上述行为，*               无名科创团队将诉之以法律解决！！！
-----------------------------------------------------------------------------------------------------------------------/*               生命不息、奋斗不止；前人栽树，后人乘凉！！！*               开源不易，且学且珍惜，祝早日逆袭、进阶成功！！！
-----------------------------------------------------------------------------------------------------------------------/*       无名科创开源飞控 V1.1   武汉科技大学  By.YuYi*        CSDN博客: http://blog.csdn.net/u011992534*               优酷ID：NamelessCotrun无名小哥*               无名科创开源飞控QQ群：540707961*               https://shop348646912.taobao.com/?spm=2013.1.1000126.2.5ce78a88ht1sO2*               百度贴吧:无名科创开源飞控*               修改日期:2017/10/30*               版本：V1.1*               版权所有，盗版必究。*               Copyright(C) 武汉科技大学无名科创团队 2017-2019*               All rights reserved
----------------------------------------------------------------------------------------------------------------------*/
Fhan_Data ADRC_Pitch_Controller;
Fhan_Data ADRC_Roll_Controller;
const float ADRC_Unit[3][15]=
{
/*TD跟踪微分器   改进最速TD,h0=N*h      扩张状态观测器ESO           扰动补偿     非线性组合*/
/*  r     h      N                  beta_01   beta_02    beta_03     b0       beta_0  beta_1  beta_2   N1     C    alpha1  alpha2*/{300000 ,0.005 , 2,               100,      1000,      2000,     0.001,    0.002,   1.0,      0.0005,    5,    5,    0.8,   1.5,    50},{300000 ,0.005 , 2,               100,      1000,      2000,     0.001,    0.002,   1.0,      0.0005,    5,    5,    0.8,   1.5,    50},{50000  ,0.005 , 30,              100,      2000,      10000,    5    ,    0.002,   10,        0.001,    5,    5,    0.5,   1.05,   50},
};float Constrain_Float(float amt, float low, float high){return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}int16_t Sign_ADRC(float Input)
{int16_t output=0;if(Input>1E-6) output=1;else if(Input<-1E-6) output=-1;else output=0;return output;
}int16_t Fsg_ADRC(float x,float d)
{int16_t output=0;output=(Sign_ADRC(x+d)-Sign_ADRC(x-d))/2;return output;
}void ADRC_Init(Fhan_Data *fhan_Input1,Fhan_Data *fhan_Input2)
{fhan_Input1->r=ADRC_Unit[0][0];fhan_Input1->h=ADRC_Unit[0][1];fhan_Input1->N0=(uint16)(ADRC_Unit[0][2]);fhan_Input1->beta_01=ADRC_Unit[0][3];fhan_Input1->beta_02=ADRC_Unit[0][4];fhan_Input1->beta_03=ADRC_Unit[0][5];fhan_Input1->b0=ADRC_Unit[0][6];fhan_Input1->beta_0=ADRC_Unit[0][7];fhan_Input1->beta_1=ADRC_Unit[0][8];fhan_Input1->beta_2=ADRC_Unit[0][9];fhan_Input1->N1=(uint16)(ADRC_Unit[0][10]);fhan_Input1->c=ADRC_Unit[0][11];fhan_Input1->alpha1=ADRC_Unit[0][12];fhan_Input1->alpha2=ADRC_Unit[0][13];fhan_Input1->zeta=ADRC_Unit[0][14];fhan_Input2->r=ADRC_Unit[1][0];fhan_Input2->h=ADRC_Unit[1][1];fhan_Input2->N0=(uint16)(ADRC_Unit[1][2]);fhan_Input2->beta_01=ADRC_Unit[1][3];fhan_Input2->beta_02=ADRC_Unit[1][4];fhan_Input2->beta_03=ADRC_Unit[1][5];fhan_Input2->b0=ADRC_Unit[1][6];fhan_Input2->beta_0=ADRC_Unit[1][7];fhan_Input2->beta_1=ADRC_Unit[1][8];fhan_Input2->beta_2=ADRC_Unit[1][9];fhan_Input2->N1=(uint16)(ADRC_Unit[1][10]);fhan_Input2->c=ADRC_Unit[1][11];fhan_Input2->alpha1=ADRC_Unit[1][12];fhan_Input2->alpha2=ADRC_Unit[1][13];fhan_Input2->zeta=ADRC_Unit[1][14];
}//ADRC最速跟踪微分器TD，改进的算法fhan
void Fhan_ADRC(Fhan_Data *fhan_Input,float expect_ADRC)//安排ADRC过度过程
{float d=0,a0=0,y=0,a1=0,a2=0,a=0;float x1_delta=0;//ADRC状态跟踪误差项x1_delta=fhan_Input->x1-expect_ADRC;//用x1-v(k)替代x1得到离散更新公式fhan_Input->h0=fhan_Input->N0*fhan_Input->h;//用h0替代h，解决最速跟踪微分器速度超调问题d=fhan_Input->r*fhan_Input->h0*fhan_Input->h0;//d=rh^2;a0=fhan_Input->h0*fhan_Input->x2;//a0=h*x2y=x1_delta+a0;//y=x1+a0a1=sqrt(d*(d+8*ABS(y)));//a1=sqrt(d*(d+8*ABS(y))])a2=a0+Sign_ADRC(y)*(a1-d)/2;//a2=a0+sign(y)*(a1-d)/2;a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d));fhan_Input->fh=-fhan_Input->r*(a/d)*Fsg_ADRC(a,d)-fhan_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));//得到最速微分加速度跟踪量fhan_Input->x1+=fhan_Input->h*fhan_Input->x2;//跟新最速跟踪状态量x1fhan_Input->x2+=fhan_Input->h*fhan_Input->fh;//跟新最速跟踪状态量微分x2
}//原点附近有连线性段的连续幂次函数
float Fal_ADRC(float e,float alpha,float zeta)
{
int16 s=0;
float fal_output=0;
s=(Sign_ADRC(e+zeta)-Sign_ADRC(e-zeta))/2;
fal_output=e*s/(powf(zeta,1-alpha))+powf(ABS(e),alpha)*Sign_ADRC(e)*(1-s);
return fal_output;
}/************扩张状态观测器********************/
//状态观测器参数beta01=1/h  beta02=1/(3*h^2)  beta03=2/(8^2*h^3) ...
void ESO_ADRC(Fhan_Data *fhan_Input)
{fhan_Input->e=fhan_Input->z1-fhan_Input->y;//状态误差fhan_Input->fe=Fal_ADRC(fhan_Input->e,0.5,fhan_Input->h);//非线性函数，提取跟踪状态与当前状态误差fhan_Input->fe1=Fal_ADRC(fhan_Input->e,0.25,fhan_Input->h);/*************扩展状态量更新**********/fhan_Input->z1+=fhan_Input->h*(fhan_Input->z2-fhan_Input->beta_01*fhan_Input->e);fhan_Input->z2+=fhan_Input->h*(fhan_Input->z3-fhan_Input->beta_02*fhan_Input->fe+fhan_Input->b0*fhan_Input->u);//ESO估计状态加速度信号，进行扰动补偿，传统MEMS陀螺仪漂移较大，估计会产生漂移fhan_Input->z3+=fhan_Input->h*(-fhan_Input->beta_03*fhan_Input->fe1);
}/************非线性组合****************/
/*
void Nolinear_Conbination_ADRC(Fhan_Data *fhan_Input)
{float d=0,a0=0,y=0,a1=0,a2=0,a=0;float Sy=0,Sa=0;//ADRC状态跟踪误差项fhan_Input->h1=fhan_Input->N1*fhan_Input->h;d=fhan_Input->r*fhan_Input->h1*fhan_Input->h1;a0=fhan_Input->h1*fhan_Input->c*fhan_Input->e2;y=fhan_Input->e1+a0;a1=sqrt(d*(d+8*ABS(y)));a2=a0+Sign_ADRC(y)*(a1-d)/2;Sy=Fsg_ADRC(y,d);a=(a0+y-a2)*Sy+a2;Sa=Fsg_ADRC(a,d);fhan_Input->u0=-fhan_Input->r*((a/d)-Sign_ADRC(a))*Sa-fhan_Input->r*Sign_ADRC(a);//a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d));//fhan_Input->fh=-fhan_Input->r*(a/d)*Fsg_ADRC(a,d)//                -fhan_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));//得到最速微分加速度跟踪量
}
*/
void Nolinear_Conbination_ADRC(Fhan_Data *fhan_Input)
{float temp_e2=0;temp_e2=Constrain_Float(fhan_Input->e2,-3000,3000);fhan_Input->u0=fhan_Input->beta_1*Fal_ADRC(fhan_Input->e1,fhan_Input->alpha1,fhan_Input->zeta)+fhan_Input->beta_2*Fal_ADRC(temp_e2,fhan_Input->alpha2,fhan_Input->zeta);}void ADRC_Control(Fhan_Data *fhan_Input,float expect_ADRC,float feedback_ADRC)
{/*自抗扰控制器第1步*//**************************//*****安排过度过程，输入为期望给定，由TD跟踪微分器得到：过度期望信号x1，过度期望微分信号x2******/Fhan_ADRC(fhan_Input,expect_ADRC);/*自抗扰控制器第2步*//************************//************系统输出值为反馈量，状态反馈，ESO扩张状态观测器的输入*********/fhan_Input->y=feedback_ADRC;/*****扩张状态观测器，得到反馈信号的扩张状态：1、状态信号z1；2、状态速度信号z2；3、状态加速度信号z3。其中z1、z2用于作为状态反馈与TD微分跟踪器得到的x1,x2做差后，经过非线性函数映射，乘以beta系数后，组合得到未加入状态加速度估计扰动补偿的原始控制量u*********/ESO_ADRC(fhan_Input);//低成本MEMS会产生漂移，扩展出来的z3此项会漂移，目前暂时未想到办法解决，未用到z3/*自抗扰控制器第3步*//**************************//********状态误差反馈率***/fhan_Input->e0+=fhan_Input->e1*fhan_Input->h;//状态积分项fhan_Input->e1=fhan_Input->x1-fhan_Input->z1;//状态偏差项fhan_Input->e2=fhan_Input->x2-fhan_Input->z2;//状态微分项，/********线性组合*******//*fhan_Input->u0=//fhan_Input->beta_0*fhan_Input->e0+fhan_Input->beta_1*fhan_Input->e1+fhan_Input->beta_2*fhan_Input->e2;*/Nolinear_Conbination_ADRC(fhan_Input);/**********扰动补偿*******///fhan_Input->u=fhan_Input->u0//             -fhan_Input->z3/fhan_Input->b0;//由于MEMS传感器漂移比较严重，当beta_03取值比较大时，长时间z3漂移比较大，目前不加入扰动补偿控制量fhan_Input->u=Constrain_Float(fhan_Input->u0,-200,200);
}

无名的ADRC的C语言实现相关推荐

ACfly的Ctrl_Attitude.cpp的代码（可以看到在角速度环和高度环用ADRC）
ACfly的Ctrl_Attitude.cpp的代码 Attitude是姿势的意思我看有个姿态ESO,实际应该是角速度的,还有个高度ESO 在高度上用ADRC可能是为了高度无参自适应,你每次改变无人 ...
ADRC线性跟踪微分器(ST+SCL语言)
ADRC自抗扰相关算法源代码和公式请参看下面文章链接: ADRC/Matlab一步步实现跟踪微分器TD(附完整PLC测试代码链接)_ladrc线性跟踪微分器差分方程_RXXW_Dor的博客-CSDN博 ...
adrc过渡过程 c语言,初步认识ADRC与应用
初步认识ADRC与应用初步认识ADRC与应用这是一个目录ADRC的基本原理一.参考资料推荐二.为什么PID好,以及,为什么PID不够好1.为什么PID好--基于模型的现代控制理论不实用 2.为什 ...
adrc算法思想C语言编,关于ADRC算法以及参数整定（调参）的一些心得体会
关于ADRC算法以及参数整定(调参)的一些心得体会 ADRC,全称叫做Active Disturbance Rejection Control,中文名是自抗扰控制技术.这项控制算法是由中科院的韩京清教 ...
c语言字符串前加l,【C】 30_C语言中的字符串
字符串的概念字符串是有序字符的集合字符串是程序中的基本元素之一 C 语言中没有字符串的概念 C 语言中通过特殊的字符数组模拟字符串 C 语言中的字符串是以 '\0' 结尾的字符数组字符串与字符数 ...
C 语言编程 — 高级数据类型 — 结构体与位域
目录文章目录目录前文列表结构体定义结构体初始化结构体变量访问结构体成员结构体的内存分布将结构体作为实参传入函数指向结构体变量的指针位域定义位域使用位域结构体的成员前文列表 ...
C语言笔试题--从CSDN转发
C语言笔试题--从CSDN转发关键字: 工作,C语言 4.static有什么用途?(请至少说明两种) 1.限制变量的作用域 2.设置变量的存储域 7.引用与指针有什么区别? 1) 引用必须被初始化, ...
C语言中的位域 bit field [转]
一.位域有些信息在存储时,并不需要占用一个完整的字节, 而只需占几个或一个二进制位.例如在存放一个开关量时,只有0和1 两种状态, 用一位 ...
C语言struct中冒号用法
typedef struct { unsigned char unused_bits:4; unsigned char wchair_state:2; } xyz; typedef struct { ...

无名的ADRC的C语言实现

无名的ADRC的C语言实现相关推荐

最新文章

热门文章