使用ADRC替换掉ardupilot中的PID控制器，首先我们需要借助simulink搭建ADRC控制器，进行初步调参，仿真通过后利用simulink to C 生成我们想要的ADRC代码，插入到ardupilot中，最终调参得到ADRC飞控，开发思路借鉴基于模型的开发思路，真实飞机控制效果还待验证，已经通过Ardupilot中SITL仿真。

Adrc控制器

ADRC的控制框图如下：主要包括跟踪微分器（TD）、非线性状态误差反馈（NLSEF）、扩张状态观测器（ESO）。引用

L-ADRC中最重要的是ESO，ESO的核心思想是建立新的状态方程，输入被观测模型的输出与被观测模型的输入，将状态方程对被观测模型进行辨识，最终得到被观测模型的响应特性以及外部扰动，而ADRC的主要思想是对ESO观测出的外部扰动量进行补偿（博主认为就是将扰动作为前馈）。ESO的公式：

在simulink中搭建被控对象的模型，搭建ADRC控制器。

面对ADRC的众多参数，可以参考韩京清教授的书对参数进行简化，甚至可以用粒子群算法进行寻优，此处simulink中仿真步长建议和Ardupilot中控制器更新频率保持一致400HZ。

博主大概调到一个不错的效果就进行下一步了。

Simulink to C++

这里使用Simulink自带模块Embedded coder，将系统中的control system单拿出来进行自动代码生成。Embedded coder功能见下图，博主使用的是MATLAB 2020A，功能模块是下图这样的。使用图中红框的Quick start向导进行设置，会检测你的模型是否有错误，编译通过会生成代码。

Ardupilot PID库替换

自动代码生成源文件

Ardupilot中PID库主要用在角速率环使用，而我的ADRC正是要换掉这部分的控制器，那让我们开始。
首先simulink生成的代码更新函数是rt_OneStep()。这个函数位于simulink生成代码XXX(你的.slx名).cpp中。

我们需要的ADRC库文件就是生成的文件中所有文件。

PID库

此处博主直接对APM的PID库进行修改。首先将ADRC库进行实例化，添加进AC_PID类（libraries/AC_PID/AC_PID.h）的末尾。

 sModelClass ADRC_model_Obj;

将ADRC参数的初始化加入到AC_PID的构造函数中(libraries/AC_PID/AC_PID.cpp)。

ADRC_model_Obj.initialize();

Ardupilot中对pid的更新是调用了

float update_all(float target, float measurement, bool limit = false);	

我们需要将ADRC的更新函数添加到update_all中

float AC_PID::update_all(float target, float measurement, bool limit)
{
    // don't process inf or NaN
    if (!isfinite(target) || !isfinite(measurement)) {
        return 0.0f;
    }

    // reset input filter to value received
    if (_flags._reset_filter) {
        _flags._reset_filter = false;
        _target = target;
        _error = _target - measurement;
        _derivative = 0.0f;
    } else {
        float error_last = _error;
        _target += get_filt_T_alpha() * (target - _target);
        _error += get_filt_E_alpha() * ((_target - measurement) - _error);

        // calculate and filter derivative
        if (_dt > 0.0f) {
            float derivative = (_error - error_last) / _dt;
            _derivative += get_filt_D_alpha() * (derivative - _derivative);
        }
    }

    // update I term
    update_i(limit);

    float P_out = (_error * _kp);
    float D_out = (_derivative * _kd);

    _pid_info.target = _target;
    _pid_info.actual = measurement;
    _pid_info.error = _error;
    _pid_info.P = P_out;
    _pid_info.D = D_out;
    
    ADRC_model_Obj.b=_b;								//ADRC参数b的更新
    ADRC_model_Obj.rtU.target_angle=_target;			//ADRC目标量
    ADRC_model_Obj.rtU.zhuangtai=measurement;		//ADRC ESO需要的反馈回来的信息
    ADRC_model_Obj._kp=_kp;ADRC_model_Obj._ki=_ki;ADRC_model_Obj._kd=_kd;		//ADRC线性组合两个参数的更新
    ADRC_model_Obj._kimax=_kimax;ADRC_model_Obj._kimin=-_kimax;
    ADRC_model_Obj._dt=_dt;


    rt_OneStep();						//ADRC的更新函数
    return ADRC_model_Obj.rtY.caozong;	//控制器返回ADRC的输出
}

以上完成了ADRC控制器的添加。

参数添加

ADRC中存在大量可调参数，根据系统带宽已经可以减少掉许多参数，但是这里我还需在地面站中添加几个参数，方便我们调试。
在AC_PID类中添加一个变量，作为接受地面站设置的参数。

AP_Float _b;

每个库的参数表会放在.cpp文件的开始，AC_PID库也不例外，在AC_PID.cpp的开头的const AP_Param::GroupInfo AC_PID::var_info[] 中添加

AP_GROUPINFO("b", 12, AC_PID, _b, 200),

其中b是地面站中显示的参数，注意还会再添加前缀显示再地面站，这里应该会最终显示为ATC_RAT_PIT_b，会随着调用pid库的类改变而增加前缀,具体参照我那篇添加参数的博客。_b是库中接受地面站参数的变量。

仿真&实验

仿真和试飞两项工作博主只完成了第一项，效果不错，利用Ardupilot提供的软件在环仿真工具包进行ADRC控制器的调参仿真。直接使用下列指令

./Tools/autotest/sim_vehicle.py -v ArduCopter --console --output 192.168.145.1 --map -D 

直接和windows下的地面站联合仿真，ps（本人不太会Xplane与ardupilot联合仿真，听说Xplane可以自建新机型，求教）。