1 参考链接

Chenglin Li：自抗扰控制理论（十七）使用C++实现LADRC封装类

2 相关程序

#include<stdio.h>
#include<stdlib.h>
//#include<math.h>
//ODE参数数量声明
#define ODE_NUM 2

//静态全局变量的声明
static double _h;// 步长
static double _t1; //初始和末尾时刻 
//LESO和控制器参数
static double _ref;
static double _y;
static double _u;
static double _Wo;//LESO带宽
static double _Wc;//控制带宽
static double _b;//控制输入系数
static double beta_Wo[ODE_NUM + 1];//LESO系数
static double beta_Wc[ODE_NUM];//控制器系数
//static double *_z[ODE_NUM + 1] = { 0 }; //表示LESO返回值
static double *_z=NULL;
static double _x[ODE_NUM + 1] = { 0 };////表示RK计算的当前节点值
 
static double _t;
static double _s[ODE_NUM + 1] = { 0 };
static double *y1 = NULL, *y2 = NULL, *y3 = NULL, *y4 = NULL;

//子函数声明
void calcCoefficient();
double powj(double Wo, int n);
double comb(int n, int m);
double jiecheng(int n);
double* LESO(double t, double x[]);
void calc();
void Lx();
void Controller();
double Output();

//主函数

int main()
{
	//参数初始化
	_u = 0;
	_Wo = 10;
	_Wc = 10;
	_b = 1;
	_y = 0.2;
	_h = 0.02;
	_ref = 1;
	Controller();//计算函数
	_u = Output();
	printf("u=%.3f\n", _u);

	return 0;
}

//控制量输出
double Output()
{
	return _u;
}
//控制器计算函数
//vector<double> fx(double t, double x[]);
void Controller()
{
	int i;
	calcCoefficient(); 
	calc();
	_u = _z[ODE_NUM]+beta_Wc[0]*(_z[0]-_ref);//取z3
	for (i = 1; i < ODE_NUM; i++)
	{
		_u = _u + beta_Wc[i] * _z[i];
	}
	_u = -_u / _b;
}
//迭代计算函数 
void calc()
{
	int j;
	//vector<double> s(ODE_NUM+1,0);
	Lx();
	//vector<double> _s(ODE_NUM + 1, 0);
	for (j = 0; j<ODE_NUM + 1; j++)
		_x[j] = _x[j] + _s[j];//更新当前ODE计算的值
	_t = _t + _h;// 更新时间节点
	_z = _x;//返回当前节点解算的LESO

}
//RungeKutta计算所需项 
//vector<double>  LADRC::Lx(double _t, vector<double> _x)
void Lx()
{
	int j;
	//vector<double> x1(ODE_NUM + 1, 0), x2(ODE_NUM + 1, 0), x3(ODE_NUM + 1, 0);
	//vector<double> y1(ODE_NUM + 1, 0), y2(ODE_NUM + 1, 0), y3(ODE_NUM + 1, 0), y4(ODE_NUM + 1, 0);// 使用vector数据类型，子函数可返回数组
	//N = ODE_NUM + 1;
	//vector<double> _s(ODE_NUM + 1, 0);//
	double x1[ODE_NUM + 1] = { 0 }, x2[ODE_NUM + 1] = { 0 }, x3[ODE_NUM + 1] = { 0 };
	y1 = LESO(_t, _x);


	for (j = 0; j<ODE_NUM + 1; j++)
		x1[j] = _x[j] + _h / 2 * y1[j];
	y2 = LESO(_t + _h / 2, x1);
	for (j = 0; j<ODE_NUM + 1; j++)
		x2[j] = _x[j] + _h / 2 * y2[j];
	y3 = LESO(_t + _h / 2, x2);
	for (j = 0; j<ODE_NUM + 1; j++)
		x3[j] = _x[j] + _h*y3[j];
	y4 = LESO(_t + _h, x3);
	for (j = 0; j<ODE_NUM + 1; j++)
		_s[j] = _s[j] + y1[j];
	for (j = 0; j<ODE_NUM + 1; j++)
		_s[j] = _s[j] + 2 * y2[j];
	for (j = 0; j<ODE_NUM + 1; j++)
		_s[j] = _s[j] + 2 * y3[j];
	for (j = 0; j<ODE_NUM + 1; j++)
		_s[j] = _s[j] + y4[j];
	for (j = 0; j < ODE_NUM + 1; j++)
		_s[j] = _s[j] * _h / 6;

}

//计算观测器和控制器系数
void calcCoefficient()
{
	int i;
	//生成LESO系数
	for (i = 0; i < ODE_NUM + 1; i++)
	{
		beta_Wo[i] = powj(_Wo, i + 1)*comb(ODE_NUM + 1, i + 1);

	}
	//生成LESO系数
	for (i = 0; i < ODE_NUM + 1; i++)
	{
		beta_Wo[i] = powj(_Wc, i + 1)*comb(ODE_NUM + 1, i + 1);
	}
	//控制律系数
	for (i = 0; i < ODE_NUM; i++)
	{
		beta_Wc[i] = powj(_Wc, ODE_NUM - i)*comb(ODE_NUM, ODE_NUM - i);
	}
	for (i = 0; i < ODE_NUM; i++)
		printf("beta_Wc[%d]=%.3f\n",i,beta_Wc[i]);

	for (i = 0; i < ODE_NUM + 1; i++)
		printf("beta_Wo[%d]=%.3f\n", i,beta_Wo[i]);
}

// 计算幂函数
double powj(double Wo, int n)
{
	int i;
	double z = 1;
	if (n == 0)
		return 1;
	else
	{
		for (i = n; i >= 1; i--)
		{
			z = z*Wo;
		}
	}
	return z;

}
//组合数计算公式
double comb(int n, int m)
{
	if (n < 0 || n < 0)
		return 0;
	else
		return jiecheng(n) / jiecheng(m) / jiecheng(n - m);

}
//计算阶乘子函数
double jiecheng(int n)
{
	if (n < 0)
		return 0;
	else if (n == 0 || n == 1)
		return 1.0;
	else
		return n*jiecheng(n - 1);
}

//LESO微分方程组定义
double* LESO(double t, double x[])
{
	int i;
	double e;
	double dx[ODE_NUM + 1] = { 0 };//
	e = _y - _x[0];
	for (i = 0; i < ODE_NUM - 1; i++)
	{
		dx[i] = beta_Wo[i] * e + x[i + 1];
	}
	dx[ODE_NUM - 1] = beta_Wo[ODE_NUM - 1] * e + _b*_u;
	dx[ODE_NUM] = beta_Wo[ODE_NUM] * e;
	return dx;

}