! REF: ! - https://ejde.math.txstate.edu/conf-proc/21/k3/knowles.pdf ! - https://arxiv.org/pdf/2009.01911.pdf module spectrum_diff use iso_fortran_env use blas use linalg implicit none private public :: finite_difference public :: tvr_derivative public :: stencil_diff_5 public :: stencil_second_diff_5 public :: filter_diff interface finite_difference module procedure :: finite_difference_1 module procedure :: finite_difference_2 end interface interface finite_difference_driver module procedure :: finite_difference_driver_1 module procedure :: finite_difference_driver_2 end interface ! BLAS Routines: interface ! subroutine dgbmv(trans, m, n, kl, ku, alpha, a, lda, x, incx, beta, y, incy) ! use iso_fortran_env, only : int32, real64 ! character, intent(in) :: trans ! integer(int32), intent(in) :: m, n, kl, ku, lda, incx, incy ! real(real64), intent(in) :: alpha, beta, a(lda,*), x(*) ! real(real64), intent(inout) :: y(*) ! end subroutine ! subroutine dgemm(transa, transb, m, n, k, alpha, a, lda, b, ldb, beta, c, ldc) ! use iso_fortran_env, only : int32, real64 ! character, intent(in) :: transa, transb ! integer(int32), intent(in) :: m, n, k, lda, ldb, ldc ! real(real64), intent(in) :: alpha, beta, a(lda,*), b(ldb,*) ! real(real64), intent(inout) :: c(ldc,*) ! end subroutine ! subroutine dgemv(trans, m, n, alpha, a, lda, x, incx, beta, y, incy) ! use iso_fortran_env, only : int32, real64 ! character, intent(in) :: trans ! integer(int32), intent(in) :: m, n, lda, incx, incy ! real(real64), intent(in) :: alpha, beta, a(lda,*), x(*) ! real(real64), intent(inout) :: y(*) ! end subroutine subroutine dgesv(n, nrhs, a, lda, ipiv, b, ldb, info) use iso_fortran_env, only : int32, real64 integer(int32), intent(in) :: n, nrhs, lda, ldb real(real64), intent(inout) :: a(lda,*), b(ldb,*) integer(int32), intent(out) :: ipiv(*), info end subroutine end interface contains ! ------------------------------------------------------------------------------ pure subroutine finite_difference_driver_1(dt, x, dxdt) ! Arguments real(real64), intent(in) :: dt real(real64), intent(in), dimension(:) :: x real(real64), intent(out), dimension(:) :: dxdt ! Local Variables integer(int32) :: i, n ! Process n = size(x) if (n == 0) return if (n == 1) then dxdt = 0.0d0 return end if dxdt(1) = (x(2) - x(1)) / dt do i = 2, n - 1 dxdt(i) = 0.5d0 * (x(i + 1) - x(i - 1)) / dt end do dxdt(n) = (x(n) - x(n - 1)) / dt end subroutine ! ------------------------------------------------------------------------------ pure subroutine finite_difference_driver_2(t, x, dxdt) ! Arguments real(real64), intent(in), dimension(:) :: t, x real(real64), intent(out), dimension(:) :: dxdt ! Local Variables integer(int32) :: i, n ! Process n = size(x) if (n == 0) return if (n == 1) then dxdt = 0.0d0 return end if dxdt(1) = (x(2) - x(1)) / (t(2) - t(1)) do i = 2, n - 1 dxdt(i) = (x(i + 1) - x(i - 1)) / (t(i + 1) - t(i - 1)) end do dxdt(n) = (x(n) - x(n - 1)) / (t(n) - t(n - 1)) end subroutine ! ------------------------------------------------------------------------------ pure function finite_difference_1(dt, x) result(rst) !! Estimates the derivative of a data set by means of a naive !! implementation of a finite difference scheme based upon central !! differences. real(real64), intent(in) :: dt !! The time step between data points. real(real64), intent(in), dimension(:) :: x !! An N-element array containing the data whose derivative is to be !! estimated. real(real64), allocatable, dimension(:) :: rst !! An N-element array containing the derivative estimate. ! Local Variables integer(int32) :: n ! Initialization n = size(x) allocate(rst(n)) ! Process call finite_difference_driver(dt, x, rst) end function ! ------------------------------------------------------------------------------ pure function finite_difference_2(t, x) result(rst) !! Computes an estimate to the derivative of an evenly-sampled data !! set using total variation regularization. real(real64), intent(in), dimension(:) :: t !! An N-element array containing the time points at which x was sampled. real(real64), intent(in), dimension(:) :: x !! An N-element array containing the data whose derivative is to be !! estimated. real(real64), allocatable, dimension(:) :: rst !! An N-element array containing the derivative estimate. ! Local Variables integer(int32) :: n ! Initialization n = size(t) ! Input Checking if (size(x) /= n) return ! Memory Allocation allocate(rst(n)) ! Process call finite_difference_driver(t, x, rst) end function ! ****************************************************************************** ! TOTAL VARIATION REGULARIZATION ! ------------------------------------------------------------------------------ ! REF: https://oliver-k-ernst.medium.com/how-to-differentiate-noisy-signals-2baf71b8bb65 ! https://github.com/smrfeld/Total-Variation-Regularization-Derivative-Python/blob/main/python/diff_tvr.py ! https://github.com/florisvb/PyNumDiff/blob/master/pynumdiff/total_variation_regularization/__chartrand_tvregdiff__.py ! Constructs the N-by-N+1 D matrix: ! | -1 1 | ! D = 1/dx | 0 -1 1 | ! | 0 0 -1 | pure subroutine make_d_full(dx, d) ! Arguments real(real64), intent(in) :: dx real(real64), intent(out), dimension(:,:) :: d ! Local Variables integer(int32) :: j, n real(real64) :: idx ! Process n = size(d, 1) idx = 1.0d0 / dx d = 0.0d0 do j = 1, n + 1 if (j > 1) d(j-1,j) = idx if (j <= n) d(j,j) = -idx end do end subroutine ! ------------------------------------------------------------------------------ ! Constructs the N-by-N+1 A matrix. pure subroutine make_a_full(dx, a) ! Arguments real(real64), intent(in) :: dx real(real64), intent(out), dimension(:,:) :: a ! Local Variables integer(int32) :: j, n real(real64) :: hdx ! Process n = size(a, 1) hdx = 0.5d0 * dx do j = 1, n + 1 if (j == 1) then a(:,j) = hdx else if (j > 1) a(j-1,j) = hdx if (j <= n) a(j:,j) = dx end if end do end subroutine ! ------------------------------------------------------------------------------ ! Constructs the N-by-N E matrix. The matrix is a diagonal matrix with only the ! diagonal stored. subroutine make_e(d, u, e) ! Arguments real(real64), intent(in), dimension(:,:) :: d real(real64), intent(in), dimension(:) :: u real(real64), intent(out), dimension(:) :: e ! Local Variables integer(int32) :: j, n, n1 real(real64) :: eps ! Process eps = sqrt(epsilon(eps)) n = size(d, 1) n1 = n + 1 call dgemv("N", n, n1, 1.0d0, d, n, u, 1, 0.0d0, e, 1) do j = 1, n e(j) = 1.0d0 / sqrt(e(j)**2 + eps) end do end subroutine ! ------------------------------------------------------------------------------ subroutine tvr_diff_small(alpha, dt, x, maxiter, dxdt, tol, niter) real(real64), intent(in) :: alpha ! variational parameter real(real64), intent(in) :: dt ! time step real(real64), intent(in), dimension(:) :: x ! data array to differentiate integer(int32), intent(in) :: maxiter ! max # of iterations real(real64), intent(out), dimension(:) :: dxdt ! derivative dx/dt real(real64), intent(in) :: tol ! tolerance on change in gradient integer(int32), intent(out) :: niter ! # of iterations taken ! Local Variables integer(int32) :: i, n, n1, flag integer(int32), allocatable, dimension(:) :: ipiv real(real64) :: offset, nrm, nrmold real(real64), allocatable, dimension(:,:) :: d, a, dte, l, ata, h real(real64), allocatable, dimension(:) :: e, u, atb, atau, lu, g ! Initialization n = size(x) n1 = n + 1 nrmold = huge(nrmold) ! Memory Allocations allocate( & d(n, n1), & a(n, n1), & e(n), & u(n1), & atb(n), & dte(n1, n), & l(n1, n1), & ata(n1, n1), & atau(n1), & lu(n1), & g(n1), & h(n1, n1), & ipiv(n1) & ) ! Construct matrices call make_d_full(dt, d) call make_a_full(dt, a) call dgemm("T", "N", n1, n1, n, 1.0d0, a, n, a, n, 0.0d0, ata, n1) ! A**T * A ! Provide a first estimate of the derivative u(1) = 0.0d0 u(2:) = finite_difference(dt, x) ! Precompute A**T * (X(1) - X) call dgemv("T", n, n1, 1.0d0, a, n, offset - x, 1, 0.0d0, atb, 1) ! Iteration Process do i = 1, maxiter ! Compute E and L call make_e(d, u, e) call diag_mtx_mult(.false., .true., dt, e, d, 0.0d0, dte) ! dt * D**T * E call dgemm("N", "N", n1, n1, n, 1.0d0, dte, n1, d, n, 0.0d0, l, n1) ! L = (dx * D**T * E) * D ! Compute the gradient call dgemv("N", n1, n1, 1.0d0, ata, n1, u, 1, 0.0d0, atau, 1) call dgemv("N", n1, n1, alpha, l, n1, u, 1, 0.0d0, lu, 1) g = atau + atb + lu ! Compute H h = ata + alpha * l ! Solve H * s = g, for s - stored in g call dgesv(n1, 1, h, n1, ipiv, g, n1, flag) if (flag /= 0) return ! Check the solution nrm = norm2(g) if (abs(nrm - nrmold) < tol) exit nrmold = nrm ! Update the derivative estimate u = u - g end do niter = min(i, maxiter) ! Extract the computed derivative dxdt = u(1:n) end subroutine ! ------------------------------------------------------------------------------ function tvr_derivative(dt, x, alpha, maxiter, tol, niter) result(rst) !! Computes an estimate to the derivative of an evenly-sampled data !! set using total variation regularization. !! !! See Also !! !! - van Breugel, Floris & Brunton, Bingni & Kutz, J.. (2020). Numerical !! differentiation of noisy data: A unifying multi-objective optimization !! framework. !! !! - Oliver K. Ernst, Ph. D. (2021, February 16). How to differentiate !! noisy signals. Medium. https://oliver-k-ernst.medium.com/how-to-differentiate-noisy-signals-2baf71b8bb65 real(real64), intent(in) :: dt !! The time step between data points. real(real64), intent(in), dimension(:) :: x !! An N-element array containing the data whose derivative is !! to be estimated. real(real64), intent(in) :: alpha !! The regularization parameter. integer(int32), intent(in), optional :: maxiter !! The maximum number of iterations to allow. The default is 20 !! iterations. real(real64), intent(in), optional :: tol !! The convergence tolerance to use. The tolerance is !! applied to the difference in Euclidean norms of the derivative update !! vector. Once the norm of the update vector is changing less than !! this tolerance, the iteration process will terminate. The default !! is 1e-3. integer(int32), intent(out), optional :: niter !! The number of iterations actually performed. real(real64), allocatable, dimension(:) :: rst !! An N-element array containing the estimate of the derivative. ! Local Variables integer(int32) :: mi, n, ni real(real64) :: gtol ! Initialization if (present(maxiter)) then mi = maxiter else mi = 20 end if if (present(tol)) then gtol = tol else gtol = 1.0d-3 end if n = size(x) allocate(rst(n)) ! Process call tvr_diff_small(alpha, dt, x, mi, rst, gtol, ni) if (present(niter)) niter = ni end function ! ****************************************************************************** ! V1.1.2 ADDITIONS ! ------------------------------------------------------------------------------ pure function stencil_diff_5(dt, x) result(rst) !! Utilizes a 5-point stencil to estimate the derivative of a data set. !! !! See Also !! !! - [Wikipedia](https://en.wikipedia.org/wiki/Five-point_stencil) real(real64), intent(in) :: dt !! The time step between data points. real(real64), intent(in), dimension(:) :: x !! An N-element array containing the data whose derivative is to be !! estimated. real(real64), allocatable, dimension(:) :: rst !! An N-element array containing the derivative estimate. ! Local Variables integer(int32) :: i, n ! Initialization n = size(x) allocate(rst(n)) ! Process ! Step in and out of the problem via finite differences; else, use ! a 5-point stencil of the form: ! ! f'(x) = (-f(x+2h) + 8f(x+h) - 8f(x-h) + f(x-2h)) / (12h) rst(1) = (x(2) - x(1)) / dt rst(2) = (x(3) - x(2)) / dt do i = 3, n - 2 rst(i) = (-x(i + 2) + 8.0d0 * (x(i + 1) - x(i - 1)) + x(i - 2)) / & (12.0d0 * dt) end do rst(n-1) = (x(n-1) - x(n-2)) / dt rst(n) = (x(n) - x(n-1)) / dt end function ! ------------------------------------------------------------------------------ pure function stencil_second_diff_5(dt, x) result(rst) !! Utilizes a 5-point stencil to estimate the second derivative of a data !! set. !! !! See Also !! !! - [Wikipedia](https://en.wikipedia.org/wiki/Five-point_stencil) real(real64), intent(in) :: dt !! The time step between data points. real(real64), intent(in), dimension(:) :: x !! An N-element array containing the data whose derivative is to be !! estimated. real(real64), allocatable, dimension(:) :: rst !! An N-element array containing the derivative estimate. ! Local Variables integer(int32) :: i, n real(real64) :: h2 ! Initialization n = size(x) allocate(rst(n)) ! Process ! Step in and out of the problem via finite differences; else, use ! a 5-point stencil of the form: ! ! f"(x) = (-f(x+2h) + 16f(x+h) - 30f(x) + 16f(x-h) - f(x-2h)) / (12h**2) h2 = dt**2 rst(1) = (x(3) - 2.0d0 * x(2) + x(1)) / h2 rst(2) = (x(4) - 2.0d0 * x(3) + x(2)) / h2 do i = 3, n - 2 rst(i) = (-x(i + 2) - 3.0d1 * x(i) + 1.6d1 * (x(i + 1) + x(i - 1)) - & x(i - 2)) / (1.2d1 * h2) end do rst(n - 1) = (x(n - 1) - 2.0d0 * x(n - 2) + x(n - 3)) / h2 rst(n) = (x(n) - 2.0d0 * x(n - 1) + x(n - 2)) / h2 end function ! ****************************************************************************** ! V1.1.3 ADDITIONS ! ------------------------------------------------------------------------------ pure function filter_diff(dt, x, fc) result(rst) !! Estimates the derivative of a signal by utilization of a second-order !! system as a filter. real(real64), intent(in) :: dt !! The time step between data points. real(real64), intent(in), dimension(:) :: x !! An N-element array containing the data whose derivative is to be !! estimated. real(real64), intent(in) :: fc !! The filter cutoff frequency, in Hz. real(real64), allocatable, dimension(:,:) :: rst !! An N-element array containing the filtered signal in the first column !! and the derivative estimate in the second. ! Parameters real(real64), parameter :: pi = 2.0d0 * acos(0.0d0) real(real64), parameter :: zeta = 0.5d0 * sqrt(2.0d0) ! Local Variables integer(int32) :: i, n real(real64) :: fs, wn ! Initialization n = size(x) fs = 1.0d0 / dt wn = 2.0d0 * pi * fc ! Input Checking if (fc >= 0.5d0 * fs .or. fc <= 0.0d0) return ! Memory Allocations allocate(rst(n,2)) ! Define the initial conditions rst(1,1) = x(1) ! output initial value is equivalent to the original value rst(1,2) = (x(2) - x(1)) / dt ! finite difference estimate of the first point ! Perform the integration using Euler's method do i = 2, n ! Predictor Stage (Explicit Method) rst(i,:) = rst(i-1,:) + dt * fcn(rst(i-1,:), x(i-1), wn, zeta) ! Corrector Stage (Implicit Method) rst(i,:) = rst(i-1,:) + dt * fcn(Rst(i,:), x(i), wn, zeta) end do end function ! ---------- pure function fcn(x, y, wn, zeta) result(dxdt) !! The second-order equations of motion. real(real64), intent(in) :: x(2) !! The current state variables. real(real64), intent(in) :: y !! The current value of the original signal. real(real64), intent(in) :: wn !! The second-order system natural frequency, in rad/s. real(real64), intent(in) :: zeta !! The second-order system damping ratio. real(real64) :: dxdt(2) !! The output derivative values. ! Equation of Motion: ! x" + 2 * zeta * wn * x' + wn**2 * x = wn**2 * y dxdt(1) = x(2) dxdt(2) = wn**2 * (y - x(1)) - 2.0d0 * zeta * wn * x(2) end function ! ------------------------------------------------------------------------------ end module