From e4fa8f568cd27f2d118de2e1693d2ef8543f81ee Mon Sep 17 00:00:00 2001 From: Alexey Lagoshin Date: Tue, 9 Mar 2021 17:35:02 +0200 Subject: [PATCH 1/4] Use forward Euler method for PIDLoop integration. This ensures, that the very first value won't be missed. --- src/kOS.Safe/Encapsulation/PIDLoop.cs | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/kOS.Safe/Encapsulation/PIDLoop.cs b/src/kOS.Safe/Encapsulation/PIDLoop.cs index c2c36af9c9..f821ae6752 100644 --- a/src/kOS.Safe/Encapsulation/PIDLoop.cs +++ b/src/kOS.Safe/Encapsulation/PIDLoop.cs @@ -80,6 +80,7 @@ public static PIDLoop DeepCopy(PIDLoop source) DTerm = source.DTerm, ExtraUnwind = source.ExtraUnwind, ChangeRate = source.ChangeRate, + lastIError = source.lastIError, unWinding = source.unWinding }; return newLoop; @@ -119,6 +120,7 @@ public static PIDLoop DeepCopy(PIDLoop source) public double ChangeRate { get; set; } + private double lastIError; private bool unWinding; public PIDLoop() @@ -198,7 +200,11 @@ public ScalarValue Update(ScalarValue sampleTime, ScalarValue input) double pTerm = error * Kp; double iTerm = 0; double dTerm = 0; - if (LastSampleTime < sampleTime) + if (LastSampleTime == double.MaxValue) + { + lastIError = error * Ki; + } + else if (LastSampleTime < sampleTime) { double dt = sampleTime - LastSampleTime; if (Ki != 0) @@ -219,7 +225,8 @@ public ScalarValue Update(ScalarValue sampleTime, ScalarValue input) unWinding = false; } } - iTerm = ITerm + error * dt * Ki; + iTerm = ITerm + lastIError * dt; + lastIError = error * Ki; } ChangeRate = (input - Input) / dt; if (Kd != 0) From c1f4dff450b184a7d36a8cd063a7bc75e2647369 Mon Sep 17 00:00:00 2001 From: Alexey Lagoshin Date: Wed, 10 Mar 2021 09:18:39 +0200 Subject: [PATCH 2/4] Add unit tests for PIDLoop --- src/kOS.Safe.Test/Structure/PIDLoopTest.cs | 57 ++++++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 src/kOS.Safe.Test/Structure/PIDLoopTest.cs diff --git a/src/kOS.Safe.Test/Structure/PIDLoopTest.cs b/src/kOS.Safe.Test/Structure/PIDLoopTest.cs new file mode 100644 index 0000000000..e916e9c6e0 --- /dev/null +++ b/src/kOS.Safe.Test/Structure/PIDLoopTest.cs @@ -0,0 +1,57 @@ +using System; +using kOS.Safe.Encapsulation; +using NUnit.Framework; + +namespace kOS.Safe.Test.Structure +{ + [TestFixture] + public class PIDLoopTest + { + + private double[] input; + + [SetUp] + public void Setup() + { + input = new double[] {-10, -10, -10, -10, 6, 6, 6, 6, 6, 6}; + } + + [Test] + public void CanCorrectlyReactToSineWaveInput() + { + var pidLoop = new PIDLoop(1, 1, 1); + double[] sineWaveInput = {0, -4.207, -4.547, -0.706, 3.784, 4.795, 1.397, -3.285, -4.947, -2.060}; + double[] output = new double[10]; + double[] pTermOutput = new double[10]; + double[] iTermOutput = new double[10]; + double[] dTermOutput = new double[10]; + + for (var i = 0; i < sineWaveInput.Length; i++) + { + output[i] = Math.Round((double) pidLoop.Update(i, sineWaveInput[i]), 2); + pTermOutput[i] = Math.Round(pidLoop.PTerm, 2); + iTermOutput[i] = Math.Round(pidLoop.ITerm, 2); + dTermOutput[i] = Math.Round(pidLoop.DTerm, 2); + } + + Assert.AreEqual(new double[] {0, 4.21, 4.55, 0.71, -3.78, -4.80, -1.40, 3.28, 4.95, 2.06}, pTermOutput, "Proportional part is incorrect"); + Assert.AreEqual(new double[] {0, 0, 4.21, 8.75, 9.46, 5.68, 0.88, -0.52, 2.77, 7.72}, iTermOutput, "Integral part is incorrect"); + Assert.AreEqual(new double[] {0, 4.21, 0.34, -3.84, -4.49, -1.01, 3.40, 4.68, 1.66, -2.89}, dTermOutput, "Derivative part is incorrect"); + Assert.AreEqual(new double[] {0, 8.41, 9.09, 5.62, 1.19, -0.13, 2.88, 7.45, 9.38, 6.89}, output, "PID output is incorrect"); + } + + [Test] + public void CanAntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + double[] output = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + output[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 5, 0, 0, 0, 0}, output); + } + } +} From 00d84d70a152e7d138b7aeed34fb8aa4f1cd3019 Mon Sep 17 00:00:00 2001 From: Alexey Lagoshin Date: Wed, 10 Mar 2021 20:55:47 +0200 Subject: [PATCH 3/4] Add more anti windup modes to PIDLoop --- src/kOS.Safe.Test/Structure/PIDLoopTest.cs | 61 ++++++++ src/kOS.Safe/Encapsulation/PIDLoop.cs | 171 ++++++++++++++------- 2 files changed, 175 insertions(+), 57 deletions(-) diff --git a/src/kOS.Safe.Test/Structure/PIDLoopTest.cs b/src/kOS.Safe.Test/Structure/PIDLoopTest.cs index e916e9c6e0..0e69953b96 100644 --- a/src/kOS.Safe.Test/Structure/PIDLoopTest.cs +++ b/src/kOS.Safe.Test/Structure/PIDLoopTest.cs @@ -53,5 +53,66 @@ public void CanAntiWindUp() Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 5, 0, 0, 0, 0}, output); } + + [Test] + public void CanNoneAntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + pidLoop.AntiWindupMode = "NONE"; + double[] outputs = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + outputs[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 11, 11, 11, 11, 10}, outputs); + } + + [Test] + public void CanClampingAntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + pidLoop.AntiWindupMode = "CLAMPING"; + double[] outputs = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + outputs[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 11, 8, 2, 0, 0}, outputs); + } + + [Test] + public void CanBackCalculationAntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + pidLoop.AntiWindupMode = "BACK-CALC"; + double[] outputs = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + outputs[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 5, 0, 0, 0, 0}, outputs); + } + + [Test] + public void CanBackCalculationWithK2AntiWindUp() + { + var pidLoop = new PIDLoop(0, 1, 0, 11, 0); + pidLoop.AntiWindupMode = "BACK-CALC"; + pidLoop.KBackCalc = 2; + double[] outputs = new double[10]; + + for (var i = 0; i < input.Length; i++) + { + outputs[i] = pidLoop.Update(i, input[i]); + } + + Assert.AreEqual(new double[] {0, 10, 11, 11, 11, 0, 0, 0, 0, 0}, outputs); + } } } diff --git a/src/kOS.Safe/Encapsulation/PIDLoop.cs b/src/kOS.Safe/Encapsulation/PIDLoop.cs index f821ae6752..1e402df61e 100644 --- a/src/kOS.Safe/Encapsulation/PIDLoop.cs +++ b/src/kOS.Safe/Encapsulation/PIDLoop.cs @@ -62,8 +62,7 @@ public override void Execute(SafeSharedObjects shared) public static PIDLoop DeepCopy(PIDLoop source) { - PIDLoop newLoop = new PIDLoop - { + PIDLoop newLoop = new PIDLoop { LastSampleTime = source.LastSampleTime, Kp = source.Kp, Ki = source.Ki, @@ -74,6 +73,8 @@ public static PIDLoop DeepCopy(PIDLoop source) Output = source.Output, MinOutput = source.MinOutput, MaxOutput = source.MaxOutput, + AntiWindupMode = source.AntiWindupMode, + KBackCalc = source.KBackCalc, ErrorSum = source.ErrorSum, PTerm = source.PTerm, ITerm = source.ITerm, @@ -106,6 +107,10 @@ public static PIDLoop DeepCopy(PIDLoop source) public double MaxOutput { get; set; } + public string AntiWindupMode { get; set; } + + public double KBackCalc { get; set; } + public double Epsilon { get; set; } public double ErrorSum { get; set; } @@ -140,6 +145,8 @@ public PIDLoop(double kp, double ki, double kd, double maxoutput = double.MaxVal Output = 0; MaxOutput = maxoutput; MinOutput = minoutput; + AntiWindupMode = "DEFAULT"; + KBackCalc = 1; Epsilon = nullzone; ErrorSum = 0; PTerm = 0; @@ -161,6 +168,8 @@ public void InitializeSuffixes() AddSuffix("OUTPUT", new Suffix(() => Output)); AddSuffix("MAXOUTPUT", new SetSuffix(() => MaxOutput, value => MaxOutput = value)); AddSuffix("MINOUTPUT", new SetSuffix(() => MinOutput, value => MinOutput = value)); + AddSuffix("ANTIWINDUPMODE", new SetSuffix(() => AntiWindupMode, value => AntiWindupMode = value)); + AddSuffix("KBACKCALC", new SetSuffix(() => KBackCalc, value => KBackCalc = value)); AddSuffix(new string[] { "IGNOREERROR", "EPSILON" }, new SetSuffix(() => Epsilon, value => Epsilon = value)); AddSuffix("ERRORSUM", new Suffix(() => ErrorSum)); AddSuffix("PTERM", new Suffix(() => PTerm)); @@ -187,84 +196,128 @@ public double Update(double sampleTime, double input, double setpoint, double ma public ScalarValue Update(ScalarValue sampleTime, ScalarValue input) { - double error = Setpoint - input; - if (error > -Epsilon && error < Epsilon) + Error = Setpoint - input; + if (Error > -Epsilon && Error < Epsilon) { // Pretend there is no error (get everything to zero out) // because the error is within the epsilon: - error = 0; + Error = 0; input = Setpoint; Input = input; - Error = error; } - double pTerm = error * Kp; - double iTerm = 0; - double dTerm = 0; + + PTerm = Error * Kp; + if (LastSampleTime == double.MaxValue) { - lastIError = error * Ki; + lastIError = Error * Ki; } else if (LastSampleTime < sampleTime) { double dt = sampleTime - LastSampleTime; + + ChangeRate = (input - Input) / dt; + DTerm = -ChangeRate * Kd; + if (Ki != 0) { - if (ExtraUnwind) + ExtraUnwindIfEnabled(); + + ITerm += lastIError * dt; + lastIError = AntiWindup(Error * Ki); + } + else + { + ITerm = 0; + lastIError = Error * Ki; + } + } + + LastSampleTime = sampleTime; + Input = input; + if (Ki != 0) ErrorSum = ITerm / Ki; + else ErrorSum = 0; + + // Limit output according to MinOutput and MaxOutput + Output = Math.Min(MaxOutput, Math.Max(MinOutput, PTerm + ITerm + DTerm)); + return Output; + } + + private void ExtraUnwindIfEnabled() + { + if (ExtraUnwind) + { + if (Math.Sign(Error) != Math.Sign(ErrorSum)) + { + if (!unWinding) { - if (Math.Sign(error) != Math.Sign(ErrorSum)) - { - if (!unWinding) - { - Ki *= 2; - unWinding = true; - } - } - else if (unWinding) - { - Ki /= 2; - unWinding = false; - } + Ki *= 2; + unWinding = true; } - iTerm = ITerm + lastIError * dt; - lastIError = error * Ki; } - ChangeRate = (input - Input) / dt; - if (Kd != 0) + else if (unWinding) { - dTerm = -ChangeRate * Kd; + Ki /= 2; + unWinding = false; } } - else + } + + private double AntiWindup(double iError) + { + double preSatOutput = PTerm + ITerm + DTerm; + + switch (AntiWindupMode.ToUpper()) { - dTerm = DTerm; - iTerm = ITerm; + case "NONE": + return iError; + case "CLAMPING": + return ClampingAntiWindup(preSatOutput, iError); + case "BACK-CALC": + return BackCalculationAntiWindup(preSatOutput, iError); + default: + return DefaultAntiWindup(preSatOutput, iError); } - Output = pTerm + iTerm + dTerm; - if (Output > MaxOutput) + } + + private double ClampingAntiWindup(double preSatOutput, double preIntError) + { + double preSatSign = 0; + if (preSatOutput > MaxOutput) { - Output = MaxOutput; - if (Ki != 0 && LastSampleTime < sampleTime) - { - iTerm = Output - Math.Min(pTerm + dTerm, MaxOutput); - } + preSatSign = 1; + } + else if (preSatOutput < MinOutput) + { + preSatSign = -1; } - if (Output < MinOutput) + + if (preSatSign != 0 && Math.Sign(preSatSign) == Math.Sign(preIntError)) { - Output = MinOutput; - if (Ki != 0 && LastSampleTime < sampleTime) - { - iTerm = Output - Math.Max(pTerm + dTerm, MinOutput); - } + preIntError = 0; } - LastSampleTime = sampleTime; - Input = input; - Error = error; - PTerm = pTerm; - ITerm = iTerm; - DTerm = dTerm; - if (Ki != 0) ErrorSum = iTerm / Ki; - else ErrorSum = 0; - return Output; + + return preIntError; + } + + private double BackCalculationAntiWindup(double preSatOutput, double preIntError) + { + double postSatOutput = Math.Min(MaxOutput, Math.Max(MinOutput, preSatOutput)); + return (postSatOutput - preSatOutput) * KBackCalc + preIntError; + } + + private double DefaultAntiWindup(double preSatOutput, double preIntError) + { + if (preSatOutput > MaxOutput) + { + ITerm = MaxOutput - Math.Min(PTerm + DTerm, MaxOutput); + } + if (preSatOutput < MinOutput) + { + ITerm = MinOutput - Math.Max(PTerm + DTerm, MinOutput); + } + + return preIntError; } public void ResetI() @@ -308,12 +361,14 @@ public override Dump Dump() result.Add("Setpoint", Setpoint); result.Add("MaxOutput", MaxOutput); result.Add("MinOutput", MinOutput); + result.Add("AntiWindupMode", AntiWindupMode); + result.Add("KBackCalc", KBackCalc); result.Add("ExtraUnwind", ExtraUnwind); return result; - } + } - public override void LoadDump(Dump dump) + public override void LoadDump(Dump dump) { Kp = Convert.ToDouble(dump["Kp"]); Ki = Convert.ToDouble(dump["Ki"]); @@ -321,7 +376,9 @@ public override void LoadDump(Dump dump) Setpoint = Convert.ToDouble(dump["Setpoint"]); MinOutput = Convert.ToDouble(dump["MinOutput"]); MaxOutput = Convert.ToDouble(dump["MaxOutput"]); + AntiWindupMode = Convert.ToString(dump["AntiWindupMode"]); + KBackCalc = Convert.ToDouble(dump["KBackCalc"]); ExtraUnwind = Convert.ToBoolean(dump["ExtraUnwind"]); } } -} \ No newline at end of file +} From a67d7073647761777faf905b5ae067f98e4ac2d1 Mon Sep 17 00:00:00 2001 From: Alexey Lagoshin Date: Thu, 1 Apr 2021 00:40:14 +0300 Subject: [PATCH 4/4] Derivative part of PID should work measure the error, not input --- src/kOS.Safe/Encapsulation/PIDLoop.cs | 32 ++++++++++++--------------- 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/src/kOS.Safe/Encapsulation/PIDLoop.cs b/src/kOS.Safe/Encapsulation/PIDLoop.cs index 1e402df61e..814a004df6 100644 --- a/src/kOS.Safe/Encapsulation/PIDLoop.cs +++ b/src/kOS.Safe/Encapsulation/PIDLoop.cs @@ -196,45 +196,41 @@ public double Update(double sampleTime, double input, double setpoint, double ma public ScalarValue Update(ScalarValue sampleTime, ScalarValue input) { - Error = Setpoint - input; - if (Error > -Epsilon && Error < Epsilon) + if (LastSampleTime == sampleTime) return Output; + + double error = Setpoint - input; + if (error > -Epsilon && error < Epsilon) { // Pretend there is no error (get everything to zero out) // because the error is within the epsilon: - Error = 0; + error = 0; input = Setpoint; - Input = input; } - PTerm = Error * Kp; + PTerm = error * Kp; - if (LastSampleTime == double.MaxValue) - { - lastIError = Error * Ki; - } - else if (LastSampleTime < sampleTime) + if (LastSampleTime < sampleTime) { double dt = sampleTime - LastSampleTime; - ChangeRate = (input - Input) / dt; - DTerm = -ChangeRate * Kd; + ChangeRate = (error - Error) / dt; + DTerm = ChangeRate * Kd; if (Ki != 0) { - ExtraUnwindIfEnabled(); - + ExtraUnwindIfEnabled(error); ITerm += lastIError * dt; - lastIError = AntiWindup(Error * Ki); } else { ITerm = 0; - lastIError = Error * Ki; } } + lastIError = AntiWindup(error * Ki); LastSampleTime = sampleTime; Input = input; + Error = error; if (Ki != 0) ErrorSum = ITerm / Ki; else ErrorSum = 0; @@ -243,11 +239,11 @@ public ScalarValue Update(ScalarValue sampleTime, ScalarValue input) return Output; } - private void ExtraUnwindIfEnabled() + private void ExtraUnwindIfEnabled(double error) { if (ExtraUnwind) { - if (Math.Sign(Error) != Math.Sign(ErrorSum)) + if (Math.Sign(error) != Math.Sign(ErrorSum)) { if (!unWinding) {