adrc算法c语言实现

ADRC（Active Disturbance Rejection Control）算法是一种新型的控制算法，它可以有效地抑制系统的干扰和误差，提高系统的控制精度和鲁棒性。下面是ADRC算法的C语言实现代码：

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

/* 定义ADRC算法结构体 */
typedef struct {
    double h;           // 预估器带宽参数
    double beta;        // 预估器比例增益参数
    double gamma;       // 预估器微分增益参数
    double r;           // 观测器带宽参数
    double delta;       // 观测器比例增益参数
    double a1;          // 观测器一阶惯性环节参数
    double a2;          // 观测器二阶惯性环节参数
    double x1;          // 观测器一阶状态变量
    double x2;          // 观测器二阶状态变量
    double b0;          // 控制器比例增益参数
    double b1;          // 控制器微分增益参数
    double u;           // 控制器输出
    double e;           // 控制器误差
    double ed;          // 控制器误差微分
    double y;           // 控制器反馈量
} adrc_t;

/* 初始化ADRC算法结构体 */
void adrc_init(adrc_t *adrc, double h, double beta, double gamma, double r, double delta, double a1, double a2, double b0, double b1) {
    adrc->h = h;
    adrc->beta = beta;
    adrc->gamma = gamma;
    adrc->r = r;
    adrc->delta = delta;
    adrc->a1 = a1;
    adrc->a2 = a2;
    adrc->x1 = 0;
    adrc->x2 = 0;
    adrc->b0 = b0;
    adrc->b1 = b1;
    adrc->u = 0;
    adrc->e = 0;
    adrc->ed = 0;
    adrc->y = 0;
}

/* ADRC算法控制函数 */
double adrc_control(adrc_t *adrc, double input, double feedback) {
    double vd, d, v1, v2, v3;

    /* 计算预估器输出 */
    vd = adrc->beta * adrc->e - adrc->gamma * adrc->x2;

    /* 更新预估器状态 */
    v1 = adrc->x2 + adrc->h * (-adrc->r * adrc->x2 + adrc->r * (input - vd));
    v2 = adrc->x1 + adrc->h * adrc->x2;
    adrc->x1 = v1;
    adrc->x2 = v2;

    /* 计算观测器输出 */
    d = adrc->delta * adrc->e - adrc->a1 * adrc->x1 - adrc->a2 * adrc->x2;

    /* 计算控制器输出 */
    adrc->ed = feedback - adrc->y;
    adrc->e = d;
    v3 = adrc->b0 * adrc->e + adrc->b1 * adrc->ed;
    adrc->u = v3;

    /* 更新控制器状态 */
    adrc->y = adrc->y + adrc->h * adrc->u;

    return adrc->u;
}

int main() {
    adrc_t adrc;
    double input, feedback, output;
    int i;

    /* 初始化ADRC算法结构体 */
    adrc_init(&adrc, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1, 0.1);

    /* 模拟ADRC控制器 */
    for (i = 0; i < 1000; i++) {
        input = sin(i * 0.01);
        feedback = output;
        output = adrc_control(&adrc, input, feedback);
        printf("%lf\n", output);
    }

    return 0;
}

在上面的代码中，我们首先定义了一个ADRC算法结构体，包含了ADRC算法中的各个参数和状态变量。然后，我们定义了初始化ADRC算法结构体的函数adrc_init()和控制函数adrc_control()。最后，在main()函数中，我们使用了adrc_control()函数模拟了ADRC控制器的控制过程。