用C++写一个自抗扰控制算法

自抗扰控制（Active disturbance rejection control，简称ADRC）是一种新型控制技术，它采用在线估计和抑制干扰的方式来实现控制。下面是一个简单的用C语言编写的ADRC控制算法：

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

#define Ts 0.01 // 采样周期
#define Td 1.0 // 干扰预估时间常数
#define N 7 // 滑动窗口长度

double x[N], y[N], d[N]; // 滑动窗口

double LPF(double u, double y, double T) { // 一阶低通滤波器
    return (1-T/Ts)*u + T/Ts*y;
}

double ADRC(double r, double y) {
    double e = r - y; // 控制误差
    double d0 = LPF(d[N-1], d[N-2], Ts/Td); // 干扰预估值
    double u = LPF(x[N-1] + d0, y[N-2], Ts); // 控制器输出
    double d1 = LPF(d0, d[N-1] - d0, Ts/Td); // 干扰估计值
    for (int i = N-1; i > 0; i--) { // 更新滑动窗口
        x[i] = x[i-1];
        y[i] = y[i-1];
        d[i] = d[i-1];
    }
    x[0] = e; // 更新滑动窗口
    y[0] = y; // 更新滑动窗口
    d[0] = d1; // 更新滑动窗口
    return u;
}

int main() {
    double r = 10.0; // 参考输入
    double y = 0.0; // 实际输出
    for (int i = 0; i < N; i++) { // 初始化滑动窗口
        x[i] = 0.0;
        y[i] = 0.0;
        d[i] = 0.0;
    }
    for (int i = 0; i < 1000; i++) { // 控制循环
        double u = ADRC(r, y); // 计算控制器输出
        y += (u + sin(y))/10.0; // 模拟被控对象
        printf("%f\n", u); // 输出控制器输出
    }
    return 0;
}

该算法实现了一个简单的ADRC控制器，其中包括一阶低通滤波器和滑动窗口来实现干扰预估和抑制。在主函数中，我们初始化了滑动窗口，然后进入控制循环，每次调用ADRC函数计算控制器输出，模拟了一个被控对象，并输出控制器输出。