PI error control

Signed-off-by: Lamont Granquist <lamont@scriptkiddie.org>
MuMech · May 3, 2023 · c73a9cc · c73a9cc
1 parent 3010d00
commit c73a9cc
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 10 deletions.
diff --git a/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs b/MechJeb2/MechJebLib/Core/ODE/AbstractIVP.cs
@@ -61,6 +61,8 @@ public abstract class AbstractIVP
 
         public CancellationToken CancellationToken { get; set; }
 
+        protected int N;
+
         /// <summary>
         ///     Dormand Prince 5(4)7FM ODE integrator (aka DOPRI5 aka ODE45)
         /// </summary>
@@ -75,13 +77,14 @@ public abstract class AbstractIVP
         public void Solve(IVPFunc f, IReadOnlyList<double> y0, IList<double> yf, double t0, double tf, Hn? interpolant = null,
             List<IVPEvent>? events = null)
         {
-            int n = y0.Count;
+            N = y0.Count;
+            Initialize();
 
-            using var ynew = Vn.Rent(n);
-            using var dynew = Vn.Rent(n);
-            using var dy = Vn.Rent(n);
-            using var y = Vn.Rent(n);
-            using IDisposable data = SetupData(n);
+            using var ynew = Vn.Rent(N);
+            using var dynew = Vn.Rent(N);
+            using var dy = Vn.Rent(N);
+            using var y = Vn.Rent(N);
+            using IDisposable data = SetupData(N);
 
             int direction = t0 != tf ? Math.Sign(tf - t0) : 1;
             double habs = SelectInitialStep(t0, tf);
@@ -125,8 +128,8 @@ public abstract class AbstractIVP
                         if (!tinterp.IsWithin(t, tnew))
                             break;
 
-                        using var yinterp = Vn.Rent(n);
-                        using var finterp = Vn.Rent(n);
+                        using var yinterp = Vn.Rent(N);
+                        using var finterp = Vn.Rent(N);
 
                         PrepareInterpolant(habs, direction, y, dy, ynew, dynew, data);
                         Interpolate(tinterp, t, h, y, yinterp, data);
@@ -161,5 +164,6 @@ public abstract class AbstractIVP
         protected abstract void        PrepareInterpolant(double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, object data);
         protected abstract void        Interpolate(double x, double t, double h, Vn y, Vn yout, object data);
         protected abstract IDisposable SetupData(int n);
+        protected abstract void        Initialize();
     }
 }
diff --git a/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs b/MechJeb2/MechJebLib/Core/ODE/AbstractRungeKutta.cs
@@ -24,7 +24,16 @@ public abstract class AbstractRungeKutta : AbstractIVP
         protected abstract int Stages              { get; }
         protected abstract int ErrorEstimatorOrder { get; }
 
-        private double _alpha => 1.0 / (ErrorEstimatorOrder + 1.0);
+        private double _beta = 0.2;
+
+        public double Beta
+        {
+            get => _beta / (ErrorEstimatorOrder + 1.0);
+            set => _beta = value * (ErrorEstimatorOrder + 1.0);
+        }
+
+        private double _alpha => 1.0 / (ErrorEstimatorOrder + 1.0) - 0.75 * Beta;
+        private double _lastErrorNorm = 1e-4;
 
         protected override double Step(IVPFunc f, double t, double habs, int direction, Vn y, Vn dy, Vn ynew, Vn dynew, object data)
         {
@@ -41,13 +50,19 @@ protected override double Step(IVPFunc f, double t, double habs, int direction,
 
                 if (errorNorm < 1)
                 {
-                    double factor = errorNorm == 0 ? MAX_FACTOR : Math.Min(MAX_FACTOR,SAFETY*Math.Pow(errorNorm, -_alpha));
+                    double factor;
+                    if (errorNorm == 0)
+                        factor = MAX_FACTOR;
+                    else
+                        factor = Math.Min(MAX_FACTOR,SAFETY*Math.Pow(errorNorm, -_alpha)*Math.Pow(_lastErrorNorm, Beta));
 
                     if (previouslyRejected)
                         factor = Math.Min(1.0, factor);
 
                     habs *= factor;
 
+                    _lastErrorNorm = Math.Max(errorNorm, 1e-4);
+
                     break;
                 }
 
@@ -68,6 +83,11 @@ protected override double SelectInitialStep(double t0, double tf)
             return 0.001 * v;
         }
 
+        protected override void Initialize()
+        {
+            _lastErrorNorm = 1e-4;
+        }
+
         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)