! Copyright (c) 2022-2026 Jason Christopherson ! SPDX-License-Identifier: MIT ! ! Permission is hereby granted, free of charge, to any person obtaining a copy ! of this software and associated documentation files (the "Software"), to deal ! in the Software without restriction, including without limitation the rights ! to use, copy, modify, merge, publish, distribute, sublicense, and/or sell ! copies of the Software, and to permit persons to whom the Software is ! furnished to do so, subject to the following conditions: ! ! The Software is provided "as is", without warranty of any kind, express or ! implied, including but not limited to the warranties of merchantability, ! fitness for a particular purpose and noninfringement. module dynamics_controls use iso_fortran_env use nonlin_polynomials use ieee_arithmetic use dynamics_error_handling use diffeq use linalg, only : eigen use lapack, only : dgetrf, dgetri, dgetrs, zgels implicit none private public :: polynomial public :: state_space public :: transfer_function public :: operator(*) public :: lti_solve public :: ss_excitation public :: ode_integrator ! ------------------------------------------------------------------------------ type state_space !! Defines a state-space representation of a dynamic system. This !! implementation takes the form: !! !! $$ \dot{x}(t) = A x(t) + B u(t) $$ !! $$ y(t) = C x(t) + D u(t) $$ !! !! Where: !! !! - \( t \) denotes time. !! !! - \( x(t) \) is the state vector. !! !! - \( u(t) \) is the input vector. !! !! - \( y(t) \) is the output vector. real(real64), allocatable, dimension(:,:) :: A !! The N-by-N dynamics matrix, where N is the number of state !! variables. real(real64), allocatable, dimension(:,:) :: B !! The N-by-M input matrix, where M is the number of inputs. real(real64), allocatable, dimension(:,:) :: C !! The P-by-N output matrix, where P is the number of outputs. real(real64), allocatable, dimension(:,:) :: D !! The P-by-M feedthrough matrix. contains procedure, public :: evaluate_derivatives => ss_eval_deriv procedure, public :: evaluate_output => ss_eval_output procedure, public :: poles => ss_poles procedure, public :: zeros => ss_zeros generic, public :: transfer_function => ss_transfer_fcn, & ss_transfer_fcn_omega, ss_transfer_fcn_array, & ss_transfer_fcn_omega_array procedure, private :: ss_transfer_fcn procedure, private :: ss_transfer_fcn_omega procedure, private :: ss_transfer_fcn_array procedure, private :: ss_transfer_fcn_omega_array end type interface state_space module procedure :: state_space_init module procedure :: state_space_init_scalar module procedure :: state_space_init_matrices module procedure :: state_space_init_pid module procedure :: state_space_init_pid_plant end interface ! ------------------------------------------------------------------------------ type transfer_function !! Defines a transfer function for a continuous system of the form !! \( H(s) = \frac{Y(s)}{X(s)} \). type(polynomial) :: Y !! The numerator polynomial \(Y(s)\) in !! \( H(s) = \frac{Y(s)}{X(s)} \). The polynomial coefficients !! are stored in acending order such that !! \( y_1 + y_2 s + y_3 s^2 ... \). type(polynomial) :: X !! The denominator polynomial \(X(s)\) in !! \( H(s) = \frac{Y(s)}{X(s)} \). The polynomial coefficients !! are stored in acending order such that !! \( x_1 + x_2 s + x_3 s^2 ... \). contains procedure, private :: tf_eval_s procedure, private :: tf_eval_omega generic, public :: evaluate => tf_eval_omega, tf_eval_s procedure, public :: poles => tf_poles procedure, public :: zeros => tf_zeros procedure, public :: to_ccf_state_space => tf_to_ccf_statespace procedure, public :: to_ocf_state_space => tf_to_ocf_statespace end type interface transfer_function module procedure :: init_tf_array module procedure :: init_tf_poly end interface ! ------------------------------------------------------------------------------ interface operator(*) module procedure :: tf_tf_mult module procedure :: poly_tf_mult module procedure :: tf_poly_mult module procedure :: tf_scalar_mult module procedure :: scalar_tf_mult end interface ! ------------------------------------------------------------------------------ interface subroutine ss_excitation(t, u, args) !! A routine for computing the excitation vector for a state-space !! model. use iso_fortran_env, only : real64 real(real64), intent(in) :: t !! The time value at which to compute the excitation. real(real64), intent(out), dimension(:) :: u !! The excitation vector. class(*), intent(inout), optional :: args !! An optional argument used to pass objects in and out of the !! routine. end subroutine end interface ! ------------------------------------------------------------------------------ ! PRIVATE TYPES ! ------------------------------------------------------------------------------ type argument_container procedure(ss_excitation), pointer, nopass :: excitation class(*), allocatable :: user_args logical :: has_user_args class(state_space), pointer :: model end type contains ! ****************************************************************************** ! STATE_SPACE ! ------------------------------------------------------------------------------ pure subroutine validate_state_space(this) !! Validates allocation and matrix dimensions for a state-space model. class(state_space), intent(in) :: this !! The state-space model to validate. integer(int32) :: n, m, p if (.not.allocated(this%A) .or. .not.allocated(this%B) .or. & .not.allocated(this%C) .or. .not.allocated(this%D)) & error stop DYN_INVALID_INPUT_ERROR n = size(this%A, 1) m = size(this%B, 2) p = size(this%C, 1) if (size(this%A, 2) /= n .or. m < 1 .or. p < 1 .or. & size(this%B, 1) /= n .or. size(this%C, 2) /= n .or. & size(this%D, 1) /= p .or. size(this%D, 2) /= m) & error stop DYN_INVALID_INPUT_ERROR end subroutine ! ------------------------------------------------------------------------------ pure function state_space_init(m, b, k, n_out) result(rst) !! Initializes the state space model. !! For the second-order mechanical system !! $$ M\ddot{q}+B\dot{q}+Kq=u, $$ !! the state \(x=[q^T\ \dot{q}^T]^T\) satisfies !! $$ \dot{x}=Ax+Bu,\qquad y=Cx+Du. $$ !! !! The output matrix \(C\) is initialized to one, and the !! feedthrough matrix \(D\) is initialized to zero. real(real64), intent(in), dimension(:,:) :: m !! The N-by-N mass matrix. real(real64), intent(in), dimension(size(m, 1), size(m, 2)) :: b !! The N-by-N damping matrix. real(real64), intent(in), dimension(size(m, 1), size(m, 2)) :: k !! The N-by-N stiffness matrix. integer(int32), intent(in), optional :: n_out !! The number of outputs. The default is 1. type(state_space) :: rst !! The [[state_space]] model. ! Local Variables integer(int32) :: ii, jj, n, p, q, lwork, info integer(int32), allocatable, dimension(:) :: pvt real(real64) :: temp(1) real(real64), allocatable, dimension(:) :: work ! Initialization p = size(m, 1) if (p < 1 .or. size(m, 2) /= p) & error stop DYN_INVALID_INPUT_ERROR n = 2 * p q = 1 if (present(n_out)) q = n_out if (q < 1) error stop DYN_INVALID_INPUT_ERROR allocate( & rst%A(n, n), & rst%B(n, p), & rst%D(q, p), & source = 0.0d0 & ) allocate(rst%C(q, n), source = 1.0d0) allocate(pvt(p)) ! Workspace call dgetri(p, rst%b(p+1:n,:), p, pvt, temp, -1, info) if (info /= 0) error stop DYN_INVALID_INPUT_ERROR lwork = int(temp(1), int32) allocate(work(lwork)) ! Build p-by-p Identity sub-matrix matrix jj = p + 1 do ii = 1, p rst%A(ii,jj) = 1.0d0 jj = jj + 1 end do ! Fill in the matrices rst%B(p+1:n,1:p) = m rst%A(p+1:n,1:p) = -k rst%A(p+1:n,p+1:n) = -b call dgetrf(p, p, rst%B(p+1:n,1:p), p, pvt, info) if (info /= 0) error stop DYN_INVALID_INPUT_ERROR call dgetrs('N', p, p, rst%B(p+1:n,1:p), p, pvt, rst%A(p+1:n,1:p), p, & info) if (info /= 0) error stop DYN_INVALID_INPUT_ERROR call dgetrs('N', p, p, rst%B(p+1:n,1:p), p, pvt, rst%A(p+1:n,p+1:n), & p, info) if (info /= 0) error stop DYN_INVALID_INPUT_ERROR call dgetri(p, rst%B(p+1:n,1:p), p, pvt, work, lwork, info) if (info /= 0) error stop DYN_INVALID_INPUT_ERROR end function ! ------------------------------------------------------------------------------ pure function state_space_init_scalar(m, b, k) result(rst) !! Initializes the state space model. !! The scalar realization corresponds to !! $$ m\ddot{q}+b\dot{q}+kq=u,\qquad !! A=\begin{bmatrix}0&1\\-k/m&-b/m\end{bmatrix},\quad !! B=\begin{bmatrix}0\\1/m\end{bmatrix}. $$ !! !! The output matrix \(C\) is initialized to one, and the !! feedthrough matrix \(D\) is initialized to zero. real(real64), intent(in) :: m !! The mass. real(real64), intent(in) :: b !! The damping. real(real64), intent(in) :: k !! The stiffness. type(state_space) :: rst !! The [[state_space]] model. if (m <= 0.0d0) error stop DYN_INVALID_INPUT_ERROR ! Process allocate( & rst%A(2, 2), & rst%B(2, 1), & rst%D(1, 1), & source = 0.0d0 & ) allocate(rst%C(1, 2), source = 1.0d0) rst%A(2,1) = -k / m rst%A(1,2) = 1.0d0 rst%A(2,2) = -b / m rst%B(2,1) = 1.0d0 / m end function ! ------------------------------------------------------------------------------ pure function state_space_init_matrices(a, b, c, d) result(rst) !! Initializes the state space model. !! The stored realization uses the continuous-time equations !! $$ \dot{x}=Ax+Bu,\qquad y=Cx+Du. $$ real(real64), intent(in), dimension(:,:) :: a !! The N-by-N dynamics matrix. real(real64), intent(in), dimension(:,:) :: b !! The N-by-M input matrix. real(real64), intent(in), dimension(:,:) :: c !! The P-by-N output matrix. real(real64), intent(in), dimension(:,:) :: d !! The P-by-M feedthrough matrix. type(state_space) :: rst !! The resulting [[state_space]] object. if (size(a, 1) < 1 .or. size(a, 2) /= size(a, 1) .or. & size(b, 1) /= size(a, 1) .or. size(c, 2) /= size(a, 1) .or. & size(d, 1) /= size(c, 1) .or. size(d, 2) /= size(b, 2)) & error stop DYN_INVALID_INPUT_ERROR allocate(rst%A, source = a) allocate(rst%B, source = b) allocate(rst%C, source = c) allocate(rst%D, source = d) end function ! ------------------------------------------------------------------------------ pure function state_space_init_pid(kp, ki, kd, tau, a, b, c, d) result(rst) !! Initializes a state-space model that employs a closed-loop PID !! controller. !! !! The PID model is augmented into the plant model as follows. !! !! $$ e = r - y $$ !! $$ \dot{x_1} = e $$ !! $$ \dot{x_3} = -\frac{x_3}{\tau} + \frac{e}{\tau} $$ !! $$ u = K_{i} x_{i} + K_{p} e + \frac{K_{d}}{\tau} \left(e - !! x_{3} \right) $$ !! $$ \alpha = K_{p} + \frac{K_{d}}{\tau} $$ !! $$ \beta = \frac{1}{1 + \alpha D} $$ !! $$ x = \left[ \begin{matrix} x_{p} \\ x_{1} \\ x_{3} \end{matrix} !! \right] $$ !! $$ \dot{x} = A_{cl} x + B_{cl} r $$ !! $$ y = C_{cl} x + D_{cl} r $$ !! !! Where the augmented matrices are as follows. !! !! $$ A_{cl} = \left[ \begin{matrix} A - \alpha \beta B C & !! \beta K_{i} B & -\frac{\beta K_{d}}{\tau} B \\ !! -C + \alpha \beta D C & -\beta K_{i} D & \frac{\beta K_{d}}{\tau} D !! \\ \frac{1}{\tau}\left(-C + \alpha \beta D C \right) & !! -\frac{\beta K_{i}}{\tau} D & -\frac{1}{\tau} + !! \frac{\beta K_{d}}{\tau^{2}} D \end{matrix} \right] $$ !! $$ B_{cl} = \left[ \begin{matrix} \alpha \beta B \\ 1 - !! \alpha \beta D \\ \frac{1}{\tau} \left( 1 - \alpha \beta D \right) !! \end{matrix} \right] $$ !! $$ C_{cl} = \left[ \begin{matrix} C - \alpha \beta D C & !! \beta K_{i} D & -\frac{\beta K_{d}}{\tau} D \end{matrix} \right] $$ !! $$ D_{cl} = \alpha \beta D $$ real(real64), intent(in) :: kp !! The proportional gain term. real(real64), intent(in) :: ki !! The integral gain term. real(real64), intent(in) :: kd !! The derivative gain term. real(real64), intent(in) :: tau !! The time constant of the first order derivative filter !! \( K_{d} \frac{s}{\tau s + 1} \). real(real64), intent(in), dimension(:,:) :: a !! The N-by-N dynamics matrix for the plant. real(real64), intent(in), dimension(size(a, 1), 1) :: b !! The N-by-1 input matrix for the plant. real(real64), intent(in), dimension(1, size(a, 1)) :: c !! The 1-by-N output matrix for the plant. real(real64), intent(in), dimension(1, 1) :: d !! The 1-by-1 feedthrough matrix for the plant. type(state_space) :: rst !! The resulting [[state_space]] object. ! Local Variables integer(int32) :: n, n1, n2 real(real64) :: alpha, beta, ab real(real64), allocatable, dimension(:,:) :: dc if (tau <= 0.0d0 .or. size(a, 1) < 1 .or. & size(a, 2) /= size(a, 1) .or. size(b, 1) /= size(a, 1) .or. & size(c, 2) /= size(a, 1)) error stop DYN_INVALID_INPUT_ERROR ! Initialization n = size(a, 1) n1 = n + 1 n2 = n + 2 allocate( & rst%A(n2, n2), & rst%B(n2, 1), & rst%C(1, n2), & rst%D(1, 1), & source = 0.0d0 & ) dc = matmul(d, c) ! Process alpha = kp + kd / tau if (d(1,1) == 0.0d0) then beta = 1.0d0 else if (abs(1.0d0 + alpha * d(1,1)) <= & sqrt(epsilon(1.0d0)) * max(1.0d0, abs(alpha * d(1,1)))) & error stop DYN_INVALID_INPUT_ERROR beta = 1.0d0 / (1.0d0 + alpha * d(1,1)) end if ab = alpha * beta rst%A(1:n,1:n) = a - ab * matmul(b, c) rst%A(n1,1:n) = -c(1,1:n) + ab * dc(1,1:n) rst%A(n2,1:n) = (1.0d0 / tau) * rst%A(n1,1:n) rst%A(1:n,n1) = beta * ki * b(1:n,1) rst%A(n1,n1) = -beta * ki * d(1,1) rst%A(n2,n1) = rst%A(n1,n1) / tau rst%A(1:n,n2) = -(beta * kd / tau) * b(1:n,1) rst%A(n1,n2) = beta * kd * d(1,1) / tau rst%A(n2,n2) = -1.0d0 / tau + beta * kd * d(1,1) / (tau**2) rst%B(1:n,1) = ab * b(1:n,1) rst%B(n1,1) = 1.0d0 - ab * d(1,1) rst%B(n2,1) = (1.0d0 / tau) * rst%B(n1,1) rst%C(1,1:n) = c(1,1:n) - ab * dc(1,1:n) rst%C(1,n1) = beta * ki * d(1,1) rst%C(1,n2) = -(beta * kd / tau) * d(1,1) rst%D = ab * d end function ! ------------------------------------------------------------------------------ pure function state_space_init_pid_plant(kp, ki, kd, tau, plant) result(rst) !! Initializes a state-space model that employs a closed-loop PID !! controller. !! !! The PID model is augmented into the plant model as follows. !! !! $$ e = r - y $$ !! $$ \dot{x_1} = e $$ !! $$ \dot{x_3} = -\frac{x_3}{\tau} + \frac{e}{\tau} $$ !! $$ u = K_{i} x_{i} + K_{p} e + \frac{K_{d}}{\tau} \left(e - !! x_{3} \right) $$ !! $$ \alpha = K_{p} + \frac{K_{d}}{\tau} $$ !! $$ \beta = \frac{1}{1 + \alpha D} $$ !! $$ x = \left[ \begin{matrix} x_{p} \\ x_{1} \\ x_{3} \end{matrix} !! \right] $$ !! $$ \dot{x} = A_{cl} x + B_{cl} r $$ !! $$ y = C_{cl} x + D_{cl} r $$ !! !! Where the augmented matrices are as follows. !! !! $$ A_{cl} = \left[ \begin{matrix} A - \alpha \beta B C & !! \beta K_{i} B & -\frac{\beta K_{d}}{\tau} B \\ !! -C + \alpha \beta D C & -\beta K_{i} D & \frac{\beta K_{d}}{\tau} D !! \\ \frac{1}{\tau}\left(-C + \alpha \beta D C \right) & !! -\frac{\beta K_{i}}{\tau} D & \frac{1}{\tau} \left( !! 1 + \frac{\beta K_{d}}{\tau^{2}} D \right) \end{matrix} \right] $$ !! $$ B_{cl} = \left[ \begin{matrix} \alpha \beta B \\ 1 - !! \alpha \beta D \\ \frac{1}{\tau} \left( 1 - \alpha \beta D \right) !! \end{matrix} \right] $$ !! $$ C_{cl} = \left[ \begin{matrix} C - \alpha \beta D C & !! \beta K_{i} D & -\frac{\beta K_{d}}{\tau} D \end{matrix} \right] $$ !! $$ D_{cl} = \alpha \beta D $$ real(real64), intent(in) :: kp !! The proportional gain term. real(real64), intent(in) :: ki !! The integral gain term. real(real64), intent(in) :: kd !! The derivative gain term. real(real64), intent(in) :: tau !! The time constant of the first order derivative filter !! \( K_{d} \frac{s}{\tau s + 1} \). class(state_space), intent(in) :: plant !! The plant model. type(state_space) :: rst !! The resulting [[state_space]] object. rst = state_space_init_pid(kp, ki, kd, tau, plant%A, plant%B, & plant%C, plant%D) end function ! ------------------------------------------------------------------------------ pure function ss_eval_deriv(this, u, x) result(rst) !! Evaluates the state time derivative \( \dot{x}(t) = A x(t) + !! B u(t) \). !! This is the vector field evaluated at one state and input sample. class(state_space), intent(in) :: this !! The [[state_space]] object. real(real64), intent(in), dimension(:) :: u !! The M-element input array. real(real64), intent(in), dimension(:) :: x !! The N-element state array. real(real64), allocatable, dimension(:) :: rst !! The N-element state time derivative vector. call validate_state_space(this) rst = matmul(this%A, x) + matmul(this%B, u) end function ! ------------------------------------------------------------------------------ pure function ss_eval_output(this, u, x) result(rst) !! Evaluates the output vector \( y(t) = C x(t) + D u(t) \). !! The \(D u\) term is the direct feedthrough contribution. class(state_space), intent(in) :: this !! The [[state_space]] object. real(real64), intent(in), dimension(:) :: u !! The M-element input array. real(real64), intent(in), dimension(:) :: x !! The N-element state array. real(real64), allocatable, dimension(:) :: rst !! The P-element output array. call validate_state_space(this) rst = matmul(this%C, x) + matmul(this%D, u) end function ! ------------------------------------------------------------------------------ pure function ss_poles(this) result(rst) !! Computes the poles of the state space model. !! The poles are the eigenvalues of \(A\) and are the roots of !! $$ \det(sI-A)=0. $$ class(state_space), intent(in) :: this !! The [[state_space]] object. complex(real64), allocatable, dimension(:) :: rst !! The poles of the model. ! Local Variables integer(int32) :: n real(real64), allocatable, dimension(:,:) :: ac call validate_state_space(this) ! Process n = size(this%A, 1) allocate(rst(n)) if (n == 0) return call eigen(this%A, rst) end function ! ------------------------------------------------------------------------------ pure function ss_zeros(this) result(rst) !! Computes the zeros of the state space model. !! Transmission zeros are obtained from the generalized eigenproblem of !! the Rosenbrock system matrix !! $$ \mathcal{R}(s)=\begin{bmatrix}sI-A&-B\\C&D\end{bmatrix}. $$ class(state_space), intent(in) :: this !! The [[state_space]] object. complex(real64), allocatable, dimension(:) :: rst !! The zeros of the model. ! Local Variables integer(int32) :: i, j, n, m, p, nz real(real64), allocatable, dimension(:,:) :: Az, Bz complex(real64), allocatable, dimension(:) :: buffer, trimmed call validate_state_space(this) ! Initialization n = size(this%A, 1) m = size(this%B, 2) p = size(this%C, 1) nz = n + max(m, p) allocate(Az(nz, nz), Bz(nz, nz), source = 0.0d0) allocate(buffer(nz), trimmed(nz)) ! Build the matrices Az(1:n,1:n) = this%A Az(1:n,n+1:n+m) = this%B do i = 1, nz Bz(i,i) = 1.0d0 end do Bz(n+1:n+p,1:n) = this%C Bz(n+1:n+p,n+1:n+m) = this%D ! Compute the eigenvalues call eigen(Az, Bz, buffer) ! Only keep the physically realizable zeros - exclude NaN's and (0,0) j = 0 do i = 1, nz if (ieee_is_nan(real(buffer(i)))) cycle if (ieee_is_nan(aimag(buffer(i)))) cycle if (.not.ieee_is_finite(real(buffer(i)))) cycle if (.not.ieee_is_finite(aimag(buffer(i)))) cycle if (abs(buffer(i)) <= epsilon(0.0d0)) cycle j = j + 1 trimmed(j) = buffer(i) end do rst = trimmed(1:j) end function ! ------------------------------------------------------------------------------ pure function ss_transfer_fcn(this, s) result(rst) !! Evaluates the transfer functions for the model at the parameter \(s\). !! In the Laplace domain, !! $$ H(s)=C(sI-A)^{-1}B+D. $$ class(state_space), intent(in) :: this !! The [[state_space]] object. complex(real64), intent(in) :: s !! The frequency at which to evaluate the transfer functions. complex(real64), allocatable, dimension(:,:) :: rst !! The resulting transfer functions. ! Local Variables integer(int32) :: j, ninput, noutput, n, info, lwork complex(real64) :: temp(1) complex(real64), allocatable, dimension(:) :: work, Bj complex(real64), allocatable, dimension(:,:) :: A, Ac call validate_state_space(this) ! Initialization n = size(this%A, 1) ninput = size(this%B, 2) noutput = size(this%C, 1) allocate(rst(noutput, ninput)) allocate(Ac(n, n), Bj(n)) ! Determine DGELS workspace requirements call zgels('N', n, n, 1, Ac, n, Bj, n, temp, -1, info) if (info /= 0) error stop DYN_INVALID_INPUT_ERROR lwork = int(temp(1), int32) allocate(work(lwork)) ! Build s * I - A do j = 1, n Ac(:,j) = -this%A(:,j) Ac(j,j) = Ac(j,j) + s end do allocate(A(n, n), source = Ac) ! For each column j, solve Ac * x(j) = B(j) and then compute ! the output: C x(j) + D(j) do j = 1, ninput if (j /= 1) A = Ac ! We need a copy as A will be overwritten Bj = this%B(:,j) call zgels('N', n, n, 1, A, n, Bj, n, work, lwork, info) if (info /= 0) error stop DYN_INVALID_INPUT_ERROR rst(:,j) = matmul(this%C, Bj) + this%D(:,j) end do end function ! ------------------------------------------------------------------------------ pure function ss_transfer_fcn_omega(this, omega) result(rst) !! Evaluates the transfer functions for the model at frequency \(\omega\). !! This evaluates the transfer matrix on the imaginary axis, \(s=j\omega\). class(state_space), intent(in) :: this !! The [[state_space]] object. real(real64), intent(in) :: omega !! The frequency at which to evaluate the transfer functions. complex(real64), allocatable, dimension(:,:) :: rst !! The resulting transfer functions. ! Local Variables complex(real64), parameter :: j = (0.0d0, 1.0d0) complex(real64) :: s ! Process s = j * omega rst = ss_transfer_fcn(this, s) end function ! ------------------------------------------------------------------------------ pure function ss_transfer_fcn_array(this, s) result(rst) !! Evaluates the transfer functions for the model at the frequencies given !! in the array \(s\). class(state_space), intent(in) :: this !! The [[state_space]] object. complex(real64), intent(in), dimension(:) :: s !! The frequencies at which to evaluate the transfer functions. complex(real64), allocatable, dimension(:,:,:) :: rst !! The resulting transfer functions, with each page of the array !! containing the transfer functions for a specific frequency. ! Local Variables integer(int32) :: i, ninput, noutput, n ! Initialization ninput = size(this%B, 2) noutput = size(this%C, 1) n = size(s) allocate(rst(noutput, ninput, n)) ! Process do concurrent (i = 1:n) rst(:,:,i) = ss_transfer_fcn(this, s(i)) end do end function ! ------------------------------------------------------------------------------ pure function ss_transfer_fcn_omega_array(this, omega) result(rst) !! Evaluates the transfer functions for the model at the frequencies given !! in the array \(omega\). class(state_space), intent(in) :: this !! The [[state_space]] object. real(real64), intent(in), dimension(:) :: omega !! The frequencies at which to evaluate the transfer functions. complex(real64), allocatable, dimension(:,:,:) :: rst !! The resulting transfer functions, with each page of the array !! containing the transfer functions for a specific frequency. ! Local Variables integer(int32) :: i, ninput, noutput, n ! Initialization ninput = size(this%B, 2) noutput = size(this%C, 1) n = size(omega) allocate(rst(noutput, ninput, n)) ! Process do concurrent (i = 1:n) rst(:,:,i) = ss_transfer_fcn_omega(this, omega(i)) end do end function ! ****************************************************************************** ! TRANSFER_FUNCTION ! ------------------------------------------------------------------------------ pure function init_tf_poly(y, x) result(rst) !! Initializes a new transfer function. class(polynomial), intent(in) :: y !! The numerator polynomial \(Y(s)\) in !! \( H(s) = \frac{Y(s)}{X(s)} \). class(polynomial), intent(in) :: x !! The denominator polynomial \(X(s)\) in !! \( H(s) = \frac{Y(s)}{X(s)} \). type(transfer_function) :: rst !! The resulting [[transfer_function]]. ! Process call rst%Y%initialize(y%get_all()) call rst%X%initialize(x%get_all()) end function ! ------------------------------------------------------------------------------ pure function init_tf_array(y, x) result(rst) !! Initializes a new transfer function. real(real64), intent(in), dimension(:) :: y !! The numerator polynomial \(Y(s)\) in !! \( H(s) = \frac{Y(s)}{X(s)} \). The polynomial coefficients !! are stored in acending order such that !! \( y_1 + y_2 s + y_3 s^2 ... \). real(real64), intent(in), dimension(:) :: x !! The denominator polynomial \(X(s)\) in !! \( H(s) = \frac{Y(s)}{X(s)} \). The polynomial coefficients !! are stored in acending order such that !! \( x_1 + x_2 s + x_3 s^2 ... \). type(transfer_function) :: rst !! The resulting [[transfer_function]]. if (size(y) < 1 .or. size(x) < 1) error stop DYN_INVALID_INPUT_ERROR ! Process call rst%Y%initialize(y) call rst%X%initialize(x) end function ! ------------------------------------------------------------------------------ pure elemental function tf_eval_s(this, s) result(rst) !! Evaluates the transfer function at the specified value of the !! Laplace variable \(s\). class(transfer_function), intent(in) :: this !! The transfer_function object. complex(real64), intent(in) :: s !! The Laplace variable at which to evaluate the transfer function. complex(real64) :: rst !! The value of the transfer function. ! Process if (this%X%evaluate(s) == (0.0d0, 0.0d0)) & error stop DYN_INVALID_INPUT_ERROR rst = this%Y%evaluate(s) / this%X%evaluate(s) end function ! ------------------------------------------------------------------------------ pure elemental function tf_eval_omega(this, omega) result(rst) !! Evaluates the transfer function at the specified value of the !! \(\omega\). class(transfer_function), intent(in) :: this !! The transfer_function object. real(real64), intent(in) :: omega !! The frequency, in rad/s, at which to evaluate the transfer !! function. complex(real64) :: rst !! The value of the transfer function. ! Parameters complex(real64), parameter :: j = (0.0d0, 1.0d0) ! Local Variables complex(real64) :: s ! Process s = j * omega rst = this%tf_eval_s(s) end function ! ------------------------------------------------------------------------------ pure function tf_poles(this) result(rst) !! Computes the poles of the transfer function. class(transfer_function), intent(in) :: this !! The transfer_function object. complex(real64), allocatable, dimension(:) :: rst !! The poles of the transfer function. ! Process rst = this%X%roots() end function ! ------------------------------------------------------------------------------ pure function tf_zeros(this) result(rst) !! Computes the zeros of the transfer function. class(transfer_function), intent(in) :: this !! The transfer function object. complex(real64), allocatable, dimension(:) :: rst !! The zeros of the transfer function. ! Process rst = this%Y%roots() end function ! ------------------------------------------------------------------------------ pure function tf_to_ccf_statespace(this) result(rst) !! Converts a transfer_function type into a controllable canonical form !! state_space type. See !! <a href="https://en.wikipedia.org/wiki/State-space_representation">this</a> !! article !! for a description of this form. class(transfer_function), intent(in) :: this !! The transfer_function to convert. type(state_space) :: rst !! The resulting state-space object. ! Local Variables integer(int32) :: i, order, norder real(real64) :: a ! Process order = this%X%order() norder = this%Y%order() if (norder > order) error stop DYN_INVALID_INPUT_ERROR allocate(rst%A(order, order), rst%B(order, 1), rst%C(1, order), & rst%D(1, 1), source = 0.0d0) a = this%X%get(order + 1) if (a == 0.0d0) error stop DYN_INVALID_INPUT_ERROR if (norder == order) rst%D(1,1) = this%Y%get(order + 1) / a if (order == 0) return do i = 1, order rst%A(order,i) = -this%X%get(i) / a end do do i = 1, order - 1 rst%A(i,i+1) = 1.0d0 end do rst%B(order, 1) = 1.0d0 do i = 1, min(norder + 1, order) rst%C(1,i) = (this%Y%get(i) - rst%D(1,1) * this%X%get(i)) / a end do end function ! ------------------------------------------------------------------------------ pure function tf_to_ocf_statespace(this) result(rst) !! Converts a transfer_function type into an observable canonical form !! state_space type. See !! <a href="https://en.wikipedia.org/wiki/State-space_representation">this</a> !! article !! for a description of this form. class(transfer_function), intent(in) :: this !! The transfer_function to convert. type(state_space) :: rst !! The resulting state-space object. ! Local Variables integer(int32) :: i, order, norder real(real64) :: a ! Process order = this%X%order() norder = this%Y%order() if (norder > order) error stop DYN_INVALID_INPUT_ERROR allocate(rst%A(order, order), rst%B(order, 1), rst%C(1, order), & rst%D(1, 1), source = 0.0d0) a = this%X%get(order + 1) if (a == 0.0d0) error stop DYN_INVALID_INPUT_ERROR if (norder == order) rst%D(1,1) = this%Y%get(order + 1) / a if (order == 0) return do i = 1, order rst%A(i,order) = -this%X%get(i) / a end do do i = 1, order - 1 rst%A(i+1,i) = 1.0d0 end do do i = 1, min(norder + 1, order) rst%B(i,1) = (this%Y%get(i) - rst%D(1,1) * this%X%get(i)) / a end do rst%C(1,order) = 1.0d0 end function ! ****************************************************************************** ! OPERATORS ! ------------------------------------------------------------------------------ pure function tf_tf_mult(x, y) result(rst) !! Multiplies two transfer functions. class(transfer_function), intent(in) :: x !! The left-hand-side argument. class(transfer_function), intent(in) :: y !! The right-hand-side argument. type(transfer_function) :: rst !! The resulting transfer function. ! Process rst%Y = x%Y * y%Y rst%X = x%X * y%X end function ! ------------------------------------------------------------------------------ pure function poly_tf_mult(x, y) result(rst) !! Multiplies a polynomial and a transfer function to result in a new !! transfer function. class(polynomial), intent(in) :: x !! The left-hand-side argument. class(transfer_function), intent(in) :: y !! The right-hand-side argument. type(transfer_function) :: rst !! The resulting transfer function. ! Process rst%Y = x * y%Y rst%X = y%X end function ! ------------------------------------------------------------------------------ pure function tf_poly_mult(x, y) result(rst) !! Multiplies a transfer function and a polynomial to result in a new !! transfer function. class(transfer_function), intent(in) :: x !! The left-hand-side argument. class(polynomial), intent(in) :: y !! The right-hand-side argument. type(transfer_function) :: rst !! The resulting transfer function. ! Process rst%Y = x%Y * y rst%X = x%X end function ! ------------------------------------------------------------------------------ pure function tf_scalar_mult(x, y) result(rst) !! Multiplies a transfer function by a scalar value. class(transfer_function), intent(in) :: x !! The left-hand-side argument. real(real64), intent(in) :: y !! The right-hand-side argument. type(transfer_function) :: rst !! The resulting transfer function. ! Process rst%Y = y * x%Y rst%X = x%X end function ! ------------------------------------------------------------------------------ pure function scalar_tf_mult(x, y) result(rst) !! Multiplies a transfer function by a scalar value. real(real64), intent(in) :: x !! The left-hand-side argument. class(transfer_function), intent(in) :: y !! The right-hand-side argument. type(transfer_function) :: rst !! The resulting transfer function. ! Process rst%Y = x * y%Y rst%X = y%X end function ! ****************************************************************************** ! LTI SOLVERS ! ------------------------------------------------------------------------------ function lti_solve(mdl, u, t, ic, solver, args) result(rst) !! Solves the LTI system given by the specified state space model. class(state_space), intent(in), target :: mdl !! The state_space model to solve. procedure(ss_excitation), pointer, intent(in) :: u !! The routine used to compute the excitation vector. real(real64), intent(in), dimension(:) :: t !! The time points at which to compute the solution. The array must !! have at least 2 values; however, more may be specified. If only !! 2 values are specified, the integrator will compute the solution at !! those points, but it will also return any intermediate integration !! steps that may be required. However, if more than 2 points are !! given, the integrator will return the solution values only at the !! specified time points. real(real64), intent(in), dimension(:) :: ic !! The initial condition vector. This array must be the same size as !! the number of state variables. class(ode_integrator), intent(in), optional, target :: solver !! The ODE solver to utilize. If not specified, the default solver !! is a 4th/5th order Runge-Kutta integrator. class(*), intent(inout), optional :: args !! An optional container for arguments to pass to the excitation !! routine. real(real64), allocatable, dimension(:,:) :: rst !! The solution. The time points at which the solution was evaluated !! are stored in the first column and the output(s) are stored in the !! remaining column(s). ! Local Variables integer(int32) :: i, n, npts, nInputs, nOutputs real(real64), allocatable, dimension(:) :: uv real(real64), allocatable, dimension(:,:) :: sol type(argument_container) :: container class(ode_integrator), pointer :: integrator type(runge_kutta_45), target :: defaultIntegrator type(ode_container) :: odeMdl ! Initialization container%excitation => u container%model => mdl container%has_user_args = present(args) if (present(args)) allocate(container%user_args, source = args) n = size(mdl%A, 1) nInputs = size(mdl%B, 2) nOutputs = size(mdl%C, 1) ! Input Checking if (size(ic) /= n) error stop DYN_ARRAY_SIZE_ERROR ! Set up the integrator if (present(solver)) then integrator => solver else integrator => defaultIntegrator end if odeMdl%fcn => ode_solver_routine ! Call the solver call integrator%solve(odeMdl, t, ic, args = container) ! Get the output from the solver sol = integrator%get_solution() npts = size(sol, 1) allocate(rst(npts, nOutputs + 1), uv(nInputs), source = 0.0d0) ! Compute: C * x + D * u do i = 1, npts call u(sol(i,1), uv, args) rst(i,1) = sol(i,1) rst(i,2:) = mdl%evaluate_output(uv, sol(i,2:)) end do end function ! ---------- subroutine ode_solver_routine(t, x, dxdt, args) ! The routine called by the LTI solver real(real64), intent(in) :: t, x(:) real(real64), intent(out) :: dxdt(:) class(*), intent(inout), optional :: args ! Local Variables integer(int32) :: n, p real(real64) :: nan real(real64), allocatable, dimension(:) :: u ! Initialization nan = ieee_value(nan, IEEE_QUIET_NAN) ! Process select type (args) class is (argument_container) ! Evaluate the excitation vector at t n = size(args%model%B, 2) allocate(u(n), source = 0.0d0) if (args%has_user_args) then call args%excitation(t, u, args = args%user_args) else call args%excitation(t, u) end if ! Evaluate the model dxdt = args%model%evaluate_derivatives(u, x) class default dxdt = nan end select end subroutine ! ------------------------------------------------------------------------------ end module