adrc算法c语言实现

ADRC（Active Disturbance Rejection Control）算法是一种新型控制算法，它能够有效地抵消系统内外部干扰，提高控制系统的鲁棒性和稳定性。下面是ADRC算法的C语言实现示例：

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

#define Ts 0.001    // 采样时间
#define h 0.01      // 参考模型带宽
#define r 1         // 参考模型恒定输出

float U = 0;        // 控制量
float e = 0;        // 偏差
float e1 = 0;       // 一阶导偏差
float e2 = 0;       // 二阶导偏差
float x1 = 0;       // 系统状态变量一
float x2 = 0;       // 系统状态变量二
float x3 = 0;       // 系统状态变量三
float u1 = 0;       // 内部模型控制量一
float u2 = 0;       // 内部模型控制量二
float a = 0.5;      // ADRC算法参数a
float b = 0.1;      // ADRC算法参数b
float L = 100;      // 观测器增益
float h1 = h;       // 参考模型带宽（一阶）
float h2 = h*h1;    // 参考模型带宽（二阶）

float f(float e, float e1, float a, float b)
{
    return b/(a+Ts) * e1 - (a-Ts)/(a+Ts) * e + b/(a+Ts) * e;
}

float Saturation(float u)
{
    if (u > 1) {
        return 1;
    } else if (u < -1) {
        return -1;
    } else {
        return u;
    }
}

void ADRC(float r, float y, float *u)
{
    // 内部模型
    u2 = u1 + h1 * f(u1-y, x1-u2, a, b);
    u1 = Saturation(u2);

    // 扰动观测器
    e2 = e1 + h2 * f(e1-x3, x2, a, b);
    e1 = e + L * (y-e2);

    // 转换环节
    x1 = x1 + Ts * x2;
    x2 = x2 + Ts * x3;
    x3 = f(e1, x2, a, b);

    // 控制器
    e = r - y;
    *u = Saturation(u1 + x3);
}

int main()
{
    float y = 0;    // 系统输出
    float t = 0;    // 时间

    while(t < 10) {
        ADRC(r, y, &U);
        y = sin(t);   // 系统是正弦信号
        printf("%f, %f\n", t, U);
        t += Ts;
    }

    return 0;
}

这段代码实现了一个简单的ADRC控制器，其中各个参数的含义可以参考ADRC算法的原理。需要注意的是，这里只是一个简单的示例，实际应用中需要根据具体的系统进行参数调整和优化。