diff --git a/src/+otp/+pendulumdae/+presets/Canonical.m b/src/+otp/+pendulumdae/+presets/Canonical.m new file mode 100644 index 00000000..f5e88b93 --- /dev/null +++ b/src/+otp/+pendulumdae/+presets/Canonical.m @@ -0,0 +1,24 @@ +classdef Canonical < otp.pendulumdae.PendulumDAEProblem + %CANONICAL The constrained pendulum problem + % + % See + % Hairer, E., Roche, M., Lubich, C. (1989). Description of differential-algebraic problems. + % In: The Numerical Solution of Differential-Algebraic Systems by Runge-Kutta Methods. + % Lecture Notes in Mathematics, vol 1409. Springer, Berlin, Heidelberg. + % https://doi.org/10.1007/BFb0093948 + + methods + function obj = Canonical + tspan = [0; 10]; + + params = otp.pendulumdae.PendulumDAEParameters; + params.Mass = 1; + params.Length = 1; + params.Gravity = otp.utils.PhysicalConstants.EarthGravity; + + y0 = [sqrt(2)/2; sqrt(2)/2; 0; 0; 0; 0; 0]; + + obj = obj@otp.pendulumdae.PendulumDAEProblem(tspan, y0, params); + end + end +end diff --git a/src/+otp/+pendulumdae/PendulumDAEParameters.m b/src/+otp/+pendulumdae/PendulumDAEParameters.m new file mode 100644 index 00000000..2465a1c6 --- /dev/null +++ b/src/+otp/+pendulumdae/PendulumDAEParameters.m @@ -0,0 +1,11 @@ +classdef PendulumDAEParameters + %CONSTRAINEDPENDULUMPARAMETERS + properties + %GRAVITY is acceleration due to gravity + Gravity %MATLAB ONLY: (1,1) {mustBeReal, mustBeFinite, mustBePositive} = 9.8 + %MASSE of the node of the pendulum + Mass %MATLAB ONLY: (:,1) {mustBeReal, mustBeFinite, mustBePositive} = 1 + %LENGTH to the node of the pendulum + Length %MATLAB ONLY: (:,1) {mustBeReal, mustBeFinite, mustBePositive} = 1 + end +end diff --git a/src/+otp/+pendulumdae/PendulumDAEProblem.m b/src/+otp/+pendulumdae/PendulumDAEProblem.m new file mode 100644 index 00000000..709098ed --- /dev/null +++ b/src/+otp/+pendulumdae/PendulumDAEProblem.m @@ -0,0 +1,102 @@ +classdef PendulumDAEProblem < otp.Problem + %CONSTRAINEDPENDULUM PROBLEM This is a Hessenberg Index-2 DAE problem posed in terms of three constraints + % + + properties (SetAccess=protected) + RHSDifferential + RHSAlgebraic + RHSHalfExplicit + end + + properties (Dependent) + Y0Differential + Y0Algebraic + end + + methods + function obj = PendulumDAEProblem(timeSpan, y0, parameters) + obj@otp.Problem('Constrained Pendulum', 7, timeSpan, y0, parameters); + end + end + + methods (Access = protected) + function onSettingsChanged(obj) + m = obj.Parameters.Mass; + l = obj.Parameters.Length; + g = obj.Parameters.Gravity; + + % get initial energy + initialconstraints = otp.pendulumdae.fAlgebraic([], obj.Y0Differential, g, m, l, 0); + E0 = initialconstraints(3); + + % The right hand size in terms of x, y, x', y', and three control parameters z + obj.RHS = otp.RHS(@(t, y) otp.pendulumdae.f(t, y, g, m, l, E0), ... + 'Mass', otp.pendulumdae.mass([], [], g, m, l, E0), ... + 'Jacobian', @(t, y) otp.pendulumdae.jacobian(t, y, g, m, l, E0), ... + 'JacobianVectorProduct', ... + @(t, y, v) otp.pendulumdae.jacobianVectorProduct(t, y, v, g, m, l, E0), ... + 'Vectorized', 'on'); + + % Generate the constituent RHS for the differential part + obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fDifferential(t, y, g, m, l, E0), ... + 'Jacobian', @(t, y) otp.pendulumdae.jacobianDifferential(t, y, g, m, l, E0), ... + 'JacobianVectorProduct', ... + @(t, y, v) otp.pendulumdae.jacobianVectorProductDifferential(t, y, v, g, m, l, E0), ... + 'Vectorized', 'on'); + + % Generate the constituent RHS for the algebraic part + obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.fAlgebraic(t, y, g, m, l, E0), ... + 'Jacobian', @(t, y) otp.pendulumdae.jacobianAlgebraic(t, y, g, m, l, E0), ... + 'JacobianVectorProduct', ... + @(t, y, v) otp.pendulumdae.jacobianVectorProductAlgebraic(t, y, v, g, m, l, E0), ... + 'JacobianAdjointVectorProduct', ... + @(t, y, v) otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, y, v, g, m, l, E0, v), ... + 'HessianAdjointVectorProduct', ... + @(t, y, v, u) otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, y, v, u, g, m, l, E0), ... + 'Vectorized', 'on'); + + % Generate an Auxillary RHS for the half-explicit Jacobian + obj.RHSHalfExplicit = otp.RHS([], ... + 'Jacobian', @(t, y) otp.pendulumdae.jacobianHalfExplicit(t, y, g, m, l, E0), ... + 'JacobianVectorProduct', ... + @(t, y, v) otp.pendulumdae.jacobianVectorProductHalfExplicit(t, y, v, g, m, l, E0), ... + 'Vectorized', 'on'); + + end + + function label = internalIndex2label(~, index) + switch index + case 1 + label = 'x position'; + case 2 + label = 'y position'; + case 3 + label = 'x velocity'; + case 4 + label = 'y velocity'; + case 5 + label = 'Control 1'; + case 6 + label = 'Control 2'; + case 7 + label = 'Control 3'; + end + end + + function sol = internalSolve(obj, varargin) + sol = internalSolve@otp.Problem(obj, 'Solver', @otp.utils.solvers.dae34, varargin{:}); + end + end + + methods + function y0differential = get.Y0Differential(obj) + y0differential = obj.Y0(1:4); + end + + function y0algebraic = get.Y0Algebraic(obj) + y0algebraic = obj.Y0(5:end); + end + end + +end + diff --git a/src/+otp/+pendulumdae/f.m b/src/+otp/+pendulumdae/f.m new file mode 100644 index 00000000..1a85d022 --- /dev/null +++ b/src/+otp/+pendulumdae/f.m @@ -0,0 +1,13 @@ +function dfull = f(t, statepluscontrol, g, m, l, E0) + +state = statepluscontrol(1:4, :); +control = statepluscontrol(5:end, :); + +dstate = otp.pendulumdae.fDifferential(t, state, g, m, l, E0) ... + - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, control, g, m, l, E0); + +c = otp.pendulumdae.fAlgebraic(t, state, g, m, l, E0); + +dfull = [dstate; c]; + +end diff --git a/src/+otp/+pendulumdae/fAlgebraic.m b/src/+otp/+pendulumdae/fAlgebraic.m new file mode 100644 index 00000000..b306ad87 --- /dev/null +++ b/src/+otp/+pendulumdae/fAlgebraic.m @@ -0,0 +1,19 @@ +function c = fAlgebraic(~, state, g, m, l, E0) + +x = state(1, :); +y = state(2, :); +u = state(3, :); +v = state(4, :); + +% the fisrt invariant is length preservation +c1 = x.^2 + y.^2 - l^2; + +% the second invariant is the derivative of the above, to convert this into an index-2 DAE +c2 = 2*(x.*u + y.*v); + +% the third invariant is energy preservation, for numerical stability +c3 = m*(g*(y + l) + 0.5*(u.^2 + v.^2)) - E0; + +c = [c1; c2; c3]; + +end \ No newline at end of file diff --git a/src/+otp/+pendulumdae/fDifferential.m b/src/+otp/+pendulumdae/fDifferential.m new file mode 100644 index 00000000..a99ef132 --- /dev/null +++ b/src/+otp/+pendulumdae/fDifferential.m @@ -0,0 +1,19 @@ +function dstate = fDifferential(~, state, g, m, ~, ~) + +x = state(1, :); +y = state(2, :); +u = state(3, :); +v = state(4, :); + +lxy2 = x.^2 + y.^2; + +lambda = (m./lxy2).*(u.^2 + v.^2) - (g./lxy2).*y; + +dx = u; +dy = v; +du = -(lambda/m).*x; +dv = -(lambda/m).*y - g/m; + +dstate = [dx; dy; du; dv]; + +end diff --git a/src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m b/src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m new file mode 100644 index 00000000..1b5b70bb --- /dev/null +++ b/src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m @@ -0,0 +1,19 @@ +function dvp = hessianAdjointVectorProductAlgebraic(~, ~, control, vec, ~, m, ~, ~) + +z1 = control(1, :); +z2 = control(2, :); +z3 = control(3, :); + +vec1 = vec(1, :); +vec2 = vec(2, :); +vec3 = vec(3, :); +vec4 = vec(4, :); + +dvp1 = 2*(vec1.*z1 + vec3.*z2); +dvp2 = 2*(vec2.*z1 + vec4.*z2); +dvp3 = 2*vec1.*z2 + m*vec3.*z3; +dvp4 = 2*vec2.*z2 + m*vec4.*z3; + +dvp = [dvp1; dvp2; dvp3; dvp4]; + +end diff --git a/src/+otp/+pendulumdae/jacobian.m b/src/+otp/+pendulumdae/jacobian.m new file mode 100644 index 00000000..1a42d41f --- /dev/null +++ b/src/+otp/+pendulumdae/jacobian.m @@ -0,0 +1,15 @@ +function j = jacobian(t, statepluscontrol, g, m, l, E0) + +state = statepluscontrol(1:4); +control = statepluscontrol(5:end); + +jaccontrol = otp.pendulumdae.jacobianAlgebraic(t, state, g, m, l, E0); + +jacstate = otp.pendulumdae.jacobianDifferential(t, state, g, m, l, E0) ... + - otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, state, control, eye(4), g, m, l, E0); + +jacstatecontrol = -jaccontrol.'; + +j = [jacstate, jacstatecontrol; jaccontrol, zeros(3, 3)]; + +end diff --git a/src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m b/src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m new file mode 100644 index 00000000..086de231 --- /dev/null +++ b/src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m @@ -0,0 +1,19 @@ +function dvp = jacobianAdjointVectorProductAlgebraic(~, state, control, g, m, ~, ~) + +x = state(1, :); +y = state(2, :); +u = state(3, :); +v = state(4, :); + +z1 = control(1, :); +z2 = control(2, :); +z3 = control(3, :); + +dvp1 = 2*(x.*z1 + u.*z2); +dvp2 = 2*(y.*z1 + v.*z2) + g*m*z3; +dvp3 = 2*x.*z2 + m*u.*z3; +dvp4 = 2*y.*z2 + m*v.*z3; + +dvp = [dvp1; dvp2; dvp3; dvp4]; + +end diff --git a/src/+otp/+pendulumdae/jacobianAlgebraic.m b/src/+otp/+pendulumdae/jacobianAlgebraic.m new file mode 100644 index 00000000..bc9b49b2 --- /dev/null +++ b/src/+otp/+pendulumdae/jacobianAlgebraic.m @@ -0,0 +1,27 @@ +function dc = jacobianAlgebraic(~, state, g, m, ~, ~) + +x = state(1); +y = state(2); +u = state(3); +v = state(4); + +dc1dx = 2*x; +dc1dy = 2*y; +dc1du = 0; +dc1dv = 0; + +dc2dx = 2*u; +dc2dy = 2*v; +dc2du = 2*x; +dc2dv = 2*y; + +dc3dx = 0; +dc3dy = m*g; +dc3du = m*u; +dc3dv = m*v; + +dc = [dc1dx, dc1dy, dc1du, dc1dv; ... + dc2dx, dc2dy, dc2du, dc2dv; ... + dc3dx, dc3dy, dc3du, dc3dv]; + +end diff --git a/src/+otp/+pendulumdae/jacobianDifferential.m b/src/+otp/+pendulumdae/jacobianDifferential.m new file mode 100644 index 00000000..8c99abda --- /dev/null +++ b/src/+otp/+pendulumdae/jacobianDifferential.m @@ -0,0 +1,24 @@ +function j = jacobianDifferential(~, state, g, m, ~, ~) + +x = state(1); +y = state(2); +u = state(3); +v = state(4); + +lxy2 = x.^2 + y.^2; + +lambda = (m./lxy2).*(u.^2 + v.^2) - (g./lxy2).*y; + +dlambdadx = (-2*m*(u.^2 + v.^2).*x + 2*g*x.*y)./(lxy2.^2); +dlambdady = (-2*m*(u.^2 + v.^2).*y + g*(y.^2 - x.^2))./(lxy2.^2); +dlambdadu = (2*m./lxy2).*u; +dlambdadv = (2*m./lxy2).*v; + +dx = [0, 0, 1, 0]; +dy = [0, 0, 0, 1]; +du = -(1/m)*[lambda + x.*dlambdadx, x.*dlambdady, x.*dlambdadu, x.*dlambdadv]; +dv = -(1/m)*[y.*dlambdadx, lambda + y.*dlambdady, y.*dlambdadu, y.*dlambdadv]; + +j = [dx; dy; du; dv]; + +end diff --git a/src/+otp/+pendulumdae/jacobianHalfExplicit.m b/src/+otp/+pendulumdae/jacobianHalfExplicit.m new file mode 100644 index 00000000..ab9b6980 --- /dev/null +++ b/src/+otp/+pendulumdae/jacobianHalfExplicit.m @@ -0,0 +1,19 @@ +function gyfz = jacobianHalfExplicit(~, state, g, m, ~, ~) + +x = state(1); +y = state(2); +u = state(3); +v = state(4); + +gyfz11 = -4*(x^2 + y^2); +gyfz12 = -4*(u*x + v*y); +gyfz13 = -2*g*m*y; +gyfz22 = -4*(x^2 + y^2 + u^2 + v^2); +gyfz23 = -2*m*(u*x + v*(g + y)); +gyfz33 = -(m^2)*(g^2 + u^2 + v^2); + +gyfz = [gyfz11, gyfz12, gyfz13; ... + gyfz12, gyfz22, gyfz23; ... + gyfz13, gyfz23, gyfz33]; + +end diff --git a/src/+otp/+pendulumdae/jacobianVectorProduct.m b/src/+otp/+pendulumdae/jacobianVectorProduct.m new file mode 100644 index 00000000..fe141ac5 --- /dev/null +++ b/src/+otp/+pendulumdae/jacobianVectorProduct.m @@ -0,0 +1,17 @@ +function dfull = jacobianVectorProduct(t, statepluscontrol, v, g, m, l, E0) + +state = statepluscontrol(1:4, :); +control = statepluscontrol(5:end, :); + +vstate = v(1:4, :); +vcontrol = v(5:end, :); + +dstate = otp.pendulumdae.jacobianVectorProductDifferential(t, state, vstate, g, m, l, E0) ... + - otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, state, control, vstate, g, m, l, E0) ... + - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, vcontrol, g, m, l, E0); + +c = otp.pendulumdae.jacobianVectorProductAlgebraic(t, state, vstate, g, m, l, E0); + +dfull = [dstate; c]; + +end diff --git a/src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m b/src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m new file mode 100644 index 00000000..2c303dbe --- /dev/null +++ b/src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m @@ -0,0 +1,20 @@ +function dvp = jacobianVectorProductAlgebraic(~, state, vec, g, m, ~, ~) + +x = state(1, :); +y = state(2, :); +u = state(3, :); +v = state(4, :); + +vec1 = vec(1, :); +vec2 = vec(2, :); +vec3 = vec(3, :); +vec4 = vec(4, :); + + +dvp1 = 2*(x.*vec1 + y.*vec2); +dvp2 = 2*(u.*vec1 + v.*vec2 + x.*vec3 + y.*vec4); +dvp3 = m*(g*vec2 + u.*vec3 + v.*vec4); + +dvp = [dvp1; dvp2; dvp3]; + +end diff --git a/src/+otp/+pendulumdae/jacobianVectorProductDifferential.m b/src/+otp/+pendulumdae/jacobianVectorProductDifferential.m new file mode 100644 index 00000000..9e60d72b --- /dev/null +++ b/src/+otp/+pendulumdae/jacobianVectorProductDifferential.m @@ -0,0 +1,31 @@ +function dstatevp = jacobianVectorProductDifferential(~, state, vec, g, m, ~, ~) + +x = state(1, :); +y = state(2, :); +u = state(3, :); +v = state(4, :); + +vec1 = vec(1, :); +vec2 = vec(2, :); +vec3 = vec(3, :); +vec4 = vec(4, :); + +lxy2 = x.^2 + y.^2; + +lambda = (m./lxy2).*(u.^2 + v.^2) - (g./lxy2).*y; + +dlambdadx = (-2*m*(u.^2 + v.^2).*x + 2*g*x.*y)./(lxy2.^2); +dlambdady = (-2*m*(u.^2 + v.^2).*y + g*(y.^2 - x.^2))./(lxy2.^2); +dlambdadu = (2*m./lxy2).*u; +dlambdadv = (2*m./lxy2).*v; + +inner = (vec1.*dlambdadx + vec2.*dlambdady + vec3.*dlambdadu + vec4.*dlambdadv); + +dxvp = vec3; +dyvp = vec4; +duvp = -(1/m)*(vec1.*lambda + x.*inner); +dvvp = -(1/m)*(vec2.*lambda + y.*inner); + +dstatevp = [dxvp; dyvp; duvp; dvvp]; + +end diff --git a/src/+otp/+pendulumdae/jacobianVectorProductHalfExplicit.m b/src/+otp/+pendulumdae/jacobianVectorProductHalfExplicit.m new file mode 100644 index 00000000..4115a5fa --- /dev/null +++ b/src/+otp/+pendulumdae/jacobianVectorProductHalfExplicit.m @@ -0,0 +1,19 @@ +function gyfzvp = jacobianVectorProductHalfExplicit(~, state, vec, g, m, ~, ~) + +x = state(1, :); +y = state(2, :); +u = state(3, :); +v = state(4, :); + +vec1 = vec(1, :); +vec2 = vec(2, :); +vec3 = vec(3, :); + +gyfz1 = -2*(2*u.*vec2.*x + (2*v.*vec2 + g*m*vec3).*y + 2*vec1.*(x.^2 + y.^2)); +gyfz2 = -2*(2*(u.^2).*vec2 + 2.*(v.^2).*vec2 + u.*(2*vec1 + m*vec3).*x + 2.*v.*vec1.*y ... + + m.*v.*vec3.*(g + y) + 2*vec2.*(x.^2 + y.^2)); +gyfz3 = -m*((g^2)*m*vec3 + m*(u.^2 + v.^2).*vec3 + 2*vec2.*(u.*x + v.*y) + 2*g*(v.*vec2 + vec1.*y)); + +gyfzvp = [gyfz1; gyfz2; gyfz3]; + +end diff --git a/src/+otp/+pendulumdae/mass.m b/src/+otp/+pendulumdae/mass.m new file mode 100644 index 00000000..b2f9cf25 --- /dev/null +++ b/src/+otp/+pendulumdae/mass.m @@ -0,0 +1,5 @@ +function M = mass(~, ~, ~, ~, ~, ~) + +M = [eye(4), zeros(4, 3); zeros(3, 7)]; + +end diff --git a/src/+otp/+utils/+solvers/dae34.m b/src/+otp/+utils/+solvers/dae34.m new file mode 100644 index 00000000..4ab90542 --- /dev/null +++ b/src/+otp/+utils/+solvers/dae34.m @@ -0,0 +1,228 @@ +function [sol, y] = dae34(f, tspan, y0, options) +% See +% Cameron, F., Palmroth, M., & Piché, R. (2002). +% Quasi stage order conditions for SDIRK methods. +% Applied numerical mathematics, 42(1-3), 61-75. + + +gamma = 1/4; + +A = zeros(4,4); +A(2,1) = 1/7; +A(3,1) = 61/144; +A(3,2) = -49/144; +A(4,1) = 0; +A(4,2) = 0; +A(4,3) = 3/4; + +b = zeros(1, 4); +b(3) = 3/4; +b(4) = 1/4; + +bhat = zeros(1, 4); +bhat(1) = -61/600; +bhat(2) = 49/600; +bhat(3) = 79/100; +bhat(4) = 23/100; + +c = sum(A, 2) + gamma; + +orderM = 3; +orderE = 4; + +%% Get all relevant options out +h = odeget(options, 'InitialStep', []); +reltol = odeget(options, 'RelTol', 1e-3); +abstol = odeget(options, 'AbsTol', 1e-6); +J = odeget(options, 'Jacobian', @(t, y) jacapprox(f, t, y)); +M = odeget(options, 'Mass', speye(numel(y0))); +MStateDependence = odeget(options, 'MStateDependence', 'none'); + +% We won't support state-dependent Mass, simple as that +if strcmp(MStateDependence, 'strong') + error('OTP:MassStateDependent', 'State dependent mass is not supported.') +end + +if ~isa(M, 'function_handle') + M = @(t) M; +else + if nargin(M) > 1 + M = @(t) M(t, y0); + end +end + +if isempty(h) + sc = abstol + reltol*abs(y0); + f0 = f(tspan(1), y0); + d0 = sqrt(mean((y0./sc).^2)); + d1 = sqrt(mean((f0./sc).^2)); + h0 = (d0/d1)*0.01; + + y1 = y0 + h0*f0; + f1 = f(tspan(1) + h0, y1); + + d2 = sqrt(mean(((f1 - f0)./sc).^2))/h0; + + h1 = (0.01/max(d1, d2))^(1/orderM); + + h = min(100*h0, h1); +end + +t = tspan(1); +y = y0.'; + +yc = y0; +tc = tspan(1); + +stagenum = size(A, 1); +tend = tspan(end); + +newtonk0 = zeros(size(yc)); + +step = 1; +while tc < tend + + bnewtonreject = false; + + if tc + h > tend + h = tend - tc; + end + + stages = zeros(numel(yc), stagenum); + + gh = gamma*h; + + for stage = 1:stagenum + + staget = tc + c(stage)*h; + + if stage > 1 + stagedy = h*(stages(:, 1:(stage - 1))*A(stage, 1:(stage - 1)).'); + else + stagedy = 0; + end + + newtonk0 = 0*newtonk0; + + ntol = min(abstol, reltol); + nmaxits = 1e2; + alpha = 1; + its = 0; + etak = inf; + nnp = inf; + kappa = 1e-1; + ng = inf; + + Jc = J(staget, yc + stagedy + gh*newtonk0); + while kappa*(etak*nnp) >= ntol && its < nmaxits + Mc = M(staget); + ycs = yc + stagedy + gh*newtonk0; + g = Mc*newtonk0 - f(staget, ycs); + H = Mc - gh*Jc; + npnew = H\g; + + newtonknew = newtonk0 - alpha*npnew; + + sc = abstol + reltol*abs(ycs); + + + if its > 2 + ngnew = sqrt(mean((g./sc).^2)); + % recompute the jacobian + if ngnew > 1.25*ng + Jc = J(staget, ycs); + end + ng = ngnew; + + nnpnew = sqrt(mean((npnew./sc).^2)); + + thetak = nnpnew/nnp; + + if thetak > 1.25 || isnan(thetak) + bnewtonreject = true; + break; + end + + etak = thetak/(1 - thetak); + end + + newtonk0 = newtonknew; + + np = npnew; + nnp = sqrt(mean((np./sc).^2)); + + its = its + 1; + end + + if bnewtonreject + break; + end + + stages(:, stage) = newtonk0; + + end + + if bnewtonreject + h = h/2; + continue; + end + + yhat = yc + h*stages*bhat.'; + ycnew = yc + h*stages*b.'; + + sc = abstol + max(abs(ycnew), abs(yc))*reltol; + + Mc = M(tc + h); + + errdifferential = sqrt(mean(((Mc*(ycnew - yhat))./sc).^2)); + err = errdifferential; + + fac = 0.38^(1/(orderE + 1)); + + facmin = 0.1; + if err > 1 || isnan(err) + % Reject timestep + facmax = 1; + else + % Accept time step + tc = tc + h; + yc = ycnew; + + t(step + 1) = tc; + y(step + 1, :) = yc.'; + + + step = step + 1; + + facmax = 2.5; + end + + % adjust step-size + h = h*min(facmax, max(facmin, fac*(1/err)^1/(orderE + 1))); + +end + +if nargout < 2 + sol = struct('x', t, 'y', y.'); +else + sol = t; +end + +end + +function J = jacapprox(f, t, y) + +n = numel(y); + +J = zeros(n, n); + +h = sqrt(1e-6); + +f0 = f(t, y); + +for i = 1:n + e = zeros(n, 1); e(i) = 1; + J(:, i) = (f(t, y + h*e) - f0)/h; +end + +end diff --git a/src/+otp/+utils/+solvers/dae43.m b/src/+otp/+utils/+solvers/dae43.m new file mode 100644 index 00000000..8fd8624a --- /dev/null +++ b/src/+otp/+utils/+solvers/dae43.m @@ -0,0 +1,239 @@ +function [sol, y] = dae43(f, tspan, y0, options) + +n = numel(y0); + +h = odeget(options, 'InitialStep', []); +reltol = odeget(options, 'RelTol', 1e-3); +abstol = odeget(options, 'AbsTol', 1e-6); +J = odeget(options, 'Jacobian', []); +M = odeget(options, 'Mass', []); +MStateDependence = odeget(options, 'MStateDependence', 'none'); + +% We won't support state-dependent Mass, simple as that +if strcmp(MStateDependence, 'strong') + error('OTP:MassStateDependent', 'Strong state dependent mass is not supported.') +end + +if isempty(J) + J = @(t, y) otp.utils.jacobianApproximation(f, t, y); + usesparse = false; +else + usesparse = issparse(J(tspan(1), y0)); +end + +if isempty(M) + if usesparse + M = @(t, y) speye(numel(y0)); + else + M = @(t, y) eye(numel(y0)); + end +elseif ~isa(M, 'function_handle') + M = @(t, y) M; +elseif isa(M, 'function_handle') && nargin(M) == 1 + M = @(t, y) M(t); +end + +% Lobatto IIIC +A = [1/6, -1/3, 1/6; 1/6, 5/12, -1/12; 1/6, 2/3, 1/6]; +b = [1/6, 2/3, 1/6]; +c = [0, 1/2, 1]; +bhat = [-1/2, 2, -1/2]; + +orderM = 4; +orderE = 3; + +% Compute initial step size +if isempty(h) + sc = abstol + reltol.*abs(y0); + f0 = f(tspan(1), y0); + d0 = sqrt(mean((y0./sc).^2)); + d1 = sqrt(mean((f0./sc).^2)); + h0 = (d0/d1)*0.01; + + y1 = y0 + h0*f0; + f1 = f(tspan(1) + h0, y1); + + d2 = sqrt(mean(((f1 - f0)./sc).^2))/h0; + + h1 = (0.01/max(d1, d2))^(1/orderM); + + h = min(100*h0, h1); +end + +t = tspan(1); +y = y0.'; + +yc = y0; +tc = tspan(1); + +stagenum = size(A, 1); +tend = tspan(end); + +step = 1; + +if usesparse + Mfull = sparse(n*stagenum, n*stagenum); + Jfull = sparse(n*stagenum, n*stagenum); +else + Mfull = zeros(n*stagenum, n*stagenum); + Jfull = zeros(n*stagenum, n*stagenum); +end + +gfull = zeros(n*stagenum, 1); +newtonk = zeros(n*stagenum, 1); +sc = zeros(n*stagenum, 1); + +brejectstage = false; + +while tc < tend + + if tc + h > tend + h = tend - tc; + end + + % build M + for stage = 1:stagenum + si = ((stage - 1)*n + 1):(stage*n); + Mfull(si, si) = M(tc + h*c(stage), yc); + end + + ntol = min(abstol, min(reltol)); + nmaxits = 1e3; + its = 0; + etak = inf; + nnp = inf; + kappa = 1e-1; + + bnewtonreject = false; + + % Here we never reset the newton stages, as the previous stages are good starting points + % This has empirically reduced the time it takes to compute everything. + + % Compute the Jacobian once per step + for stage = 1:stagenum + si = ((stage - 1)*n + 1):(stage*n); + staget = tc + c(stage)*h; + ycs = yc + h*reshape(newtonk, n, [])*(A(stage, :).'); + Jc = J(staget, ycs); + Jfull(si, :) = kron(A(stage, :), Jc); + end + + H = Mfull - h*Jfull; + if usesparse + [L, U, P, Q, D] = lu(H); + else + [L, U] = lu(H); + end + + while kappa*(etak*nnp) >= ntol && its < nmaxits + % build g and Jacobians + for stage = 1:stagenum + si = ((stage - 1)*n + 1):(stage*n); + staget = tc + c(stage)*h; + ycs = yc + h*reshape(newtonk, n, [])*(A(stage, :).'); + sc(si) = abstol + reltol.*abs(ycs); + Mc = Mfull(si, si); + gfull(si) = Mc*newtonk(si) - f(staget, ycs); + end + + if usesparse + npnew = Q*(U\(L\(P*(D\gfull)))); + else + npnew = U\(L\gfull); + end + + if its > 1 + nnpnew = sqrt(mean((npnew./sc).^2)); + + thetak = nnpnew/nnp; + + if thetak > 1.25 || isnan(thetak) || isinf(thetak) + + % If theta is large, we recompute the Jacobian and try again + if ~brejectstage + brejectstage = true; + + + for stage = 1:stagenum + si = ((stage - 1)*n + 1):(stage*n); + staget = tc + c(stage)*h; + ycs = yc + h*reshape(newtonk, n, [])*(A(stage, :).'); + Jc = J(staget, ycs); + Jfull(si, :) = kron(A(stage, :), Jc); + end + H = Mfull - h*Jfull; + if usesparse + [L, U, P, Q, D] = lu(H); + else + [L, U] = lu(H); + end + + continue; + end + + % If theta is still large after recomputing the Jacobian, we reject the step and decrease the stepsize + + bnewtonreject = true; + break; + end + + brejectstage = false; + + etak = thetak/(1 - thetak); + end + + newtonk = newtonk - npnew; + + np = npnew; + nnp = sqrt(mean((np./sc).^2)); + + its = its + 1; + end + + if bnewtonreject + h = h/2; + continue; + end + + yhat = yc + h*reshape(newtonk, n, [])*bhat.'; + ycnew = yc + h*reshape(newtonk, n, [])*b.'; + + sc = abstol + max(abs(ycnew), abs(yc)).*reltol; + + Mc = M(tc + h , ycnew); + + err = sqrt(mean(((Mc*(ycnew - yhat))./sc).^2)); + + fac = 0.9; + + facmin = 0.1; + if err > 1 || isnan(err) + % Reject timestep + facmax = 1; + else + % Accept time step + tc = tc + h; + + yc = ycnew; + + t(step + 1) = tc; + y(step + 1, :) = yc.'; + + + step = step + 1; + + facmax = 4; + end + + % adjust step-size + h = h*min(facmax, max(facmin, fac*(1/err)^1/(orderE + 1))); + +end + +if nargout < 2 + sol = struct('x', t, 'y', y.'); +else + sol = t; +end + +end diff --git a/src/+otp/+utils/+solvers/dae46.m b/src/+otp/+utils/+solvers/dae46.m new file mode 100644 index 00000000..a036f8cd --- /dev/null +++ b/src/+otp/+utils/+solvers/dae46.m @@ -0,0 +1,242 @@ +function [sol, y] = dae46(f, tspan, y0, options) +% See +% Cameron, F., Palmroth, M., & Piché, R. (2002). +% Quasi stage order conditions for SDIRK methods. +% Applied numerical mathematics, 42(1-3), 61-75. + + +gamma = 1/4; + +A = zeros(6,6); +A(2,1) = 1/4; +A(3,1) = -1/36; +A(3,2) = -1/18; +A(4,1) = -21283/32000; +A(4,2) = -5143/64000; +A(4,3) = 90909/64000; +A(5,1) = 46010759/749250000; +A(5,2) = -737693/40500000; +A(5,3) = 10931269/45500000; +A(5,4) = -1140071/34090875; +A(6,1) = 89/444; +A(6,2) = 89/804756; +A(6,3) = -27/364; +A(6,4) = -20000/171717; +A(6,5) = 843750/1140071; + +b = A(6, :); +b(6) = gamma; + +bhat5 = 1/4; +bhat6 = 1/4; +bhat1 = 163/333 + (204072709/112387500)* bhat5 - (725/111)*bhat6; +bhat2 = 17/18 + (5929277/6075000)*bhat5 - (20/3)*bhat6; +bhat3 = -103/182 - (204072709/61425000)*bhat5 + (1074/91)*bhat6; +bhat4 = 4000/30303 - (48017108/102272625)*bhat5 + (4000/10101)*bhat6; + +bhat = [bhat1, bhat2, bhat3, bhat4, bhat5, bhat6]; + +c = sum(A, 2) + gamma; + +orderM = 4; +orderE = 3; + +%% Get all relevant options out +h = odeget(options, 'InitialStep', []); +reltol = odeget(options, 'RelTol', 1e-3); +abstol = odeget(options, 'AbsTol', 1e-6); +J = odeget(options, 'Jacobian', @(t, y) jacapprox(f, t, y)); +M = odeget(options, 'Mass', speye(numel(y0))); +MStateDependence = odeget(options, 'MStateDependence', 'none'); +laststage = odeget(options, 'InitialSlope', []); + +% We won't support state-dependent Mass, simple as that +if strcmp(MStateDependence, 'strong') + error('OTP:MassStateDependent', 'Strong state dependent mass is not supported.') +end + +if ~isa(M, 'function_handle') + M = @(t, y) M; +end + +if isempty(h) + sc = abstol + reltol*abs(y0); + f0 = f(tspan(1), y0); + d0 = sqrt(mean((y0./sc).^2)); + d1 = sqrt(mean((f0./sc).^2)); + h0 = (d0/d1)*0.01; + + y1 = y0 + h0*f0; + f1 = f(tspan(1) + h0, y1); + + d2 = sqrt(mean(((f1 - f0)./sc).^2))/h0; + + h1 = (0.01/max(d1, d2))^(1/orderM); + + h = min(100*h0, h1); +end + +t = tspan(1); +y = y0.'; + +yc = y0; +tc = tspan(1); + +stagenum = size(A, 1); +tend = tspan(end); + +newtonk0 = zeros(size(yc)); + +stages = zeros(numel(yc), stagenum); + +step = 1; +while tc < tend + + bnewtonreject = false; + + if tc + h > tend + h = tend - tc; + end + + gh = gamma*h; + + if step == 1 && isempty(laststage) + [laststage, ~] = bicg(M(tc, yc), f(tc, yc)); + end + + stages(:, 1) = laststage; + + for stage = 2:stagenum + + staget = tc + c(stage)*h; + + stagedy = h*(stages(:, 1:(stage - 1))*A(stage, 1:(stage - 1)).'); + + newtonk0 = 0*newtonk0; + + ntol = min(abstol, reltol); + nmaxits = 1e2; + alpha = 1; + its = 0; + etak = inf; + nnp = inf; + kappa = 1e-1; + ng = inf; + + Jc = J(staget, yc + stagedy + gh*newtonk0); + while kappa*(etak*nnp) >= ntol && its < nmaxits + ycs = yc + stagedy + gh*newtonk0; + Mc = M(staget, ycs); + g = Mc*newtonk0 - f(staget, ycs); + H = Mc - gh*Jc; + %[npnew, ~] = bicg(H, g, [], size(H, 1)); + npnew = H\g; + + newtonknew = newtonk0 - alpha*npnew; + + sc = abstol + reltol*abs(ycs); + + + if its > 3 + ngnew = sqrt(mean((g./sc).^2)); + % recompute the jacobian + if ngnew > 1.25*ng + Jc = J(staget, ycs); + end + ng = ngnew; + + nnpnew = sqrt(mean((npnew./sc).^2)); + + thetak = nnpnew/nnp; + + if thetak > 1.25 || isnan(thetak) + bnewtonreject = true; + break; + end + + etak = thetak/(1 - thetak); + end + + newtonk0 = newtonknew; + + np = npnew; + nnp = sqrt(mean((np./sc).^2)); + + its = its + 1; + end + + if bnewtonreject + break; + end + + stages(:, stage) = newtonk0; + + end + + if bnewtonreject + h = h/2; + continue; + end + + yhat = yc + h*stages*bhat.'; + ycnew = yc + h*stages*b.'; + + sc = abstol + max(abs(ycnew), abs(yc))*reltol; + + Mc = M(tc + h, ycnew); + + errdifferential = sqrt(mean(((Mc*(ycnew - yhat))./sc).^2)); + err = errdifferential; + + fac = 0.38^(1/(orderE + 1)); + + facmin = 0.1; + if err > 1 || isnan(err) + % Reject timestep + facmax = 1; + else + % Accept time step + tc = tc + h; + yc = ycnew; + + t(step + 1) = tc; + y(step + 1, :) = yc.'; + + + step = step + 1; + + facmax = 2.5; + + laststage = stages(:, end); + + end + + % adjust step-size + h = h*min(facmax, max(facmin, fac*(1/err)^1/(orderE + 1))); + +end + +if nargout < 2 + sol = struct('x', t, 'y', y.'); +else + sol = t; +end + +end + +function J = jacapprox(f, t, y) + +n = numel(y); + +J = zeros(n, n); + +h = sqrt(1e-6); + +f0 = f(t, y); + +for i = 1:n + e = zeros(n, 1); e(i) = 1; + J(:, i) = (f(t, y + h*e) - f0)/h; +end + +end diff --git a/src/+otp/+utils/jacobianApproximation.m b/src/+otp/+utils/jacobianApproximation.m new file mode 100644 index 00000000..e743bf18 --- /dev/null +++ b/src/+otp/+utils/jacobianApproximation.m @@ -0,0 +1,17 @@ +function J = jacobianApproximation(f, t, y) +% TODO: make this more robust + +n = numel(y); + +J = zeros(n, n); + +h = sqrt(1e-6); + +f0 = f(t, y); + +for i = 1:n + e = zeros(n, 1); e(i) = 1; + J(:, i) = (f(t, y + h*e) - f0)/h; +end + +end