adrc算法c语言实现

ADRC算法（Active Disturbance Rejection Control）是一种新兴的控制算法，它具有简单、实用、鲁棒性强等特点，已经在控制领域得到了广泛的应用。下面是ADRC算法的C语言实现示例：

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define Ts 0.01 //采样时间
#define h 0.0001 //微分时间

double sign(double x) 
{
    if (x > 0) return 1;
    else if (x < 0) return -1;
    else return 0;
}

double Saturation(double x, double limit) 
{
    if (x > limit) return limit;
    else if (x < -limit) return -limit;
    else return x;
}

double ADRC(double r, double y, double *x, double *z, double *u) 
{
    double e = r - y;
    double dx = z[1] - x[1];
    double ddx = z[2] - x[2];
    double b0 = 2, b1 = -1.5, b2 = 0.5;
    double a1 = -1.9, a2 = 0.9;
    double u1 = u[0], u2 = u[1], u3 = u[2];
    double alpha1 = 10, alpha2 = 50, alpha3 = 50, alpha4 = 1.5, alpha5 = 1.5;
    double beta1 = 1, beta2 = 30, beta3 = 500, beta4 = 1, beta5 = 1;
    double gamma1 = 10, gamma2 = 50, gamma3 = 50, gamma4 = 1.5, gamma5 = 1.5;
    double e1 = e - Saturation(dx, beta1);
    double e2 = dx - Saturation(ddx, beta2);
    double e3 = ddx - b0*u3 - b1*u2 - b2*u1;
    double z1_dot = z[2] + alpha1*e1;
    double z2_dot = z[3] + alpha2*e2;
    double z3_dot = z[4] + alpha3*e3;
    double v1 = z1_dot - dx;
    double v2 = z2_dot - ddx;
    double v3 = z3_dot - b0*u3 - b1*u2 - b2*u1;
    double s1 = sign(v1);
    double s2 = sign(v2);
    double s3 = sign(v3);
    double u1_dot = alpha4*v1 - gamma4*s1;
    double u2_dot = alpha5*v2 - gamma5*s2;
    double u3_dot = gamma1*s1 + gamma2*s2 + gamma3*s3;
    double u_sat = Saturation(u1 + u1_dot*h, beta3);
    u_sat = Saturation(u2 + u2_dot*h, beta4);
    u_sat = Saturation(u3 + u3_dot*h, beta5);
    u[0] = u_sat;
    u[1] = u2;
    u[2] = u1;
    x[0] = z[0];
    x[1] += z[2]*h;
    x[2] += z[3]*h;
    z[0] += z1_dot*Ts;
    z[1] += z2_dot*Ts;
    z[2] += z3_dot*Ts;
    return u_sat;
}

int main()
{
    double r = 10, y = 0, x[3] = {0}, z[5] = {0}, u[3] = {0};
    FILE *fp = fopen("adrc.csv", "w");
    for (int i = 0; i < 1000; i++) {
        double u_adrc = ADRC(r, y, x, z, u);
        y += (-0.5*y + 0.5*u_adrc)*Ts;
        fprintf(fp, "%lf,%lf,%lf,%lf\n", r, y, u_adrc, x[0]);
    }
    fclose(fp);
    return 0;
}

该示例程序实现了一个简单的ADRC控制器，其中r为设定值，y为系统输出值，x为系统状态量，z为估计扰动量，u为控制量。Saturation函数实现了控制量限幅，sign函数实现了符号函数的功能。在主函数中，设定了一个r=10的设定值，并且将系统输出值y、控制量u和状态量x[0]的变化过程保存到了adrc.csv文件中。