Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Pidloop improvements #2922

Open
wants to merge 4 commits into
base: develop
Choose a base branch
from
Open

Pidloop improvements #2922

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e4fa8f5
Use forward Euler method for PIDLoop integration.
ayzenquwe Mar 9, 2021
c1f4dff
Add unit tests for PIDLoop
ayzenquwe Mar 10, 2021
00d84d7
Add more anti windup modes to PIDLoop
ayzenquwe Mar 10, 2021
a67d707
Derivative part of PID should work measure the error, not input
ayzenquwe Mar 31, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
61 changes: 61 additions & 0 deletions src/kOS.Safe.Test/Structure/PIDLoopTest.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}
171 changes: 114 additions & 57 deletions src/kOS.Safe/Encapsulation/PIDLoop.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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; }
Expand Down Expand Up @@ -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;
Expand All @@ -161,6 +168,8 @@ public void InitializeSuffixes()
AddSuffix("OUTPUT", new Suffix<ScalarValue>(() => Output));
AddSuffix("MAXOUTPUT", new SetSuffix<ScalarValue>(() => MaxOutput, value => MaxOutput = value));
AddSuffix("MINOUTPUT", new SetSuffix<ScalarValue>(() => MinOutput, value => MinOutput = value));
AddSuffix("ANTIWINDUPMODE", new SetSuffix<StringValue>(() => AntiWindupMode, value => AntiWindupMode = value));
AddSuffix("KBACKCALC", new SetSuffix<ScalarValue>(() => KBackCalc, value => KBackCalc = value));
AddSuffix(new string[] { "IGNOREERROR", "EPSILON" }, new SetSuffix<ScalarValue>(() => Epsilon, value => Epsilon = value));
AddSuffix("ERRORSUM", new Suffix<ScalarValue>(() => ErrorSum));
AddSuffix("PTERM", new Suffix<ScalarValue>(() => PTerm));
Expand All @@ -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())
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If this value is going to be checked repeatedly during Update it's probably better to store it internally as an enum type and check the enum here instead of doing string manipulation on the fly again and again each Update(). (i.e. when a script uses set to change the value of the suffix, check it then to see if the string is one of the known values and if it is then set the enum, else throw an error.) That way the string work only happens when the value is set, rather than 50 times a second.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You're right, I'll fix this. Thanks!

{
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()
Expand Down Expand Up @@ -308,20 +361,24 @@ 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"]);
Kd = Convert.ToDouble(dump["Kd"]);
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"]);
}
}
}
}