Exemplo n.º 1
0
 public void ChangeStep()
 {
     dt = dt / 2.0;
     if (dt < epsilon)
     {
         throw new ArgumentException("Cannot generate numerical solution");
     }
     zn = NordsieckState.Rescale(zn, 0.5d);
 }
Exemplo n.º 2
0
        /// <summary>
        /// Execute predictor-corrector scheme for Nordsieck's method
        /// </summary>
        /// <param name="qcurr">current method order</param>
        /// <param name="x">system phase vector to compute</param>
        /// <param name="e0">initial error vector (en(0) in LSODE)</param>
        /// <param name="t">current time</param>
        /// <param name="xprev">initial value of phase vector</param>
        /// <param name="z0">Zn(0) in LSODE - initial History matrix value</param>
        /// <param name="dt">current step size</param>
        /// <param name="l">current Nordsieck's parameters vector</param>
        /// <param name="b">current b0 in Gear's scheme</param>
        /// <param name="tau">current step change poarameter tau(q,q)</param>
        /// <param name="zn">current Z Nordsieck's matrix to change</param>
        /// <param name="f">right parts vector</param>
        /// <param name="opts">current options</param>
        /// <returns>en - current error vector</returns>
        private static NordsieckState PredictorCorrectorScheme(NordsieckState currstate, ref bool flag, Func <double, Vector, Vector> f, Options opts)
        {
            int n = currstate.xn.Length;

            NordsieckState newstate = new NordsieckState();
            var            ecurr    = currstate.en;

            newstate.en = ecurr.Clone();
            var xcurr = currstate.xn;
            var x0    = currstate.xn;
            var zcurr = (Matrix)currstate.zn.Clone();
            var qcurr = currstate.qn;
            var qmax  = currstate.qmax;
            var dt    = currstate.dt;
            var t     = currstate.tn;
            var z0    = (Matrix)currstate.zn.Clone();

            //Tolerance computation factors
            double Cq  = Math.Pow(qcurr + 1, -1.0);
            double tau = 1.0 / (Cq * Factorial(qcurr) * l[qcurr - 1][qcurr]);

            int count = 0;

            double Dq = 0.0, DqUp = 0.0, DqDown = 0.0;
            double delta = 0.0;

            //Scaling factors for the step size changing
            //with new method order q' = q, q + 1, q - 1, respectively
            double rSame, rUp, rDown;

            var xprev  = Vector.Zeros(n);
            var gm     = Vector.Zeros(n);
            var deltaE = Vector.Zeros(n);
            var M      = Matrix.Identity(n, qmax - 1);

            if (opts.SparseJacobian == null)
            {
                Matrix J = opts.Jacobian == null?NordsieckState.Jacobian(f, xcurr, t + dt) : opts.Jacobian;

                Matrix P = Matrix.Identity(n, n) - J * dt * b[qcurr - 1];

                do
                {
                    xprev = xcurr.Clone();
                    gm    = dt * f(t + dt, xcurr) - z0.CloneColumn(1) - ecurr;
                    ecurr = ecurr + P.SolveGE(gm);
                    xcurr = x0 + b[qcurr - 1] * ecurr;

                    //Row dimension is smaller than zcurr has
                    M = ecurr & l[qcurr - 1];
                    //So, "expand" the matrix
                    var MBig = Matrix.Identity(zcurr.RowDimension, zcurr.ColumnDimension);
                    for (int i = 0; i < zcurr.RowDimension; i++)
                    {
                        for (int j = 0; j < zcurr.ColumnDimension; j++)
                        {
                            MBig[i, j] = i < M.RowDimension && j < M.ColumnDimension ? M[i, j] : 0.0d;
                        }
                    }
                    zcurr   = z0 + MBig;
                    Dq      = ecurr.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xprev);
                    deltaE  = ecurr - currstate.en;
                    deltaE *= (1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1]);
                    DqUp    = deltaE.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
                    DqDown  = zcurr.CloneColumn(qcurr - 1).ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
                    delta   = Dq / (tau / (2 * (qcurr + 2)));
                    count++;
                } while (delta > 1.0d && count < opts.NumberOfIterations);
            }
            else
            {
                SparseMatrix J = opts.SparseJacobian;
                SparseMatrix P = SparseMatrix.Identity(n, n) - J * dt * b[qcurr - 1];


                do
                {
                    xprev = xcurr.Clone();
                    gm    = dt * f(t + dt, xcurr) - z0.CloneColumn(1) - ecurr;
                    ecurr = ecurr + P.SolveGE(gm);
                    xcurr = x0 + b[qcurr - 1] * ecurr;
                    //Row dimension is smaller than zcurr has
                    M = ecurr & l[qcurr - 1];
                    //So, "expand" the matrix
                    var MBig = Matrix.Identity(zcurr.RowDimension, zcurr.ColumnDimension);
                    for (int i = 0; i < zcurr.RowDimension; i++)
                    {
                        for (int j = 0; j < zcurr.ColumnDimension; j++)
                        {
                            MBig[i, j] = i < M.RowDimension && j < M.ColumnDimension ? M[i, j] : 0.0d;
                        }
                    }
                    zcurr   = z0 + MBig;
                    Dq      = ecurr.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xprev);
                    deltaE  = ecurr - currstate.en;
                    deltaE *= (1.0 / (qcurr + 2) * l[qcurr - 1][qcurr - 1]);
                    DqUp    = deltaE.ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
                    DqDown  = zcurr.CloneColumn(qcurr - 1).ToleranceNorm(opts.RelativeTolerance, opts.AbsoluteTolerance, xcurr);
                    delta   = Dq / (tau / (2 * (qcurr + 2)));
                    count++;
                } while (delta > 1.0d && count < opts.NumberOfIterations);
            }

            //======================================

            var nsuccess = count < opts.NumberOfIterations ? currstate.nsuccess + 1 : 0;

            if (count < opts.NumberOfIterations)
            {
                flag        = false;
                newstate.zn = (Matrix)zcurr.Clone();
                newstate.xn = zcurr.CloneColumn(0);
                newstate.en = ecurr.Clone();
            }
            else
            {
                flag        = true;
                newstate.zn = (Matrix)currstate.zn.Clone();
                newstate.xn = currstate.zn.CloneColumn(0);
                newstate.en = currstate.en.Clone();
            }

            //Compute step size scaling factors
            rUp = 0.0;

            if (currstate.qn < currstate.qmax)
            {
                rUp = rUp = 1.0 / 1.4 / (Math.Pow(DqUp, 1.0 / (qcurr + 2)) + 1e-6);
            }

            rSame = 1.0 / 1.2 / (Math.Pow(Dq, 1.0 / (qcurr + 1)) + 1e-6);

            rDown = 0.0;

            if (currstate.qn > 1)
            {
                rDown = 1.0 / 1.3 / (Math.Pow(DqDown, 1.0 / (qcurr)) + 1e-6);
            }

            //======================================
            newstate.nsuccess = nsuccess >= currstate.qn ? 0 : nsuccess;
            //Step size scale operations

            if (rSame >= rUp)
            {
                if (rSame <= rDown && nsuccess >= currstate.qn && currstate.qn > 1)
                {
                    newstate.qn = currstate.qn - 1;
                    newstate.Dq = DqDown;

                    for (int i = 0; i < n; i++)
                    {
                        for (int j = newstate.qn + 1; j < qmax + 1; j++)
                        {
                            newstate.zn[i, j] = 0.0;
                        }
                    }
                    nsuccess         = 0;
                    newstate.rFactor = rDown;
                }
                else
                {
                    newstate.qn      = currstate.qn;
                    newstate.Dq      = Dq;
                    newstate.rFactor = rSame;
                }
            }
            else
            {
                if (rUp >= rDown)
                {
                    if (rUp >= rSame && nsuccess >= currstate.qn && currstate.qn < currstate.qmax)
                    {
                        newstate.qn      = currstate.qn + 1;
                        newstate.Dq      = DqUp;
                        newstate.rFactor = rUp;
                        nsuccess         = 0;
                    }
                    else
                    {
                        newstate.qn      = currstate.qn;
                        newstate.Dq      = Dq;
                        newstate.rFactor = rSame;
                    }
                }
                else
                {
                    if (nsuccess >= currstate.qn && currstate.qn > 1)
                    {
                        newstate.qn = currstate.qn - 1;
                        newstate.Dq = DqDown;

                        for (int i = 0; i < n; i++)
                        {
                            for (int j = newstate.qn + 1; j < qmax + 1; j++)
                            {
                                newstate.zn[i, j] = 0.0;
                            }
                        }
                        nsuccess         = 0;
                        newstate.rFactor = rDown;
                    }
                    else
                    {
                        newstate.qn      = currstate.qn;
                        newstate.Dq      = Dq;
                        newstate.rFactor = rSame;
                    }
                }
            }

            newstate.qmax = qmax;
            newstate.dt   = dt;
            newstate.tn   = t;
            return(newstate);
        }
Exemplo n.º 3
0
        /// <summary>
        /// Implementation of Gear's BDF method with dynamically changed step size and order. Order changes between 1 and 3.
        /// </summary>
        /// <param name="t0">Initial time point</param>
        /// <param name="x0">Initial phase vector</param>
        /// <param name="f">Right parts of the system</param>
        /// <param name="opts">Options for accuracy control and initial step size</param>
        /// <returns>Sequence of infinite number of solution points.</returns>
        public static IEnumerable <SolPoint> GearBDF(double t0, Vector x0, Func <double, Vector, Vector> f, Options opts)
        {
            double t = t0;
            Vector x = x0.Clone();
            int    n = x0.Length;

            double tout = t0;
            Vector xout = new Vector();

            if (opts.OutputStep > 0) // Store previous solution point if OutputStep is specified (non-zero)
            {
                xout  = x0.Clone();
                tout += opts.OutputStep;
            }

            // Firstly, return initial point
            yield return(new SolPoint(t0, x0.Clone()));

            //Initial step size.
            Vector dx = f(t0, x0).Clone();
            double dt;

            if (opts.InitialStep != 0)
            {
                dt = opts.InitialStep;
            }
            else
            {
                var tol = opts.RelativeTolerance;
                var ewt = Vector.Zeros(n);
                var ywt = Vector.Zeros(n);
                var sum = 0.0;
                for (int i = 0; i < n; i++)
                {
                    ewt[i] = opts.RelativeTolerance * Math.Abs(x[i]) + opts.AbsoluteTolerance;
                    ywt[i] = ewt[i] / tol;
                    sum    = sum + (double)dx[i] * dx[i] / (ywt[i] * ywt[i]);
                }

                dt = Math.Sqrt(tol / ((double)1.0d / (ywt[0] * ywt[0]) + sum / n));
            }

            dt = Math.Min(dt, opts.MaxStep);
            var resdt = dt;

            int qmax  = 5;
            int qcurr = 2;


            //Compute Nordstieck's history matrix at t=t0;
            Matrix zn = new Matrix(n, qmax + 1);

            for (int i = 0; i < n; i++)
            {
                zn[i, 0] = x[i];
                zn[i, 1] = dt * dx[i];
                for (int j = qcurr; j < qmax + 1; j++)
                {
                    zn[i, j] = 0.0d;
                }
            }

            var eold = Vector.Zeros(n);

            NordsieckState currstate = new NordsieckState();

            currstate.delta    = 0.0d;
            currstate.Dq       = 0.0d;
            currstate.dt       = dt;
            currstate.en       = eold;
            currstate.tn       = t;
            currstate.xn       = x0;
            currstate.qn       = qcurr;
            currstate.qmax     = qmax;
            currstate.nsuccess = 0;
            currstate.zn       = zn;
            currstate.rFactor  = 1.0d;

            bool isIterationFailed = false;

            //Can produce any number of solution points
            while (true)
            {
                // Reset fail flag
                isIterationFailed = false;

                // Predictor step
                var z0 = currstate.zn.Clone();
                currstate.zn = NordsieckState.ZNew(currstate.zn);
                currstate.en = Vector.Zeros(n);
                currstate.xn = currstate.zn.CloneColumn(0);

                // Corrector step
                currstate = PredictorCorrectorScheme(currstate, ref isIterationFailed, f, opts);

                if (isIterationFailed) // If iterations are not finished - bad convergence
                {
                    currstate.zn       = z0;
                    currstate.nsuccess = 0;
                    currstate.ChangeStep();
                }
                else // Iterations finished
                {
                    var r = Math.Min(1.1d, Math.Max(0.2d, currstate.rFactor));

                    if (currstate.delta >= 1.0d)
                    {
                        if (opts.MaxStep < Double.MaxValue)
                        {
                            r = Math.Min(r, opts.MaxStep / currstate.dt);
                        }

                        if (opts.MinStep > 0)
                        {
                            r = Math.Max(r, opts.MinStep / currstate.dt);
                        }

                        r = Math.Min(r, opts.MaxScale);
                        r = Math.Max(r, opts.MinScale);

                        currstate.dt = currstate.dt * r; // Decrease step
                        currstate.zn = NordsieckState.Rescale(currstate.zn, r);
                    }
                    else
                    {
                        // Output data
                        if (opts.OutputStep > 0) // Output points with specified step
                        {
                            while (currstate.tn <= tout && tout <= currstate.tn + currstate.dt)
                            {
                                yield return(new SolPoint(tout, Vector.Lerp(tout, currstate.tn,
                                                                            xout, currstate.tn + currstate.dt, currstate.xn)));

                                tout += opts.OutputStep;
                            }
                            Vector.Copy(currstate.xn, xout);
                        }
                        else // Output each point

                        {
                            yield return(new SolPoint(currstate.tn + currstate.dt, currstate.xn));
                        }

                        currstate.tn = currstate.tn + currstate.dt;

                        if (opts.MaxStep < Double.MaxValue)
                        {
                            r = Math.Min(r, opts.MaxStep / currstate.dt);
                        }

                        if (opts.MinStep > 0)
                        {
                            r = Math.Max(r, opts.MinStep / currstate.dt);
                        }

                        r = Math.Min(r, opts.MaxScale);
                        r = Math.Max(r, opts.MinScale);

                        currstate.dt = currstate.dt * r;

                        currstate.zn = NordsieckState.Rescale(currstate.zn, r);
                    }
                }
            }
        }