PID：导数低通与条件积分抗饱和

题目

初始 $I=D=e_{\rm prev}=0$。每步先计算 $$D\leftarrow\frac{\tau}{\tau+\Delta t}D+\frac{e-e_{\rm prev}}{\tau+\Delta t},\quad I_{\rm try}=I+K_i e\Delta t,\quad u_{\rm try}=K_p e+I_{\rm try}+K_dD.$$ 若 $u_{\rm try}>L$ 且 $K_i e>0$，或 $u_{\rm try}<-L$ 且 $K_i e<0$，则拒绝本次积分，否则 $I\leftarrow I_{\rm try}$。用更新后的积分算 $$u=\operatorname{clamp}(K_p e+I+K_dD,-L,L),$$ 最后 $e_{\rm prev}\leftarrow e$。

回到相关教材 →

输入

$N,\Delta t,K_p,K_i,K_d,\tau,L$，然后 $N$ 个误差。$1\le N\le100$，$\Delta t>0$，$\tau\ge0$，$L>0$，增益非负。

输出

每步 $u,I,D$，依次输出共 $3N$ 个实数。

示例

输入

25 0.1 2 0.4 0.3 0.2 1
5 5 5 5 5 5 5 5 5 5 -5 -5 -5 -5 -5 -5 -5 -5 -5 -5 0 0 0 0 0

期望输出

1 0 16.666666666666664 1 0 11.111111111111109 1 0 7.407407407407406 1 0 4.93827160493827 1 0 3.292181069958847 1 0 2.1947873799725643 1 0 1.4631915866483762 1 0 0.9754610577655841 1 0 0.6503073718437227 1 0 0.4335382478958151 -1 0 -33.04430783473612 -1 0 -22.029538556490742 -1 0 -14.686359037660495 -1 0 -9.790906025106995 -1 0 -6.527270683404663 -1 0 -4.351513788936442 -1 0 -2.9010091926242945 -1 0 -1.9340061284161962 -1 0 -1.2893374189441307 -1 0 -0.8595582792960871 1 0.0 16.093627813802605 1 0.0 10.729085209201736 1 0.0 7.152723472801157 1 0.0 4.768482315200771 0.9536964630401541 0.0 3.178988210133847

#include <iostream>
#include <iomanip>
#include <cmath>
#include <algorithm>
#include <vector>
#include <array>
#include <queue>
#include <tuple>
#include <limits>
using namespace std;

// Reference implementation: optional; solve the starter before opening.
int main() {
    cout << setprecision(17);
    int n;
    double dt,kp,ki,kd,tau,L;
    cin>>n>>dt>>kp>>ki>>kd>>tau>>L;
    double I=0,D=0,prev=0;
    while(n--) {
        double e;
        cin>>e;
        D=tau/(tau+dt)*D+(e-prev)/(tau+dt);
        double trial=I+ki*e*dt,raw=kp*e+trial+kd*D;
        if(!((raw>L&&ki*e>0)||(raw<-L&&ki*e<0)))I=trial;
        cout<<clamp(kp*e+I+kd*D,-L,L)<<" "<<I<<" "<<D<<"\n";
        prev=e;
    }
}