今まで使っていたPID制御に問題

5,6年前に作ったPID制御のプログラムで、飽和処理に問題が見つかった。
今まで使っていた計算式は以下の通りである。

制御量 = 前回制御量 +
P × (今回誤差 – 前回誤差) +
I × 今回誤差 +
D × (今回誤差 – 2 × 前回誤差 + 前前回誤差)

制御量が飽和した際には強制的に最大、あるいは最小制御量にクリップしていたのだが、この計算式でそれをやると制御が破綻ししてしまうことに今頃気づいた。

積分ゲインが少しでもあれば動作自体はするため今まで気づかなかったが、早急に直さなければ。