From 3010d0057c3e8c39e14eacfee2498dafd9b198b7 Mon Sep 17 00:00:00 2001 From: Lamont Granquist Date: Wed, 3 May 2023 15:35:00 -0700 Subject: [PATCH] better working error controller Signed-off-by: Lamont Granquist --- MechJeb2/MechJeb2.csproj | 3 +- MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs | 9 +- .../MechJebLib/Core/ODE/AbstractRungeKutta.cs | 62 +++++--- .../MechJebLib/Core/ODE/BogackiShampine32.cs | 143 ++++++++++++++++++ .../{DormandPrince5.cs => DormandPrince54.cs} | 11 +- .../PVG/Integrators/VacuumThrustIntegrator.cs | 2 +- MechJeb2/MechJebLib/Primitives/Vn.cs | 10 ++ MechJebLibTest/Maths/DormandPrinceTests.cs | 2 +- .../Maths/TwoBody/FarnocchiaTests.cs | 4 +- MechJebLibTest/Maths/TwoBody/ShepperdTests.cs | 4 +- 10 files changed, 217 insertions(+), 33 deletions(-) create mode 100644 MechJeb2/MechJebLib/Core/ODE/BogackiShampine32.cs rename MechJeb2/MechJebLib/Core/ODE/{DormandPrince5.cs => DormandPrince54.cs} (96%) diff --git a/MechJeb2/MechJeb2.csproj b/MechJeb2/MechJeb2.csproj index 860a1f071..79636b115 100644 --- a/MechJeb2/MechJeb2.csproj +++ b/MechJeb2/MechJeb2.csproj @@ -109,7 +109,8 @@ - + + diff --git a/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs b/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs index f065c0b5d..0db4e2561 100644 --- a/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs +++ b/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs @@ -35,9 +35,14 @@ public abstract class AbstractIVP public double Maxiter { get; set; } = 2000; /// - /// Desired local accuracy. + /// Desired relative tolerance. /// - public double Accuracy { get; set; } = 1e-9; + public double Rtol { get; set; } = 1e-9; + + /// + /// Desired absolute tolerance. + /// + public double Atol { get; set; } = 1e-9; /// /// Starting step-size (can be zero for automatic guess). diff --git a/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs b/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs index 2e351d08a..4b4df30fc 100644 --- a/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs +++ b/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs @@ -3,14 +3,12 @@ * SPDX-License-Identifier: MIT-0 OR LGPL-2.1+ OR CC0-1.0 */ +#nullable enable + using System; -using System.Collections.Generic; using MechJebLib.Primitives; -using MechJebLib.Utils; using static MechJebLib.Utils.Statics; -#nullable enable - namespace MechJebLib.Core.ODE { using IVPFunc = Action; @@ -18,35 +16,44 @@ namespace MechJebLib.Core.ODE public abstract class AbstractRungeKutta : AbstractIVP { + private const double MAX_FACTOR = 10; + private const double MIN_FACTOR = 0.2; + private const double SAFETY = 0.9; + + protected abstract int Order { get; } + protected abstract int Stages { get; } + protected abstract int ErrorEstimatorOrder { get; } + + private double _alpha => 1.0 / (ErrorEstimatorOrder + 1.0); + protected override double Step(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, object data) { int n = y.Count; using var err = Vn.Rent(n); + bool previouslyRejected = false; + while (true) { RKStep(f, t, habs, direction, y, dy, ynew, dynew, err, data); - double error = 0; - for (int i = 0; i < n; i++) - // FIXME: look at dopri fortran code to see how they generate this - error = Math.Max(error, Math.Abs(err[i])); + double errorNorm = ScaledErrorNorm(y, ynew, err); - double s = 0.84 * Math.Pow(Accuracy / error, 1.0 / 5.0); + if (errorNorm < 1) + { + double factor = errorNorm == 0 ? MAX_FACTOR : Math.Min(MAX_FACTOR,SAFETY*Math.Pow(errorNorm, -_alpha)); - if (s < 0.1) - s = 0.1; - if (s > 4) - s = 4; - habs *= s; + if (previouslyRejected) + factor = Math.Min(1.0, factor); - if (Hmin > 0 && habs < Hmin) - habs = Hmin * habs; - if (Hmax > 0 && habs > Hmax) - habs = Hmax * habs; + habs *= factor; - if (error < Accuracy) break; + } + + habs *= Math.Max(MIN_FACTOR, SAFETY * Math.Pow(errorNorm, -_alpha)); + + previouslyRejected = true; } return habs; @@ -61,6 +68,21 @@ protected override double SelectInitialStep(double t0, double tf) return 0.001 * v; } - protected abstract void RKStep(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, Vn err, object data); + protected abstract void RKStep(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, Vn err, object data); + + private double ScaledErrorNorm(Vn y, Vn ynew, Vn err) + { + int n = err.Count; + + double error = 0.0; + + for (int i = 0; i < n; i++) + { + double scale = Atol + Rtol * Math.Max(Math.Abs(y[i]), Math.Abs(ynew[i])); + error += Powi(err[i]/scale, 2); + } + + return Math.Sqrt(error / n); + } } } diff --git a/MechJeb2/MechJebLib/Core/ODE/BogackiShampine32.cs b/MechJeb2/MechJebLib/Core/ODE/BogackiShampine32.cs new file mode 100644 index 000000000..d0fa2e17f --- /dev/null +++ b/MechJeb2/MechJebLib/Core/ODE/BogackiShampine32.cs @@ -0,0 +1,143 @@ +/* + * Copyright Lamont Granquist, Sebastien Gaggini and the MechJeb contributors + * SPDX-License-Identifier: MIT-0 OR LGPL-2.1+ OR CC0-1.0 + */ + +#nullable enable + +using System; +using MechJebLib.Primitives; +using MechJebLib.Utils; + +namespace MechJebLib.Core.ODE +{ + using IVPFunc = Action; + using IVPEvent = Func; + + /// + /// https://doi.org/10.1016/S0168-9274(96)00025-6 + /// https://github.com/PyNumAl/Python-Numerical-Analysis/blob/main/Initial-Value%20Problems/RK%20tableaus/DP54.txt + /// https://github.com/blackstonep/Numerical-Recipes/blob/master/stepperdopr5.h + /// https://github.com/SciML/OrdinaryDiffEq.jl/blob/master/src/tableaus/low_order_rk_tableaus.jl + /// https://github.com/scipy/scipy/blob/main/scipy/integrate/_ivp/rk.py + /// https://github.com/zachjweiner/scipy/blob/8ba609c313f09bf2a333849ca3ac2bd24d7655e7/scipy/integrate/_ivp/rk.py + /// + public class BogackiShampine32 : AbstractRungeKutta + { + protected override int Order => 3; + protected override int Stages => 3; + protected override int ErrorEstimatorOrder => 2; + + #region IntegrationConstants + + private const double A21 = 0.5; + private const double A32 = 0.75; + private const double A41 = 2.0 / 9.0; + private const double A42 = 1.0 / 3.0; + private const double A43 = 4.0 / 9.0; + + private const double C2 = 0.5; + private const double C3 = 0.75; + + private const double E1 = -5.0 / 72.0; + private const double E2 = 1.0 / 12.0; + private const double E3 = 1.0 / 9.0; + private const double E4 = -0.125; + + #endregion + + private class RKData : IDisposable + { + private static readonly ObjectPool _pool = new ObjectPool(() => new RKData()); + + public Vn K1, K2, K3, K4; + + private RKData() + { + K1 = K2 = K3 = K4 = null!; + } + + public void Dispose() + { + K1.Dispose(); + K2.Dispose(); + K3.Dispose(); + K4.Dispose(); + _pool.Return(this); + } + + public static RKData Rent(int n) + { + RKData data = _pool.Get(); + data.K1 = Vn.Rent(n); + data.K2 = Vn.Rent(n); + data.K3 = Vn.Rent(n); + data.K4 = Vn.Rent(n); + return data; + } + } + + protected override void RKStep(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, Vn err, object o) + { + var data = (RKData)o; + + int n = y.Count; + double h = habs * direction; + + dy.CopyTo(data.K1); + + for (int i = 0; i < n; i++) + ynew[i] = y[i] + h * (A21 * dy[i]); + f(ynew, t + C2 * h, data.K2); + + for (int i = 0; i < n; i++) + ynew[i] = y[i] + h * (A32 * data.K2[i]); + f(ynew, t + C3 * h, data.K3); + + for (int i = 0; i < n; i++) + ynew[i] = y[i] + h * (A41 * data.K1[i] + A42 * data.K2[i] + A43 * data.K3[i]); + f(ynew, t + h, data.K4); + + for (int i = 0; i < n; i++) + err[i] = data.K1[i] * E1 + data.K2[i] * E2 + data.K3[i] * E3 + data.K4[i] * E4; + + data.K4.CopyTo(dynew); + } + + protected override void PrepareInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, object data) + { + // intentionally left blank for now + } + + protected override void Interpolate(double x, double t, double h, Vn y, Vn yout, object o) + { + /* + var data = (RKData)o; + + int n = y.Count; + double s = (x - t) / h; + double s2 = s * s; + double s3 = s * s2; + double s4 = s2 * s2; + + double bf1 = 1.0 / 11282082432.0 * (157015080.0 * s4 - 13107642775.0 * s3 + 34969693132.0 * s2 - 32272833064.0 * s + 11282082432.0); + double bf3 = -100.0 / 32700410799.0 * s * (15701508.0 * s3 - 914128567.0 * s2 + 2074956840.0 * s - 1323431896.0); + double bf4 = 25.0 / 5641041216.0 * s * (94209048.0 * s3 - 1518414297.0 * s2 + 2460397220.0 * s - 889289856.0); + double bf5 = -2187.0 / 199316789632.0 * s * (52338360.0 * s3 - 451824525.0 * s2 + 687873124.0 * s - 259006536.0); + double bf6 = 11.0 / 2467955532.0 * s * (106151040.0 * s3 - 661884105.0 * s2 + 946554244.0 * s - 361440756.0); + double bf7 = 1.0 / 29380423.0 * s * (1.0 - s) * (8293050.0 * s2 - 82437520.0 * s + 44764047.0); + + for (int i = 0; i < n; i++) + { + yout[i] = y[i] + h * s * (bf1 * data.K1[i] + bf3 * data.K3[i] + bf4 * data.K4[i] + bf5 * data.K5[i] + bf6 * data.K6[i] + + bf7 * data.K7[i]); + } + */ + } + + protected override IDisposable SetupData(int n) + { + return RKData.Rent(n); + } + } +} diff --git a/MechJeb2/MechJebLib/Core/ODE/DormandPrince5.cs b/MechJeb2/MechJebLib/Core/ODE/DormandPrince54.cs similarity index 96% rename from MechJeb2/MechJebLib/Core/ODE/DormandPrince5.cs rename to MechJeb2/MechJebLib/Core/ODE/DormandPrince54.cs index 8c48b17cf..05069fe7b 100644 --- a/MechJeb2/MechJebLib/Core/ODE/DormandPrince5.cs +++ b/MechJeb2/MechJebLib/Core/ODE/DormandPrince54.cs @@ -3,20 +3,24 @@ * SPDX-License-Identifier: MIT-0 OR LGPL-2.1+ OR CC0-1.0 */ -#nullable enable - using System; using MechJebLib.Primitives; using MechJebLib.Utils; +#nullable enable + // ReSharper disable CompareOfFloatsByEqualityOperator namespace MechJebLib.Core.ODE { using IVPFunc = Action; using IVPEvent = Func; - public class DormandPrince5 : AbstractRungeKutta + public class DormandPrince54 : AbstractRungeKutta { + protected override int Order => 5; + protected override int Stages => 6; + protected override int ErrorEstimatorOrder => 4; + #region IntegrationConstants // constants for DP5(4)7FM @@ -60,7 +64,6 @@ private class RKData : IDisposable private static readonly ObjectPool _pool = new ObjectPool(() => new RKData()); public Vn K1, K2, K3, K4, K5, K6, K7; - private RKData() => K1 = K2 = K3 = K4 = K5 = K6 = K7 = null!; public void Dispose() diff --git a/MechJeb2/MechJebLib/PVG/Integrators/VacuumThrustIntegrator.cs b/MechJeb2/MechJebLib/PVG/Integrators/VacuumThrustIntegrator.cs index 9441dc0c5..8fb6fd160 100644 --- a/MechJeb2/MechJebLib/PVG/Integrators/VacuumThrustIntegrator.cs +++ b/MechJeb2/MechJebLib/PVG/Integrators/VacuumThrustIntegrator.cs @@ -52,7 +52,7 @@ public void dydt(Vn yin, double x, Vn dyout) } private readonly VacuumThrustKernel _ode = new VacuumThrustKernel(); - private readonly DormandPrince5 _solver = new DormandPrince5(); + private readonly DormandPrince54 _solver = new DormandPrince54(); public void Integrate(Vn y0, Vn yf, Phase phase, double t0, double tf) { diff --git a/MechJeb2/MechJebLib/Primitives/Vn.cs b/MechJeb2/MechJebLib/Primitives/Vn.cs index b59a5f273..b69ea9fc3 100644 --- a/MechJeb2/MechJebLib/Primitives/Vn.cs +++ b/MechJeb2/MechJebLib/Primitives/Vn.cs @@ -31,6 +31,16 @@ public Vn Abs() return abs; } + public double AbsMax() + { + double absmax = double.NegativeInfinity; + for (int i = 0; i < _n; i++) + if (Math.Abs(this[i]) > absmax) + absmax = this[i]; + + return absmax; + } + public Vn Dup() { Vn dup = Rent(_n); diff --git a/MechJebLibTest/Maths/DormandPrinceTests.cs b/MechJebLibTest/Maths/DormandPrinceTests.cs index 6585b0917..381655c78 100644 --- a/MechJebLibTest/Maths/DormandPrinceTests.cs +++ b/MechJebLibTest/Maths/DormandPrinceTests.cs @@ -41,7 +41,7 @@ public void SimpleOscillatorInterpolant() double x0 = 0; double v0 = 1; double tf = 4; - var solver = new DormandPrince5 { Interpnum = 20, Accuracy = 1e-9 }; + var solver = new DormandPrince54 { Interpnum = 20, Rtol = 1e-10, Atol = 1e-10 }; var ode = new SimpleOscillator(k, m); var f = new Action(ode.dydt); diff --git a/MechJebLibTest/Maths/TwoBody/FarnocchiaTests.cs b/MechJebLibTest/Maths/TwoBody/FarnocchiaTests.cs index d33ba77ee..79441237b 100644 --- a/MechJebLibTest/Maths/TwoBody/FarnocchiaTests.cs +++ b/MechJebLibTest/Maths/TwoBody/FarnocchiaTests.cs @@ -75,9 +75,9 @@ public void dydt(Vn yin, double x, Vn dyout) [Fact] private void RandomComparedToDormandPrince() { - var solver = new DormandPrince5(); + var solver = new DormandPrince54(); - solver.Accuracy = 1e-9; + solver.Rtol = 1e-9; solver.Hmin = 1e-9; solver.ThrowOnMaxIter = true; solver.Maxiter = 2000; diff --git a/MechJebLibTest/Maths/TwoBody/ShepperdTests.cs b/MechJebLibTest/Maths/TwoBody/ShepperdTests.cs index 60f55f928..3d94dd5d5 100644 --- a/MechJebLibTest/Maths/TwoBody/ShepperdTests.cs +++ b/MechJebLibTest/Maths/TwoBody/ShepperdTests.cs @@ -84,9 +84,9 @@ public void dydt(Vn yin, double x, Vn dyout) [Fact] private void RandomComparedToDormandPrince() { - var solver = new DormandPrince5(); + var solver = new DormandPrince54(); - solver.Accuracy = 1e-13; + solver.Rtol = 1e-13; solver.Hmin = EPS; solver.ThrowOnMaxIter = true; solver.Maxiter = 2000;