C
C SPDX-License-Identifier: BSD-3-Clause
C
      SUBROUTINE FD01AD( JP, L, LAMBDA, XIN, YIN, EFOR, XF, EPSBCK,
     $                   CTETA, STETA, YQ, EPOS, EOUT, SALPH, IWARN,
     $                   INFO )
C
C     PURPOSE
C
C     To solve the least-squares filtering problem recursively in time.
C     Each subroutine call implements one time update of the solution.
C     The algorithm uses a fast QR-decomposition based approach.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JP      CHARACTER*1
C             Indicates whether the user wishes to apply both prediction
C             and filtering parts, as follows:
C             = 'B':  Both prediction and filtering parts are to be
C                     applied;
C             = 'P':  Only the prediction section is to be applied.
C
C     Input/Output Parameters
C
C     L       (input) INTEGER
C             The length of the impulse response of the equivalent
C             transversal filter model.  L >= 1.
C
C     LAMBDA  (input) DOUBLE PRECISION
C             Square root of the forgetting factor.
C             For tracking capabilities and exponentially stable error
C             propagation, LAMBDA < 1.0 (strict inequality) should
C             be used.  0.0 < LAMBDA <= 1.0.
C
C     XIN     (input) DOUBLE PRECISION
C             The input sample at instant n.
C             (The situation just before and just after the call of
C             the routine are denoted by instant (n-1) and instant n,
C             respectively.)
C
C     YIN     (input) DOUBLE PRECISION
C             If JP = 'B', then YIN must contain the reference sample
C             at instant n.
C             Otherwise, YIN is not referenced.
C
C     EFOR    (input/output) DOUBLE PRECISION
C             On entry, this parameter must contain the square root of
C             exponentially weighted forward prediction error energy
C             at instant (n-1).  EFOR >= 0.0.
C             On exit, this parameter contains the square root of the
C             exponentially weighted forward prediction error energy
C             at instant n.
C
C     XF      (input/output) DOUBLE PRECISION array, dimension (L)
C             On entry, this array must contain the transformed forward
C             prediction variables at instant (n-1).
C             On exit, this array contains the transformed forward
C             prediction variables at instant n.
C
C     EPSBCK  (input/output) DOUBLE PRECISION array, dimension (L+1)
C             On entry, the leading L elements of this array must
C             contain the normalized a posteriori backward prediction
C             error residuals of orders zero through L-1, respectively,
C             at instant (n-1), and EPSBCK(L+1) must contain the
C             square-root of the so-called "conversion factor" at
C             instant (n-1).
C             On exit, this array contains the normalized a posteriori
C             backward prediction error residuals, plus the square root
C             of the conversion factor at instant n.
C
C     CTETA   (input/output) DOUBLE PRECISION array, dimension (L)
C             On entry, this array must contain the cosines of the
C             rotation angles used in time updates, at instant (n-1).
C             On exit, this array contains the cosines of the rotation
C             angles at instant n.
C
C     STETA   (input/output) DOUBLE PRECISION array, dimension (L)
C             On entry, this array must contain the sines of the
C             rotation angles used in time updates, at instant (n-1).
C             On exit, this array contains the sines of the rotation
C             angles at instant n.
C
C     YQ      (input/output) DOUBLE PRECISION array, dimension (L)
C             On entry, if JP = 'B', then this array must contain the
C             orthogonally transformed reference vector at instant
C             (n-1). These elements are also the tap multipliers of an
C             equivalent normalized lattice least-squares filter.
C             Otherwise, YQ is not referenced and can be supplied as
C             a dummy array (i.e., declare this array to be YQ(1) in
C             the calling program).
C             On exit, if JP = 'B', then this array contains the
C             orthogonally transformed reference vector at instant n.
C
C     EPOS    (output) DOUBLE PRECISION
C             The a posteriori forward prediction error residual.
C
C     EOUT    (output) DOUBLE PRECISION
C             If JP = 'B', then EOUT contains the a posteriori output
C             error residual from the least-squares filter at instant n.
C
C     SALPH   (output) DOUBLE PRECISION array, dimension (L)
C             The element SALPH(i), i=1,...,L, contains the opposite of
C             the i-(th) reflection coefficient for the least-squares
C             normalized lattice predictor (whose value is -SALPH(i)).
C
C     Warning Indicator
C
C     IWARN   INTEGER
C             = 0:  no warning;
C             = 1:  an element to be annihilated by a rotation is less
C                   than the machine precision (see LAPACK Library
C                   routine DLAMCH).
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     The output error EOUT at instant n, denoted by EOUT(n), is the
C     reference sample minus a linear combination of L successive input
C     samples:
C
C                           L-1
C        EOUT(n) = YIN(n) - SUM h_i * XIN(n-i),
C                           i=0
C
C     where YIN(n) and XIN(n) are the scalar samples at instant n.
C     A least-squares filter uses those h_0,...,h_{L-1} which minimize
C     an exponentially weighted sum of successive output errors squared:
C
C         n
C        SUM [LAMBDA**(2(n-k)) * EOUT(k)**2].
C        k=1
C
C     Each subroutine call performs a time update of the least-squares
C     filter using a fast least-squares algorithm derived from a
C     QR decomposition, as described in references [1] and [2] (the
C     notation from [2] is followed in the naming of the arrays).
C     The algorithm does not compute the parameters h_0,...,h_{L-1} from
C     the above formula, but instead furnishes the parameters of an
C     equivalent normalized least-squares lattice filter, which are
C     available from the arrays SALPH (reflection coefficients) and YQ
C     (tap multipliers), as well as the exponentially weighted input
C     signal energy
C
C         n                                              L
C        SUM [LAMBDA**(2(n-k)) * XIN(k)**2] = EFOR**2 + SUM XF(i)**2.
C        k=1                                            i=1
C
C     For more details on reflection coefficients and tap multipliers,
C     references [2] and [4] are recommended.
C
C     REFERENCES
C
C     [1]  Proudler, I. K., McWhirter, J. G., and Shepherd, T. J.
C          Fast QRD based algorithms for least-squares linear
C          prediction.
C          Proceedings IMA Conf. Mathematics in Signal Processing
C          Warwick, UK, December 1988.
C
C     [2]  Regalia, P. A., and Bellanger, M. G.
C          On the duality between QR methods and lattice methods in
C          least-squares adaptive filtering.
C          IEEE Trans. Signal Processing, SP-39, pp. 879-891,
C          April 1991.
C
C     [3]  Regalia, P. A.
C          Numerical stability properties of a QR-based fast
C          least-squares algorithm.
C          IEEE Trans. Signal Processing, SP-41, June 1993.
C
C     [4]  Lev-Ari, H., Kailath, T., and Cioffi, J.
C          Least-squares adaptive lattice and transversal filters:
C          A unified geometric theory.
C          IEEE Trans. Information Theory, IT-30, pp. 222-236,
C          March 1984.
C
C     NUMERICAL ASPECTS
C
C     The algorithm requires O(L) operations for each subroutine call.
C     It is backward consistent for all input sequences XIN, and
C     backward stable for persistently exciting input sequences,
C     assuming LAMBDA < 1.0 (see [3]).
C     If the condition of the signal is very poor (IWARN = 1), then the
C     results are not guaranteed to be reliable.
C
C     FURTHER COMMENTS
C
C     1.  For tracking capabilities and exponentially stable error
C         propagation, LAMBDA < 1.0 should be used.  LAMBDA is typically
C         chosen slightly less than 1.0 so that "past" data are
C         exponentially forgotten.
C     2.  Prior to the first subroutine call, the variables must be
C         initialized. The following initial values are recommended:
C
C         XF(i) = 0.0,        i=1,...,L
C         EPSBCK(i) = 0.0     i=1,...,L
C         EPSBCK(L+1) = 1.0
C         CTETA(i) = 1.0      i=1,...,L
C         STETA(i) = 0.0      i=1,...,L
C         YQ(i) = 0.0         i=1,...,L
C
C         EFOR = 0.0          (exact start)
C         EFOR = "small positive constant" (soft start).
C
C         Soft starts are numerically more reliable, but result in a
C         biased least-squares solution during the first few iterations.
C         This bias decays exponentially fast provided LAMBDA < 1.0.
C         If sigma is the standard deviation of the input sequence
C         XIN, then initializing EFOR = sigma*1.0E-02 usually works
C         well.
C
C     CONTRIBUTOR
C
C     P. A. Regalia (October 1994).
C     Release 4.0: V. Sima, Katholieke Univ. Leuven, Belgium, Dec. 1999.
C
C     REVISIONS
C
C     -
C
C     KEYWORDS
C
C     Kalman filtering, least-squares estimator, optimal filtering,
C     orthogonal transformation, recursive estimation, QR decomposition.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION  ONE, ZERO
      PARAMETER         ( ONE = 1.0D0, ZERO = 0.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         JP
      INTEGER           INFO, IWARN, L
      DOUBLE PRECISION  EFOR, EOUT, EPOS, LAMBDA, XIN, YIN
C     .. Array Arguments ..
      DOUBLE PRECISION  CTETA(*), EPSBCK(*), SALPH(*), STETA(*), XF(*),
     $                  YQ(*)
C     .. Local Scalars ..
      LOGICAL           BOTH
      INTEGER           I
      DOUBLE PRECISION  CTEMP, EPS, FNODE, NORM, TEMP, XFI, YQI
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DLAMCH, DLAPY2, DNRM2
      EXTERNAL          DLAMCH, DLAPY2, DNRM2, LSAME
C     .. External Subroutines ..
      EXTERNAL          DLARTG, XERBLA
C     .. Intrinsic Functions
      INTRINSIC         ABS, SQRT
C     .. Executable statements ..
C
C     Test the input scalar arguments.
C
      BOTH  = LSAME( JP, 'B' )
      IWARN = 0
      INFO  = 0
C
      IF( .NOT.BOTH .AND. .NOT.LSAME( JP, 'P' ) ) THEN
         INFO = -1
      ELSE IF( L.LT.1 ) THEN
         INFO = -2
      ELSE IF( ( LAMBDA.LE.ZERO ) .OR. ( LAMBDA.GT.ONE  ) ) THEN
         INFO = -3
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'FD01AD', -INFO )
         RETURN
      END IF
C
C     Computation of the machine precision EPS.
C
      EPS = DLAMCH( 'Epsilon' )
C
C     Forward prediction rotations.
C
      FNODE = XIN
C
      DO 10  I = 1, L
         XFI   = XF(I) * LAMBDA
         XF(I) = STETA(I) * FNODE + CTETA(I) * XFI
         FNODE = CTETA(I) * FNODE - STETA(I) * XFI
   10 CONTINUE
C
      EPOS = FNODE * EPSBCK(L+1)
C
C     Update the square root of the prediction energy.
C
      EFOR = EFOR * LAMBDA
      TEMP = DLAPY2( FNODE, EFOR )
      IF ( TEMP.LT.EPS ) THEN
         FNODE = ZERO
         IWARN = 1
      ELSE
         FNODE = FNODE * EPSBCK(L+1)/TEMP
      END IF
      EFOR = TEMP
C
C     Calculate the reflection coefficients and the backward prediction
C     errors.
C
      DO 20 I = L, 1, -1
         IF ( ABS( XF(I) ).LT.EPS )
     $      IWARN = 1
         CALL DLARTG( TEMP, XF(I), CTEMP, SALPH(I), NORM )
         EPSBCK(I+1) = CTEMP * EPSBCK(I) - SALPH(I) * FNODE
         FNODE = CTEMP * FNODE + SALPH(I) * EPSBCK(I)
         TEMP  = NORM
   20 CONTINUE
C
      EPSBCK(1) = FNODE
C
C     Update to new rotation angles.
C
      NORM = DNRM2( L, EPSBCK, 1 )
      TEMP = SQRT( ( ONE + NORM )*( ONE - NORM ) )
      EPSBCK(L+1) = TEMP
C
      DO 30 I = L, 1, -1
         IF ( ABS( EPSBCK(I) ).LT.EPS )
     $      IWARN = 1
         CALL DLARTG( TEMP, EPSBCK(I), CTETA(I), STETA(I), NORM )
         TEMP = NORM
   30 CONTINUE
C
C     Joint process section.
C
      IF ( BOTH) THEN
         FNODE = YIN
C
         DO 40  I = 1, L
            YQI   = YQ(I) * LAMBDA
            YQ(I) = STETA(I) * FNODE + CTETA(I) * YQI
            FNODE = CTETA(I) * FNODE - STETA(I) * YQI
   40    CONTINUE
C
         EOUT = FNODE * EPSBCK(L+1)
      END IF
C
      RETURN
C *** Last line of FD01AD ***
      END
