Menu
▾
▴
[Octave-cvsupdate] SF.net SVN: octave:[8789] trunk/octave-forge/extra/control-devel
|
From: <par...@us...> - 2011-10-19 16:38:42
|
Revision: 8789
http://octave.svn.sourceforge.net/octave/?rev=8789&view=rev
Author: paramaniac
Date: 2011-10-19 16:38:34 +0000 (Wed, 19 Oct 2011)
Log Message:
-----------
control-devel: add code for fitfrd
Modified Paths:
--------------
trunk/octave-forge/extra/control-devel/devel/makefile_ident.m
Added Paths:
-----------
trunk/octave-forge/extra/control-devel/src/DG01MD.f
trunk/octave-forge/extra/control-devel/src/MC01PD.f
trunk/octave-forge/extra/control-devel/src/SB10YD.f
trunk/octave-forge/extra/control-devel/src/SB10ZP.f
trunk/octave-forge/extra/control-devel/src/TD03AY.f
trunk/octave-forge/extra/control-devel/src/TD04AD.f
Modified: trunk/octave-forge/extra/control-devel/devel/makefile_ident.m
===================================================================
--- trunk/octave-forge/extra/control-devel/devel/makefile_ident.m 2011-10-19 15:05:10 UTC (rev 8788)
+++ trunk/octave-forge/extra/control-devel/devel/makefile_ident.m 2011-10-19 16:38:34 UTC (rev 8789)
@@ -23,6 +23,12 @@
MB01TD.f MA02AD.f MB04OD.f MB04OY.f MB02UD.f \
MB03UD.f MB01SD.f
+## fit state-space model to frequency response data
+mkoctfile SB10YD.f DG01MD.f AB04MD.f SB10ZP.f AB07ND.f \
+ MC01PD.f TD04AD.f TD03AY.f TB01PD.f TB01XD.f \
+ AB07MD.f TB01UD.f TB01ID.f MB01PD.f MB03OY.f \
+ MB01QD.f
+
system ("rm *.o");
cd (homedir);
Added: trunk/octave-forge/extra/control-devel/src/DG01MD.f
===================================================================
--- trunk/octave-forge/extra/control-devel/src/DG01MD.f (rev 0)
+++ trunk/octave-forge/extra/control-devel/src/DG01MD.f 2011-10-19 16:38:34 UTC (rev 8789)
@@ -0,0 +1,235 @@
+ SUBROUTINE DG01MD( INDI, N, XR, XI, INFO )
+C
+C SLICOT RELEASE 5.0.
+C
+C Copyright (c) 2002-2010 NICONET e.V.
+C
+C This program is free software: you can redistribute it and/or
+C modify it under the terms of the GNU General Public License as
+C published by the Free Software Foundation, either version 2 of
+C the License, or (at your option) any later version.
+C
+C This program is distributed in the hope that it will be useful,
+C but WITHOUT ANY WARRANTY; without even the implied warranty of
+C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+C GNU General Public License for more details.
+C
+C You should have received a copy of the GNU General Public License
+C along with this program. If not, see
+C <http://www.gnu.org/licenses/>.
+C
+C PURPOSE
+C
+C To compute the discrete Fourier transform, or inverse transform,
+C of a complex signal.
+C
+C ARGUMENTS
+C
+C Mode Parameters
+C
+C INDI CHARACTER*1
+C Indicates whether a Fourier transform or inverse Fourier
+C transform is to be performed as follows:
+C = 'D': (Direct) Fourier transform;
+C = 'I': Inverse Fourier transform.
+C
+C Input/Output Parameters
+C
+C N (input) INTEGER
+C The number of complex samples. N must be a power of 2.
+C N >= 2.
+C
+C XR (input/output) DOUBLE PRECISION array, dimension (N)
+C On entry, this array must contain the real part of either
+C the complex signal z if INDI = 'D', or f(z) if INDI = 'I'.
+C On exit, this array contains either the real part of the
+C computed Fourier transform f(z) if INDI = 'D', or the
+C inverse Fourier transform z of f(z) if INDI = 'I'.
+C
+C XI (input/output) DOUBLE PRECISION array, dimension (N)
+C On entry, this array must contain the imaginary part of
+C either z if INDI = 'D', or f(z) if INDI = 'I'.
+C On exit, this array contains either the imaginary part of
+C f(z) if INDI = 'D', or z if INDI = 'I'.
+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 If INDI = 'D', then the routine performs a discrete Fourier
+C transform on the complex signal Z(i), i = 1,2,...,N. If the result
+C is denoted by FZ(k), k = 1,2,...,N, then the relationship between
+C Z and FZ is given by the formula:
+C
+C N ((k-1)*(i-1))
+C FZ(k) = SUM ( Z(i) * V ),
+C i=1
+C 2
+C where V = exp( -2*pi*j/N ) and j = -1.
+C
+C If INDI = 'I', then the routine performs an inverse discrete
+C Fourier transform on the complex signal FZ(k), k = 1,2,...,N. If
+C the result is denoted by Z(i), i = 1,2,...,N, then the
+C relationship between Z and FZ is given by the formula:
+C
+C N ((k-1)*(i-1))
+C Z(i) = SUM ( FZ(k) * W ),
+C k=1
+C
+C where W = exp( 2*pi*j/N ).
+C
+C Note that a discrete Fourier transform, followed by an inverse
+C discrete Fourier transform, will result in a signal which is a
+C factor N larger than the original input signal.
+C
+C REFERENCES
+C
+C [1] Rabiner, L.R. and Rader, C.M.
+C Digital Signal Processing.
+C IEEE Press, 1972.
+C
+C NUMERICAL ASPECTS
+C
+C The algorithm requires 0( N*log(N) ) operations.
+C
+C CONTRIBUTOR
+C
+C Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Feb. 1997.
+C Supersedes Release 2.0 routine DG01AD by R. Dekeyser, State
+C University of Gent, Belgium.
+C
+C REVISIONS
+C
+C -
+C
+C KEYWORDS
+C
+C Complex signals, digital signal processing, fast Fourier
+C transform.
+C
+C ******************************************************************
+C
+C .. Parameters ..
+ DOUBLE PRECISION ZERO, HALF, ONE, TWO, EIGHT
+ PARAMETER ( ZERO = 0.0D0, HALF = 0.5D0, ONE = 1.0D0,
+ $ TWO = 2.0D0, EIGHT = 8.0D0 )
+C .. Scalar Arguments ..
+ CHARACTER INDI
+ INTEGER INFO, N
+C .. Array Arguments ..
+ DOUBLE PRECISION XI(*), XR(*)
+C .. Local Scalars ..
+ LOGICAL LINDI
+ INTEGER I, J, K, L, M
+ DOUBLE PRECISION PI2, TI, TR, WHELP, WI, WR, WSTPI, WSTPR
+C .. External Functions ..
+ LOGICAL LSAME
+ EXTERNAL LSAME
+C .. External Subroutines ..
+ EXTERNAL XERBLA
+C .. Intrinsic Functions ..
+ INTRINSIC ATAN, DBLE, MOD, SIN
+C .. Executable Statements ..
+C
+ INFO = 0
+ LINDI = LSAME( INDI, 'D' )
+C
+C Test the input scalar arguments.
+C
+ IF( .NOT.LINDI .AND. .NOT.LSAME( INDI, 'I' ) ) THEN
+ INFO = -1
+ ELSE
+ J = 0
+ IF( N.GE.2 ) THEN
+ J = N
+C WHILE ( MOD( J, 2 ).EQ.0 ) DO
+ 10 CONTINUE
+ IF ( MOD( J, 2 ).EQ.0 ) THEN
+ J = J/2
+ GO TO 10
+ END IF
+C END WHILE 10
+ END IF
+ IF ( J.NE.1 ) INFO = -2
+ END IF
+C
+ IF ( INFO.NE.0 ) THEN
+C
+C Error return.
+C
+ CALL XERBLA( 'DG01MD', -INFO )
+ RETURN
+ END IF
+C
+C Inplace shuffling of data.
+C
+ J = 1
+C
+ DO 30 I = 1, N
+ IF ( J.GT.I ) THEN
+ TR = XR(I)
+ TI = XI(I)
+ XR(I) = XR(J)
+ XI(I) = XI(J)
+ XR(J) = TR
+ XI(J) = TI
+ END IF
+ K = N/2
+C REPEAT
+ 20 IF ( J.GT.K ) THEN
+ J = J - K
+ K = K/2
+ IF ( K.GE.2 ) GO TO 20
+ END IF
+C UNTIL ( K.LT.2 )
+ J = J + K
+ 30 CONTINUE
+C
+C Transform by decimation in time.
+C
+ PI2 = EIGHT*ATAN( ONE )
+ IF ( LINDI ) PI2 = -PI2
+C
+ I = 1
+C
+C WHILE ( I.LT.N ) DO
+C
+ 40 IF ( I.LT.N ) THEN
+ L = 2*I
+ WHELP = PI2/DBLE( L )
+ WSTPI = SIN( WHELP )
+ WHELP = SIN( HALF*WHELP )
+ WSTPR = -TWO*WHELP*WHELP
+ WR = ONE
+ WI = ZERO
+C
+ DO 60 J = 1, I
+C
+ DO 50 K = J, N, L
+ M = K + I
+ TR = WR*XR(M) - WI*XI(M)
+ TI = WR*XI(M) + WI*XR(M)
+ XR(M) = XR(K) - TR
+ XI(M) = XI(K) - TI
+ XR(K) = XR(K) + TR
+ XI(K) = XI(K) + TI
+ 50 CONTINUE
+C
+ WHELP = WR
+ WR = WR + WR*WSTPR - WI*WSTPI
+ WI = WI + WHELP*WSTPI + WI*WSTPR
+ 60 CONTINUE
+C
+ I = L
+ GO TO 40
+C END WHILE 40
+ END IF
+C
+ RETURN
+C *** Last line of DG01MD ***
+ END
Property changes on: trunk/octave-forge/extra/control-devel/src/DG01MD.f
___________________________________________________________________
Added: svn:executable
+ *
Added: trunk/octave-forge/extra/control-devel/src/MC01PD.f
===================================================================
--- trunk/octave-forge/extra/control-devel/src/MC01PD.f (rev 0)
+++ trunk/octave-forge/extra/control-devel/src/MC01PD.f 2011-10-19 16:38:34 UTC (rev 8789)
@@ -0,0 +1,159 @@
+ SUBROUTINE MC01PD( K, REZ, IMZ, P, DWORK, INFO )
+C
+C SLICOT RELEASE 5.0.
+C
+C Copyright (c) 2002-2010 NICONET e.V.
+C
+C This program is free software: you can redistribute it and/or
+C modify it under the terms of the GNU General Public License as
+C published by the Free Software Foundation, either version 2 of
+C the License, or (at your option) any later version.
+C
+C This program is distributed in the hope that it will be useful,
+C but WITHOUT ANY WARRANTY; without even the implied warranty of
+C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+C GNU General Public License for more details.
+C
+C You should have received a copy of the GNU General Public License
+C along with this program. If not, see
+C <http://www.gnu.org/licenses/>.
+C
+C PURPOSE
+C
+C To compute the coefficients of a real polynomial P(x) from its
+C zeros.
+C
+C ARGUMENTS
+C
+C Input/Output Parameters
+C
+C K (input) INTEGER
+C The number of zeros (and hence the degree) of P(x).
+C K >= 0.
+C
+C REZ (input) DOUBLE PRECISION array, dimension (K)
+C IMZ (input) DOUBLE PRECISION array, dimension (K)
+C The real and imaginary parts of the i-th zero of P(x)
+C must be stored in REZ(i) and IMZ(i), respectively, where
+C i = 1, 2, ..., K. The zeros may be supplied in any order,
+C except that complex conjugate zeros must appear
+C consecutively.
+C
+C P (output) DOUBLE PRECISION array, dimension (K+1)
+C This array contains the coefficients of P(x) in increasing
+C powers of x. If K = 0, then P(1) is set to one.
+C
+C Workspace
+C
+C DWORK DOUBLE PRECISION array, dimension (K+1)
+C If K = 0, this array is not referenced.
+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 > 0: if INFO = i, (REZ(i),IMZ(i)) is a complex zero but
+C (REZ(i-1),IMZ(i-1)) is not its conjugate.
+C
+C METHOD
+C
+C The routine computes the coefficients of the real K-th degree
+C polynomial P(x) as
+C
+C P(x) = (x - r(1)) * (x - r(2)) * ... * (x - r(K))
+C
+C where r(i) = (REZ(i),IMZ(i)).
+C
+C Note that REZ(i) = REZ(j) and IMZ(i) = -IMZ(j) if r(i) and r(j)
+C form a complex conjugate pair (where i <> j), and that IMZ(i) = 0
+C if r(i) is real.
+C
+C NUMERICAL ASPECTS
+C
+C None.
+C
+C CONTRIBUTOR
+C
+C Release 3.0: V. Sima, Katholieke Univ. Leuven, Belgium, Mar. 1997.
+C Supersedes Release 2.0 routine MC01DD by A.J. Geurts.
+C
+C REVISIONS
+C
+C V. Sima, May 2002.
+C
+C KEYWORDS
+C
+C Elementary polynomial operations, polynomial operations.
+C
+C ******************************************************************
+C
+C .. Parameters ..
+ DOUBLE PRECISION ZERO, ONE
+ PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 )
+C .. Scalar Arguments ..
+ INTEGER INFO, K
+C .. Array Arguments ..
+ DOUBLE PRECISION DWORK(*), IMZ(*), P(*), REZ(*)
+C .. Local Scalars ..
+ INTEGER I
+ DOUBLE PRECISION U, V
+C .. External Subroutines ..
+ EXTERNAL DAXPY, DCOPY, XERBLA
+C .. Executable Statements ..
+C
+C Test the input scalar arguments.
+C
+ IF( K.LT.0 ) THEN
+ INFO = -1
+C
+C Error return.
+C
+ CALL XERBLA( 'MC01PD', -INFO )
+ RETURN
+ END IF
+C
+C Quick return if possible.
+C
+ INFO = 0
+ P(1) = ONE
+ IF ( K.EQ.0 )
+ $ RETURN
+C
+ I = 1
+C WHILE ( I <= K ) DO
+ 20 IF ( I.LE.K ) THEN
+ U = REZ(I)
+ V = IMZ(I)
+ DWORK(1) = ZERO
+C
+ IF ( V.EQ.ZERO ) THEN
+ CALL DCOPY( I, P, 1, DWORK(2), 1 )
+ CALL DAXPY( I, -U, P, 1, DWORK, 1 )
+ I = I + 1
+C
+ ELSE
+ IF ( I.EQ.K ) THEN
+ INFO = K
+ RETURN
+ ELSE IF ( ( U.NE.REZ(I+1) ) .OR. ( V.NE.-IMZ(I+1) ) ) THEN
+ INFO = I + 1
+ RETURN
+ END IF
+C
+ DWORK(2) = ZERO
+ CALL DCOPY( I, P, 1, DWORK(3), 1 )
+ CALL DAXPY( I, -(U + U), P, 1, DWORK(2), 1 )
+ CALL DAXPY( I, U**2+V**2, P, 1, DWORK, 1 )
+ I = I + 2
+ END IF
+C
+ CALL DCOPY( I, DWORK, 1, P, 1 )
+ GO TO 20
+ END IF
+C END WHILE 20
+C
+ RETURN
+C *** Last line of MC01PD ***
+ END
Property changes on: trunk/octave-forge/extra/control-devel/src/MC01PD.f
___________________________________________________________________
Added: svn:executable
+ *
Added: trunk/octave-forge/extra/control-devel/src/SB10YD.f
===================================================================
--- trunk/octave-forge/extra/control-devel/src/SB10YD.f (rev 0)
+++ trunk/octave-forge/extra/control-devel/src/SB10YD.f 2011-10-19 16:38:34 UTC (rev 8789)
@@ -0,0 +1,689 @@
+ SUBROUTINE SB10YD( DISCFL, FLAG, LENDAT, RFRDAT, IFRDAT, OMEGA, N,
+ $ A, LDA, B, C, D, TOL, IWORK, DWORK, LDWORK,
+ $ ZWORK, LZWORK, INFO )
+C
+C SLICOT RELEASE 5.0.
+C
+C Copyright (c) 2002-2010 NICONET e.V.
+C
+C This program is free software: you can redistribute it and/or
+C modify it under the terms of the GNU General Public License as
+C published by the Free Software Foundation, either version 2 of
+C the License, or (at your option) any later version.
+C
+C This program is distributed in the hope that it will be useful,
+C but WITHOUT ANY WARRANTY; without even the implied warranty of
+C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+C GNU General Public License for more details.
+C
+C You should have received a copy of the GNU General Public License
+C along with this program. If not, see
+C <http://www.gnu.org/licenses/>.
+C
+C PURPOSE
+C
+C To fit a supplied frequency response data with a stable, minimum
+C phase SISO (single-input single-output) system represented by its
+C matrices A, B, C, D. It handles both discrete- and continuous-time
+C cases.
+C
+C ARGUMENTS
+C
+C Input/Output parameters
+C
+C DISCFL (input) INTEGER
+C Indicates the type of the system, as follows:
+C = 0: continuous-time system;
+C = 1: discrete-time system.
+C
+C FLAG (input) INTEGER
+C If FLAG = 0, then the system zeros and poles are not
+C constrained.
+C If FLAG = 1, then the system zeros and poles will have
+C negative real parts in the continuous-time case, or moduli
+C less than 1 in the discrete-time case. Consequently, FLAG
+C must be equal to 1 in mu-synthesis routines.
+C
+C LENDAT (input) INTEGER
+C The length of the vectors RFRDAT, IFRDAT and OMEGA.
+C LENDAT >= 2.
+C
+C RFRDAT (input) DOUBLE PRECISION array, dimension (LENDAT)
+C The real part of the frequency data to be fitted.
+C
+C IFRDAT (input) DOUBLE PRECISION array, dimension (LENDAT)
+C The imaginary part of the frequency data to be fitted.
+C
+C OMEGA (input) DOUBLE PRECISION array, dimension (LENDAT)
+C The frequencies corresponding to RFRDAT and IFRDAT.
+C These values must be nonnegative and monotonically
+C increasing. Additionally, for discrete-time systems
+C they must be between 0 and PI.
+C
+C N (input/output) INTEGER
+C On entry, the desired order of the system to be fitted.
+C N <= LENDAT-1.
+C On exit, the order of the obtained system. The value of N
+C could only be modified if N > 0 and FLAG = 1.
+C
+C A (output) DOUBLE PRECISION array, dimension (LDA,N)
+C The leading N-by-N part of this array contains the
+C matrix A. If FLAG = 1, then A is in an upper Hessenberg
+C form, and corresponds to a minimal realization.
+C
+C LDA INTEGER
+C The leading dimension of the array A. LDA >= MAX(1,N).
+C
+C B (output) DOUBLE PRECISION array, dimension (N)
+C The computed vector B.
+C
+C C (output) DOUBLE PRECISION array, dimension (N)
+C The computed vector C. If FLAG = 1, the first N-1 elements
+C are zero (for the exit value of N).
+C
+C D (output) DOUBLE PRECISION array, dimension (1)
+C The computed scalar D.
+C
+C Tolerances
+C
+C TOL DOUBLE PRECISION
+C The tolerance to be used for determining the effective
+C rank of matrices. If the user sets TOL > 0, then the given
+C value of TOL is used as a lower bound for the reciprocal
+C condition number; a (sub)matrix whose estimated condition
+C number is less than 1/TOL is considered to be of full
+C rank. If the user sets TOL <= 0, then an implicitly
+C computed, default tolerance, defined by TOLDEF = SIZE*EPS,
+C is used instead, where SIZE is the product of the matrix
+C dimensions, and EPS is the machine precision (see LAPACK
+C Library routine DLAMCH).
+C
+C Workspace
+C
+C IWORK INTEGER array, dimension max(2,2*N+1)
+C
+C DWORK DOUBLE PRECISION array, dimension (LDWORK)
+C On exit, if INFO = 0, DWORK(1) returns the optimal value
+C of LDWORK and DWORK(2) contains the optimal value of
+C LZWORK.
+C
+C LDWORK INTEGER
+C The length of the array DWORK.
+C LDWORK = max( 2, LW1, LW2, LW3, LW4 ), where
+C LW1 = 2*LENDAT + 4*HNPTS; HNPTS = 2048;
+C LW2 = LENDAT + 6*HNPTS;
+C MN = min( 2*LENDAT, 2*N+1 )
+C LW3 = 2*LENDAT*(2*N+1) + max( 2*LENDAT, 2*N+1 ) +
+C max( MN + 6*N + 4, 2*MN + 1 ), if N > 0;
+C LW3 = 4*LENDAT + 5 , if N = 0;
+C LW4 = max( N*N + 5*N, 6*N + 1 + min( 1,N ) ), if FLAG = 1;
+C LW4 = 0, if FLAG = 0.
+C For optimum performance LDWORK should be larger.
+C
+C ZWORK COMPLEX*16 array, dimension (LZWORK)
+C
+C LZWORK INTEGER
+C The length of the array ZWORK.
+C LZWORK = LENDAT*(2*N+3), if N > 0;
+C LZWORK = LENDAT, if N = 0.
+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 = 1: if the discrete --> continuous transformation cannot
+C be made;
+C = 2: if the system poles cannot be found;
+C = 3: if the inverse system cannot be found, i.e., D is
+C (close to) zero;
+C = 4: if the system zeros cannot be found;
+C = 5: if the state-space representation of the new
+C transfer function T(s) cannot be found;
+C = 6: if the continuous --> discrete transformation cannot
+C be made.
+C
+C METHOD
+C
+C First, if the given frequency data are corresponding to a
+C continuous-time system, they are changed to a discrete-time
+C system using a bilinear transformation with a scaled alpha.
+C Then, the magnitude is obtained from the supplied data.
+C Then, the frequency data are linearly interpolated around
+C the unit-disc.
+C Then, Oppenheim and Schafer complex cepstrum method is applied
+C to get frequency data corresponding to a stable, minimum-
+C phase system. This is done in the following steps:
+C - Obtain LOG (magnitude)
+C - Obtain IFFT of the result (DG01MD SLICOT subroutine);
+C - halve the data at 0;
+C - Obtain FFT of the halved data (DG01MD SLICOT subroutine);
+C - Obtain EXP of the result.
+C Then, the new frequency data are interpolated back to the
+C original frequency.
+C Then, based on these newly obtained data, the system matrices
+C A, B, C, D are constructed; the very identification is
+C performed by Least Squares Method using DGELSY LAPACK subroutine.
+C If needed, a discrete-to-continuous time transformation is
+C applied on the system matrices by AB04MD SLICOT subroutine.
+C Finally, if requested, the poles and zeros of the system are
+C checked. If some of them have positive real parts in the
+C continuous-time case (or are not inside the unit disk in the
+C complex plane in the discrete-time case), they are exchanged with
+C their negatives (or reciprocals, respectively), to preserve the
+C frequency response, while getting a minimum phase and stable
+C system. This is done by SB10ZP SLICOT subroutine.
+C
+C REFERENCES
+C
+C [1] Oppenheim, A.V. and Schafer, R.W.
+C Discrete-Time Signal Processing.
+C Prentice-Hall Signal Processing Series, 1989.
+C
+C [2] Balas, G., Doyle, J., Glover, K., Packard, A., and Smith, R.
+C Mu-analysis and Synthesis toolbox - User's Guide,
+C The Mathworks Inc., Natick, MA, USA, 1998.
+C
+C CONTRIBUTORS
+C
+C Asparuh Markovski, Technical University of Sofia, July 2003.
+C
+C REVISIONS
+C
+C V. Sima, Research Institute for Informatics, Bucharest, Aug. 2003.
+C A. Markovski, Technical University of Sofia, October 2003.
+C
+C KEYWORDS
+C
+C Bilinear transformation, frequency response, least-squares
+C approximation, stability.
+C
+C ******************************************************************
+C
+C .. Parameters ..
+ COMPLEX*16 ZZERO, ZONE
+ PARAMETER ( ZZERO = ( 0.0D+0, 0.0D+0 ),
+ $ ZONE = ( 1.0D+0, 0.0D+0 ) )
+ DOUBLE PRECISION ZERO, ONE, TWO, FOUR, TEN
+ PARAMETER ( ZERO = 0.0D+0, ONE = 1.0D+0, TWO = 2.0D+0,
+ $ FOUR = 4.0D+0, TEN = 1.0D+1 )
+ INTEGER HNPTS
+ PARAMETER ( HNPTS = 2048 )
+C ..
+C .. Scalar Arguments ..
+ INTEGER DISCFL, FLAG, INFO, LDA, LDWORK, LENDAT,
+ $ LZWORK, N
+ DOUBLE PRECISION TOL
+C ..
+C .. Array Arguments ..
+ INTEGER IWORK(*)
+ DOUBLE PRECISION A(LDA, *), B(*), C(*), D(*), DWORK(*),
+ $ IFRDAT(*), OMEGA(*), RFRDAT(*)
+ COMPLEX*16 ZWORK(*)
+C ..
+C .. Local Scalars ..
+ INTEGER CLWMAX, DLWMAX, I, II, INFO2, IP1, IP2, ISTART,
+ $ ISTOP, IWA0, IWAB, IWBMAT, IWBP, IWBX, IWDME,
+ $ IWDOMO, IWMAG, IWS, IWVAR, IWXI, IWXR, IWYMAG,
+ $ K, LW1, LW2, LW3, LW4, MN, N1, N2, P, RANK
+ DOUBLE PRECISION P1, P2, PI, PW, RAT, TOLB, TOLL
+ COMPLEX*16 XHAT(HNPTS/2)
+C ..
+C .. External Functions ..
+ DOUBLE PRECISION DLAMCH, DLAPY2
+ EXTERNAL DLAMCH, DLAPY2
+C ..
+C .. External Subroutines ..
+ EXTERNAL AB04MD, DCOPY, DG01MD, DGELSY, DLASET, DSCAL,
+ $ SB10ZP, XERBLA
+C ..
+C .. Intrinsic Functions ..
+ INTRINSIC ACOS, ATAN, COS, DBLE, DCMPLX, DIMAG, EXP, LOG,
+ $ MAX, MIN, SIN, SQRT
+C
+C Test input parameters and workspace.
+C
+ PI = FOUR*ATAN( ONE )
+ PW = OMEGA(1)
+ N1 = N + 1
+ N2 = N + N1
+C
+ INFO = 0
+ IF( DISCFL.NE.0 .AND. DISCFL.NE.1 ) THEN
+ INFO = -1
+ ELSE IF( FLAG.NE.0 .AND. FLAG.NE.1 ) THEN
+ INFO = -2
+ ELSE IF ( LENDAT.LT.2 ) THEN
+ INFO = -3
+ ELSE IF ( PW.LT.ZERO ) THEN
+ INFO = -6
+ ELSE IF( N.GT.LENDAT - 1 ) THEN
+ INFO = -7
+ ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
+ INFO = -9
+ ELSE
+C
+ DO 10 K = 2, LENDAT
+ IF ( OMEGA(K).LT.PW )
+ $ INFO = -6
+ PW = OMEGA(K)
+ 10 CONTINUE
+C
+ IF ( DISCFL.EQ.1 .AND. OMEGA(LENDAT).GT.PI )
+ $ INFO = -6
+ END IF
+C
+ IF ( INFO.EQ.0 ) THEN
+C
+C Workspace.
+C
+ LW1 = 2*LENDAT + 4*HNPTS
+ LW2 = LENDAT + 6*HNPTS
+ MN = MIN( 2*LENDAT, N2 )
+C
+ IF ( N.GT.0 ) THEN
+ LW3 = 2*LENDAT*N2 + MAX( 2*LENDAT, N2 ) +
+ $ MAX( MN + 6*N + 4, 2*MN + 1 )
+ ELSE
+ LW3 = 4*LENDAT + 5
+ END IF
+C
+ IF ( FLAG.EQ.0 ) THEN
+ LW4 = 0
+ ELSE
+ LW4 = MAX( N*N + 5*N, 6*N + 1 + MIN ( 1, N ) )
+ END IF
+C
+ DLWMAX = MAX( 2, LW1, LW2, LW3, LW4 )
+C
+ IF ( N.GT.0 ) THEN
+ CLWMAX = LENDAT*( N2 + 2 )
+ ELSE
+ CLWMAX = LENDAT
+ END IF
+C
+ IF ( LDWORK.LT.DLWMAX ) THEN
+ INFO = -16
+ ELSE IF ( LZWORK.LT.CLWMAX ) THEN
+ INFO = -18
+ END IF
+ END IF
+C
+ IF ( INFO.NE.0 ) THEN
+C
+C Error return.
+C
+ CALL XERBLA( 'SB10YD', -INFO )
+ RETURN
+ END IF
+C
+C Set tolerances.
+C
+ TOLB = DLAMCH( 'Epsilon' )
+ TOLL = TOL
+ IF ( TOLL.LE.ZERO )
+ $ TOLL = FOUR*DBLE( LENDAT*N )*TOLB
+C
+C @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+C
+C Workspace usage 1.
+C Workspace: need 2*LENDAT + 4*HNPTS.
+C
+ IWDOMO = 1
+ IWDME = IWDOMO + LENDAT
+ IWYMAG = IWDME + 2*HNPTS
+ IWMAG = IWYMAG + 2*HNPTS
+C
+C Bilinear transformation.
+C
+ IF ( DISCFL.EQ.0 ) THEN
+ PW = SQRT( OMEGA(1)*OMEGA(LENDAT) + SQRT( TOLB ) )
+C
+ DO 20 K = 1, LENDAT
+ DWORK(IWDME+K-1) = ( OMEGA(K)/PW )**2
+ DWORK(IWDOMO+K-1) =
+ $ ACOS( ( ONE - DWORK(IWDME+K-1) )/
+ $ ( ONE + DWORK(IWDME+K-1) ) )
+ 20 CONTINUE
+C
+ ELSE
+ CALL DCOPY( LENDAT, OMEGA, 1, DWORK(IWDOMO), 1 )
+ END IF
+C
+C Linear interpolation.
+C
+ DO 30 K = 1, LENDAT
+ DWORK(IWMAG+K-1) = DLAPY2( RFRDAT(K), IFRDAT(K) )
+ DWORK(IWMAG+K-1) = ( ONE/LOG( TEN ) ) * LOG( DWORK(IWMAG+K-1) )
+ 30 CONTINUE
+C
+ DO 40 K = 1, HNPTS
+ DWORK(IWDME+K-1) = ( K - 1 )*PI/HNPTS
+ DWORK(IWYMAG+K-1) = ZERO
+C
+ IF ( DWORK(IWDME+K-1).LT.DWORK(IWDOMO) ) THEN
+ DWORK(IWYMAG+K-1) = DWORK(IWMAG)
+ ELSE IF ( DWORK(IWDME+K-1).GE.DWORK(IWDOMO+LENDAT-1) ) THEN
+ DWORK(IWYMAG+K-1) = DWORK(IWMAG+LENDAT-1)
+ END IF
+C
+ 40 CONTINUE
+C
+ DO 60 I = 2, LENDAT
+ P1 = HNPTS*DWORK(IWDOMO+I-2)/PI + ONE
+C
+ IP1 = INT( P1 )
+ IF ( DBLE( IP1 ).NE.P1 )
+ $ IP1 = IP1 + 1
+C
+ P2 = HNPTS*DWORK(IWDOMO+I-1)/PI + ONE
+C
+ IP2 = INT( P2 )
+ IF ( DBLE( IP2 ).NE.P2 )
+ $ IP2 = IP2 + 1
+C
+ DO 50 P = IP1, IP2 - 1
+ RAT = DWORK(IWDME+P-1) - DWORK(IWDOMO+I-2)
+ RAT = RAT/( DWORK(IWDOMO+I-1) - DWORK(IWDOMO+I-2) )
+ DWORK(IWYMAG+P-1) = ( ONE - RAT )*DWORK(IWMAG+I-2) +
+ $ RAT*DWORK(IWMAG+I-1)
+ 50 CONTINUE
+C
+ 60 CONTINUE
+C
+ DO 70 K = 1, HNPTS
+ DWORK(IWYMAG+K-1) = EXP( LOG( TEN )*DWORK(IWYMAG+K-1) )
+ 70 CONTINUE
+C
+C Duplicate data around disc.
+C
+ DO 80 K = 1, HNPTS
+ DWORK(IWDME+HNPTS+K-1) = TWO*PI - DWORK(IWDME+HNPTS-K)
+ DWORK(IWYMAG+HNPTS+K-1) = DWORK(IWYMAG+HNPTS-K)
+ 80 CONTINUE
+C
+C Complex cepstrum to get min phase:
+C LOG (Magnitude)
+C
+ DO 90 K = 1, 2*HNPTS
+ DWORK(IWYMAG+K-1) = TWO*LOG( DWORK(IWYMAG+K-1) )
+ 90 CONTINUE
+C
+C @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+C
+C Workspace usage 2.
+C Workspace: need LENDAT + 6*HNPTS.
+C
+ IWXR = IWYMAG
+ IWXI = IWMAG
+C
+ DO 100 K = 1, 2*HNPTS
+ DWORK(IWXI+K-1) = ZERO
+ 100 CONTINUE
+C
+C IFFT
+C
+ CALL DG01MD( 'I', 2*HNPTS, DWORK(IWXR), DWORK(IWXI), INFO2 )
+C
+C Rescale, because DG01MD doesn't do it.
+C
+ CALL DSCAL( HNPTS, ONE/( TWO*HNPTS ), DWORK(IWXR), 1 )
+ CALL DSCAL( HNPTS, ONE/( TWO*HNPTS ), DWORK(IWXI), 1 )
+C
+C Halve the result at 0.
+C
+ DWORK(IWXR) = DWORK(IWXR)/TWO
+ DWORK(IWXI) = DWORK(IWXI)/TWO
+C
+C FFT
+C
+ CALL DG01MD( 'D', HNPTS, DWORK(IWXR), DWORK(IWXI), INFO2 )
+C
+C Get the EXP of the result.
+C
+ DO 110 K = 1, HNPTS/2
+ XHAT(K) = EXP( DWORK(IWXR+K-1) )*
+ $ DCMPLX ( COS( DWORK(IWXI+K-1)), SIN( DWORK(IWXI+K-1) ) )
+ DWORK(IWDME+K-1) = DWORK(IWDME+2*K-2)
+ 110 CONTINUE
+C
+C Interpolate back to original frequency data.
+C
+ ISTART = 1
+ ISTOP = LENDAT
+C
+ DO 120 I = 1, LENDAT
+ ZWORK(I) = ZZERO
+ IF ( DWORK(IWDOMO+I-1).LE.DWORK(IWDME) ) THEN
+ ZWORK(I) = XHAT(1)
+ ISTART = I + 1
+ ELSE IF ( DWORK(IWDOMO+I-1).GE.DWORK(IWDME+HNPTS/2-1) )
+ $ THEN
+ ZWORK(I) = XHAT(HNPTS/2)
+ ISTOP = ISTOP - 1
+ END IF
+ 120 CONTINUE
+C
+ DO 140 I = ISTART, ISTOP
+ II = HNPTS/2
+ 130 CONTINUE
+ IF ( DWORK(IWDME+II-1).GE.DWORK(IWDOMO+I-1) )
+ $ P = II
+ II = II - 1
+ IF ( II.GT.0 )
+ $ GOTO 130
+ RAT = ( DWORK(IWDOMO+I-1) - DWORK(IWDME+P-2) )/
+ $ ( DWORK(IWDME+P-1) - DWORK(IWDME+P-2) )
+ ZWORK(I) = RAT*XHAT(P) + ( ONE - RAT )*XHAT(P-1)
+ 140 CONTINUE
+C
+C CASE N > 0.
+C This is the only allowed case in mu-synthesis subroutines.
+C
+ IF ( N.GT.0 ) THEN
+C
+C Preparation for frequency identification.
+C
+C @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+C
+C Complex workspace usage 1.
+C Complex workspace: need 2*LENDAT + LENDAT*(N+1).
+C
+ IWA0 = 1 + LENDAT
+ IWVAR = IWA0 + LENDAT*N1
+C
+ DO 150 K = 1, LENDAT
+ IF ( DISCFL.EQ.0 ) THEN
+ ZWORK(IWVAR+K-1) = DCMPLX( COS( DWORK(IWDOMO+K-1) ),
+ $ SIN( DWORK(IWDOMO+K-1) ) )
+ ELSE
+ ZWORK(IWVAR+K-1) = DCMPLX( COS( OMEGA(K) ),
+ $ SIN( OMEGA(K) ) )
+ END IF
+ 150 CONTINUE
+C
+C Array for DGELSY.
+C
+ DO 160 K = 1, N2
+ IWORK(K) = 0
+ 160 CONTINUE
+C
+C Constructing A0.
+C
+ DO 170 K = 1, LENDAT
+ ZWORK(IWA0+N*LENDAT+K-1) = ZONE
+ 170 CONTINUE
+C
+ DO 190 I = 1, N
+ DO 180 K = 1, LENDAT
+ ZWORK(IWA0+(N-I)*LENDAT+K-1) =
+ $ ZWORK(IWA0+(N1-I)*LENDAT+K-1)*ZWORK(IWVAR+K-1)
+ 180 CONTINUE
+ 190 CONTINUE
+C
+C @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+C
+C Complex workspace usage 2.
+C Complex workspace: need 2*LENDAT + LENDAT*(2*N+1).
+C
+ IWBP = IWVAR
+ IWAB = IWBP + LENDAT
+C
+C Constructing BP.
+C
+ DO 200 K = 1, LENDAT
+ ZWORK(IWBP+K-1) = ZWORK(IWA0+K-1)*ZWORK(K)
+ 200 CONTINUE
+C
+C Constructing AB.
+C
+ DO 220 I = 1, N
+ DO 210 K = 1, LENDAT
+ ZWORK(IWAB+(I-1)*LENDAT+K-1) = -ZWORK(K)*
+ $ ZWORK(IWA0+I*LENDAT+K-1)
+ 210 CONTINUE
+ 220 CONTINUE
+C
+C @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+C
+C Workspace usage 3.
+C Workspace: need LW3 = 2*LENDAT*(2*N+1) + max(2*LENDAT,2*N+1).
+C
+ IWBX = 1 + 2*LENDAT*N2
+ IWS = IWBX + MAX( 2*LENDAT, N2 )
+C
+C Constructing AX.
+C
+ DO 240 I = 1, N1
+ DO 230 K = 1, LENDAT
+ DWORK(2*(I-1)*LENDAT+K) =
+ $ DBLE( ZWORK(IWA0+(I-1)*LENDAT+K-1) )
+ DWORK((2*I-1)*LENDAT+K) =
+ $ DIMAG( ZWORK(IWA0+(I-1)*LENDAT+K-1) )
+ 230 CONTINUE
+ 240 CONTINUE
+C
+ DO 260 I = 1, N
+ DO 250 K = 1, LENDAT
+ DWORK(2*N1*LENDAT+2*(I-1)*LENDAT+K) =
+ $ DBLE( ZWORK(IWAB+(I-1)*LENDAT+K-1) )
+ DWORK(2*N1*LENDAT+(2*I-1)*LENDAT+K) =
+ $ DIMAG( ZWORK(IWAB+(I-1)*LENDAT+K-1) )
+ 250 CONTINUE
+ 260 CONTINUE
+C
+C Constructing BX.
+C
+ DO 270 K = 1, LENDAT
+ DWORK(IWBX+K-1) = DBLE( ZWORK(IWBP+K-1) )
+ DWORK(IWBX+LENDAT+K-1) = DIMAG( ZWORK(IWBP+K-1) )
+ 270 CONTINUE
+C
+C Estimating X.
+C Workspace: need LW3 + max( MN+3*(2*N+1)+1, 2*MN+1 ),
+C where MN = min( 2*LENDAT, 2*N+1 );
+C prefer larger.
+C
+ CALL DGELSY( 2*LENDAT, N2, 1, DWORK, 2*LENDAT, DWORK(IWBX),
+ $ MAX( 2*LENDAT, N2 ), IWORK, TOLL, RANK,
+ $ DWORK(IWS), LDWORK-IWS+1, INFO2 )
+ DLWMAX = MAX( DLWMAX, INT( DWORK(IWS) + IWS - 1 ) )
+C
+C Constructing A matrix.
+C
+ DO 280 K = 1, N
+ A(K,1) = -DWORK(IWBX+N1+K-1)
+ 280 CONTINUE
+C
+ IF ( N.GT.1 )
+ $ CALL DLASET( 'Full', N, N-1, ZERO, ONE, A(1,2), LDA )
+C
+C Constructing B matrix.
+C
+ DO 290 K = 1, N
+ B(K) = DWORK(IWBX+N1+K-1)*DWORK(IWBX) - DWORK(IWBX+K)
+ 290 CONTINUE
+C
+C Constructing C matrix.
+C
+ C(1) = -ONE
+C
+ DO 300 K = 2, N
+ C(K) = ZERO
+ 300 CONTINUE
+C
+C Constructing D matrix.
+C
+ D(1) = DWORK(IWBX)
+C
+C Transform to continuous-time case, if needed.
+C Workspace: need max(1,N);
+C prefer larger.
+C
+ IF ( DISCFL.EQ.0 ) THEN
+ CALL AB04MD( 'D', N, 1, 1, ONE, PW, A, LDA, B, LDA, C, 1,
+ $ D, 1, IWORK, DWORK, LDWORK, INFO2 )
+ IF ( INFO2.NE.0 ) THEN
+ INFO = 1
+ RETURN
+ END IF
+ DLWMAX = MAX( DLWMAX, INT( DWORK(1) ) )
+ END IF
+C
+C Make all the real parts of the poles and the zeros negative.
+C
+ IF ( FLAG.EQ.1 ) THEN
+C
+C Workspace: need max(N*N + 5*N, 6*N + 1 + min(1,N));
+C prefer larger.
+ CALL SB10ZP( DISCFL, N, A, LDA, B, C, D, IWORK, DWORK,
+ $ LDWORK, INFO )
+ IF ( INFO.NE.0 )
+ $ RETURN
+ DLWMAX = MAX( DLWMAX, INT( DWORK(1) ) )
+ END IF
+C
+ ELSE
+C
+C CASE N = 0.
+C
+C @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+C
+C Workspace usage 4.
+C Workspace: need 4*LENDAT.
+C
+ IWBMAT = 1 + 2*LENDAT
+ IWS = IWBMAT + 2*LENDAT
+C
+C Constructing AMAT and BMAT.
+C
+ DO 310 K = 1, LENDAT
+ DWORK(K) = ONE
+ DWORK(K+LENDAT) = ZERO
+ DWORK(IWBMAT+K-1) = DBLE( ZWORK(K) )
+ DWORK(IWBMAT+LENDAT+K-1) = DIMAG( ZWORK(K) )
+ 310 CONTINUE
+C
+C Estimating D matrix.
+C Workspace: need 4*LENDAT + 5;
+C prefer larger.
+C
+ IWORK(1) = 0
+ CALL DGELSY( 2*LENDAT, 1, 1, DWORK, 2*LENDAT, DWORK(IWBMAT),
+ $ 2*LENDAT, IWORK, TOLL, RANK, DWORK(IWS),
+ $ LDWORK-IWS+1, INFO2 )
+ DLWMAX = MAX( DLWMAX, INT( DWORK(IWS) + IWS - 1 ) )
+C
+ D(1) = DWORK(IWBMAT)
+C
+ END IF
+C
+C @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+C
+ DWORK(1) = DLWMAX
+ DWORK(2) = CLWMAX
+ RETURN
+C
+C *** Last line of SB10YD ***
+ END
Property changes on: trunk/octave-forge/extra/control-devel/src/SB10YD.f
___________________________________________________________________
Added: svn:executable
+ *
Added: trunk/octave-forge/extra/control-devel/src/SB10ZP.f
===================================================================
--- trunk/octave-forge/extra/control-devel/src/SB10ZP.f (rev 0)
+++ trunk/octave-forge/extra/control-devel/src/SB10ZP.f 2011-10-19 16:38:34 UTC (rev 8789)
@@ -0,0 +1,339 @@
+ SUBROUTINE SB10ZP( DISCFL, N, A, LDA, B, C, D, IWORK, DWORK,
+ $ LDWORK, INFO )
+C
+C SLICOT RELEASE 5.0.
+C
+C Copyright (c) 2002-2010 NICONET e.V.
+C
+C This program is free software: you can redistribute it and/or
+C modify it under the terms of the GNU General Public License as
+C published by the Free Software Foundation, either version 2 of
+C the License, or (at your option) any later version.
+C
+C This program is distributed in the hope that it will be useful,
+C but WITHOUT ANY WARRANTY; without even the implied warranty of
+C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+C GNU General Public License for more details.
+C
+C You should have received a copy of the GNU General Public License
+C along with this program. If not, see
+C <http://www.gnu.org/licenses/>.
+C
+C PURPOSE
+C
+C To transform a SISO (single-input single-output) system [A,B;C,D]
+C by mirroring its unstable poles and zeros in the boundary of the
+C stability domain, thus preserving the frequency response of the
+C system, but making it stable and minimum phase. Specifically, for
+C a continuous-time system, the positive real parts of its poles
+C and zeros are exchanged with their negatives. Discrete-time
+C systems are first converted to continuous-time systems using a
+C bilinear transformation, and finally converted back.
+C
+C ARGUMENTS
+C
+C Input/Output parameters
+C
+C DISCFL (input) INTEGER
+C Indicates the type of the system, as follows:
+C = 0: continuous-time system;
+C = 1: discrete-time system.
+C
+C N (input/output) INTEGER
+C On entry, the order of the original system. N >= 0.
+C On exit, the order of the transformed, minimal system.
+C
+C A (input/output) DOUBLE PRECISION array, dimension (LDA,N)
+C On entry, the leading N-by-N part of this array must
+C contain the original system matrix A.
+C On exit, the leading N-by-N part of this array contains
+C the transformed matrix A, in an upper Hessenberg form.
+C
+C LDA INTEGER
+C The leading dimension of the array A. LDA >= MAX(1,N).
+C
+C B (input/output) DOUBLE PRECISION array, dimension (N)
+C On entry, this array must contain the original system
+C vector B.
+C On exit, this array contains the transformed vector B.
+C
+C C (input/output) DOUBLE PRECISION array, dimension (N)
+C On entry, this array must contain the original system
+C vector C.
+C On exit, this array contains the transformed vector C.
+C The first N-1 elements are zero (for the exit value of N).
+C
+C D (input/output) DOUBLE PRECISION array, dimension (1)
+C On entry, this array must contain the original system
+C scalar D.
+C On exit, this array contains the transformed scalar D.
+C
+C Workspace
+C
+C IWORK INTEGER array, dimension max(2,N+1)
+C
+C DWORK DOUBLE PRECISION array, dimension (LDWORK)
+C On exit, if INFO = 0, DWORK(1) returns the optimal value
+C of LDWORK.
+C
+C LDWORK INTEGER
+C The length of the array DWORK.
+C LDWORK >= max(N*N + 5*N, 6*N + 1 + min(1,N)).
+C For optimum performance LDWORK should be larger.
+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 = 1: if the discrete --> continuous transformation cannot
+C be made;
+C = 2: if the system poles cannot be found;
+C = 3: if the inverse system cannot be found, i.e., D is
+C (close to) zero;
+C = 4: if the system zeros cannot be found;
+C = 5: if the state-space representation of the new
+C transfer function T(s) cannot be found;
+C = 6: if the continuous --> discrete transformation cannot
+C be made.
+C
+C METHOD
+C
+C First, if the system is discrete-time, it is transformed to
+C continuous-time using alpha = beta = 1 in the bilinear
+C transformation implemented in the SLICOT routine AB04MD.
+C Then the eigenvalues of A, i.e., the system poles, are found.
+C Then, the inverse of the original system is found and its poles,
+C i.e., the system zeros, are evaluated.
+C The obtained system poles Pi and zeros Zi are checked and if a
+C positive real part is detected, it is exchanged by -Pi or -Zi.
+C Then the polynomial coefficients of the transfer function
+C T(s) = Q(s)/P(s) are found.
+C The state-space representation of T(s) is then obtained.
+C The system matrices B, C, D are scaled so that the transformed
+C system has the same system gain as the original system.
+C If the original system is discrete-time, then the result (which is
+C continuous-time) is converted back to discrete-time.
+C
+C CONTRIBUTORS
+C
+C Asparuh Markovski, Technical University of Sofia, July 2003.
+C
+C REVISIONS
+C
+C V. Sima, Research Institute for Informatics, Bucharest, Aug. 2003.
+C
+C KEYWORDS
+C
+C Bilinear transformation, stability, state-space representation.
+C
+C ******************************************************************
+C
+C .. Parameters ..
+ DOUBLE PRECISION ZERO, ONE
+ PARAMETER ( ZERO = 0.0D+0, ONE = 1.0D+0 )
+C ..
+C .. Scalar Arguments ..
+ INTEGER DISCFL, INFO, LDA, LDWORK, N
+C ..
+C .. Array Arguments ..
+ INTEGER IWORK( * )
+ DOUBLE PRECISION A( LDA, * ), B( * ), C( * ), D( * ), DWORK( * )
+C ..
+C .. Local Scalars ..
+ INTEGER I, IDW1, IDW2, IDW3, IMP, IMZ, INFO2, IWA, IWP,
+ $ IWPS, IWQ, IWQS, LDW1, MAXWRK, REP, REZ
+ DOUBLE PRECISION RCOND, SCALB, SCALC, SCALD
+C ..
+C .. Local Arrays ..
+ INTEGER INDEX(1)
+C ..
+C .. External Subroutines ..
+ EXTERNAL AB04MD, AB07ND, DCOPY, DGEEV, DLACPY, DSCAL,
+ $ MC01PD, TD04AD, XERBLA
+C ..
+C .. Intrinsic Functions ..
+ INTRINSIC ABS, INT, MAX, MIN, SIGN, SQRT
+C
+C Test input parameters and workspace.
+C
+ INFO = 0
+ IF ( DISCFL.NE.0 .AND. DISCFL.NE.1 ) THEN
+ INFO = -1
+ ELSE IF ( N.LT.0 ) THEN
+ INFO = -2
+ ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
+ INFO = -4
+ ELSE IF ( LDWORK.LT.MAX( N*N + 5*N, 6*N + 1 + MIN( 1, N ) ) ) THEN
+ INFO = -10
+ END IF
+C
+ IF ( INFO.NE.0 ) THEN
+C
+C Error return.
+C
+ CALL XERBLA( 'SB10ZP', -INFO )
+ RETURN
+ END IF
+C
+C Quick return if possible.
+C
+ IF( N.EQ.0 ) THEN
+ DWORK(1) = ONE
+ RETURN
+ END IF
+C
+C Workspace usage 1.
+C
+ REP = 1
+ IMP = REP + N
+ REZ = IMP + N
+ IMZ = REZ + N
+ IWA = REZ
+ IDW1 = IWA + N*N
+ LDW1 = LDWORK - IDW1 + 1
+C
+C 1. Discrete --> continuous transformation if needed.
+C
+ IF ( DISCFL.EQ.1 ) THEN
+C
+C Workspace: need max(1,N);
+C prefer larger.
+C
+ CALL AB04MD( 'D', N, 1, 1, ONE, ONE, A, LDA, B, LDA, C, 1,
+ $ D, 1, IWORK, DWORK, LDWORK, INFO2 )
+ IF ( INFO2.NE.0 ) THEN
+ INFO = 1
+ RETURN
+ END IF
+ MAXWRK = INT( DWORK(1) )
+ ELSE
+ MAXWRK = 0
+ END IF
+C
+C 2. Determine the factors for restoring system gain.
+C
+ SCALD = D(1)
+ SCALC = SQRT( ABS( SCALD ) )
+ SCALB = SIGN( SCALC, SCALD )
+C
+C 3. Find the system poles, i.e., the eigenvalues of A.
+C Workspace: need N*N + 2*N + 3*N;
+C prefer larger.
+C
+ CALL DLACPY( 'Full', N, N, A, LDA, DWORK(IWA), N )
+C
+ CALL DGEEV( 'N', 'N', N, DWORK(IWA), N, DWORK(REP), DWORK(IMP),
+ $ DWORK(IDW1), 1, DWORK(IDW1), 1, DWORK(IDW1), LDW1,
+ $ INFO2 )
+ IF ( INFO2.NE.0 ) THEN
+ INFO = 2
+ RETURN
+ END IF
+ MAXWRK = MAX( MAXWRK, INT( DWORK(IDW1) + IDW1 - 1 ) )
+C
+C 4. Compute the inverse system [Ai, Bi; Ci, Di].
+C Workspace: need N*N + 2*N + 4;
+C prefer larger.
+C
+ CALL AB07ND( N, 1, A, LDA, B, LDA, C, 1, D, 1, RCOND, IWORK,
+ $ DWORK(IDW1), LDW1, INFO2 )
+ IF ( INFO2.NE.0 ) THEN
+ INFO = 3
+ RETURN
+ END IF
+ MAXWRK = MAX( MAXWRK, INT( DWORK(IDW1) + IDW1 - 1 ) )
+C
+C 5. Find the system zeros, i.e., the eigenvalues of Ai.
+C Workspace: need 4*N + 3*N;
+C prefer larger.
+C
+ IDW1 = IMZ + N
+ LDW1 = LDWORK - IDW1 + 1
+C
+ CALL DGEEV( 'N', 'N', N, A, LDA, DWORK(REZ), DWORK(IMZ),
+ $ DWORK(IDW1), 1, DWORK(IDW1), 1, DWORK(IDW1), LDW1,
+ $ INFO2 )
+ IF ( INFO2.NE.0 ) THEN
+ INFO = 4
+ RETURN
+ END IF
+ MAXWRK = MAX( MAXWRK, INT( DWORK(IDW1) + IDW1 - 1 ) )
+C
+C 6. Exchange the zeros and the poles with positive real parts with
+C their negatives.
+C
+ DO 10 I = 0, N - 1
+ IF ( DWORK(REP+I).GT.ZERO )
+ $ DWORK(REP+I) = -DWORK(REP+I)
+ IF ( DWORK(REZ+I).GT.ZERO )
+ $ DWORK(REZ+I) = -DWORK(REZ+I)
+ 10 CONTINUE
+C
+C Workspace usage 2.
+C
+ IWP = IDW1
+ IDW2 = IWP + N + 1
+ IWPS = 1
+C
+C 7. Construct the nominator and the denominator
+C of the system transfer function T( s ) = Q( s )/P( s ).
+C 8. Rearrange the coefficients in Q(s) and P(s) because
+C MC01PD subroutine produces them in increasing powers of s.
+C Workspace: need 6*N + 2.
+C
+ CALL MC01PD( N, DWORK(REP), DWORK(IMP), DWORK(IWP), DWORK(IDW2),
+ $ INFO2 )
+ CALL DCOPY( N+1, DWORK(IWP), -1, DWORK(IWPS), 1 )
+C
+C Workspace usage 3.
+C
+ IWQ = IDW1
+ IWQS = IWPS + N + 1
+ IDW3 = IWQS + N + 1
+C
+ CALL MC01PD( N, DWORK(REZ), DWORK(IMZ), DWORK(IWQ), DWORK(IDW2),
+ $ INFO2 )
+ CALL DCOPY( N+1, DWORK(IWQ), -1, DWORK(IWQS), 1 )
+C
+C 9. Make the conversion T(s) --> [A, B; C, D].
+C Workspace: need 2*N + 2 + N + max(N,3);
+C prefer larger.
+C
+ INDEX(1) = N
+ CALL TD04AD( 'R', 1, 1, INDEX, DWORK(IWPS), 1, DWORK(IWQS), 1, 1,
+ $ N, A, LDA, B, LDA, C, 1, D, 1, -ONE, IWORK,
+ $ DWORK(IDW3), LDWORK-IDW3+1, INFO2 )
+ IF ( INFO2.NE.0 ) THEN
+ INFO = 5
+ RETURN
+ END IF
+ MAXWRK = MAX( MAXWRK, INT( DWORK(IDW3) + IDW3 - 1 ) )
+C
+C 10. Scale the transformed system to the previous gain.
+C
+ IF ( N.GT.0 ) THEN
+ CALL DSCAL( N, SCALB, B, 1 )
+ C(N) = SCALC*C(N)
+ END IF
+C
+ D(1) = SCALD
+C
+C 11. Continuous --> discrete transformation if needed.
+C
+ IF ( DISCFL.EQ.1 ) THEN
+ CALL AB04MD( 'C', N, 1, 1, ONE, ONE, A, LDA, B, LDA, C, 1,
+ $ D, 1, IWORK, DWORK, LDWORK, INFO2 )
+
+ IF ( INFO2.NE.0 ) THEN
+ INFO = 6
+ RETURN
+ END IF
+ END IF
+C
+ DWORK(1) = MAXWRK
+ RETURN
+C
+C *** Last line of SB10ZP ***
+ END
Property changes on: trunk/octave-forge/extra/control-devel/src/SB10ZP.f
___________________________________________________________________
Added: svn:executable
+ *
Added: trunk/octave-forge/extra/control-devel/src/TD03AY.f
===================================================================
--- trunk/octave-forge/extra/control-devel/src/TD03AY.f (rev 0)
+++ trunk/octave-forge/extra/control-devel/src/TD03AY.f 2011-10-19 16:38:34 UTC (rev 8789)
@@ -0,0 +1,171 @@
+ SUBROUTINE TD03AY( MWORK, PWORK, INDEX, DCOEFF, LDDCOE, UCOEFF,
+ $ LDUCO1, LDUCO2, N, A, LDA, B, LDB, C, LDC, D,
+ $ LDD, INFO )
+C
+C SLICOT RELEASE 5.0.
+C
+C Copyright (c) 2002-2010 NICONET e.V.
+C
+C This program is free software: you can redistribute it and/or
+C modify it under the terms of the GNU General Public License as
+C published by the Free Software Foundation, either version 2 of
+C the License, or (at your option) any later version.
+C
+C This program is distributed in the hope that it will be useful,
+C but WITHOUT ANY WARRANTY; without even the implied warranty of
+C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+C GNU General Public License for more details.
+C
+C You should have received a copy of the GNU General Public License
+C along with this program. If not, see
+C <http://www.gnu.org/licenses/>.
+C
+C Calculates a state-space representation for a (PWORK x MWORK)
+C transfer matrix given in the form of polynomial row vectors over
+C common denominators (not necessarily lcd's). Such a description
+C is simply the polynomial matrix representation
+C
+C T(s) = inv(D(s)) * U(s),
+C
+C where D(s) is diagonal with (I,I)-th element D:I(s) of degree
+C INDEX(I); applying Wolovich's Observable Structure Theorem to
+C this left matrix fraction then yields an equivalent state-space
+C representation in observable companion form, of order
+C N = sum(INDEX(I)). As D(s) is diagonal, the PWORK ordered
+C 'non-trivial' columns of C and A are very simply calculated, these
+C submatrices being diagonal and (INDEX(I) x 1) - block diagonal,
+C respectively: finding B and D is also somewhat simpler than for
+C general P(s) as dealt with in TC04AD. Finally, the state-space
+C representation obtained here is not necessarily controllable
+C (as D(s) and U(s) are not necessarily relatively left prime), but
+C it is theoretically completely observable: however, its
+C observability matrix may be poorly conditioned, so it is safer
+C not to assume observability either.
+C
+C REVISIONS
+C
+C May 13, 1998.
+C
+C KEYWORDS
+C
+C Coprime matrix fraction, elementary polynomial operations,
+C polynomial matrix, state-space representation, transfer matrix.
+C
+C ******************************************************************
+C
+ DOUBLE PRECISION ZERO, ONE
+ PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0 )
+C .. Scalar Arguments ..
+ INTEGER INFO, LDA, LDB, LDC, LDD, LDDCOE, LDUCO1,
+ $ LDUCO2, MWORK, N, PWORK
+C .. Array Arguments ..
+ INTEGER INDEX(*)
+ DOUBLE PRECISION A(LDA,*), B(LDB,*), C(LDC,*), D(LDD,*),
+ $ DCOEFF(LDDCOE,*), UCOEFF(LDUCO1,LDUCO2,*)
+C .. Local Scalars ..
+ INTEGER I, IA, IBIAS, INDCUR, JA, JMAX1, K
+ DOUBLE PRECISION ABSDIA, ABSDMX, BIGNUM, DIAG, SMLNUM, UMAX1,
+ $ TEMP
+C .. External Functions ..
+ INTEGER IDAMAX
+ DOUBLE PRECISION DLAMCH
+ EXTERNAL DLAMCH, IDAMAX
+C .. External Subroutines ..
+ EXTERNAL DAXPY, DCOPY, DLASET, DSCAL
+C .. Intrinsic Functions ..
+ INTRINSIC ABS
+C .. Executable Statements ..
+C
+ INFO = 0
+C
+C Initialize A and C to be zero, apart from 1's on the subdiagonal
+C of A.
+C
+ CALL DLASET( 'Upper', N, N, ZERO, ZERO, A, LDA )
+ IF ( N.GT.1 ) CALL DLASET( 'Lower', N-1, N-1, ZERO, ONE, A(2,1),
+ $ LDA )
+C
+ CALL DLASET( 'Full', PWORK, N, ZERO, ZERO, C, LDC )
+C
+C Calculate B and D, as well as 'non-trivial' elements of A and C.
+C Check if any leading coefficient of D(s) nearly zero: if so, exit.
+C Caution is taken to avoid overflow.
+C
+ SMLNUM = DLAMCH( 'Safe minimum' ) / DLAMCH( 'Precision' )
+ BIGNUM = ONE / SMLNUM
+C
+ IBIAS = 2
+ JA = 0
+C
+ DO 20 I = 1, PWORK
+ ABSDIA = ABS( DCOEFF(I,1) )
+ JMAX1 = IDAMAX( MWORK, UCOEFF(I,1,1), LDUCO1 )
+ UMAX1 = ABS( UCOEFF(I,JMAX1,1) )
+ IF ( ( ABSDIA.LT.SMLNUM ) .OR.
+ $ ( ABSDIA.LT.ONE .AND. UMAX1.GT.ABSDIA*BIGNUM ) ) THEN
+C
+C Error return.
+C
+ INFO = I
+ RETURN
+ END IF
+ DIAG = ONE/DCOEFF(I,1)
+ INDCUR = INDEX(I)
+ IF ( INDCUR.NE.0 ) THEN
+ IBIAS = IBIAS + INDCUR
+ JA = JA + INDCUR
+ IF ( INDCUR.GE.1 ) THEN
+ JMAX1 = IDAMAX( INDCUR, DCOEFF(I,2), LDDCOE )
+ ABSDMX = ABS( DCOEFF(I,JMAX1) )
+ IF ( ABSDIA.GE.ONE ) THEN
+ IF ( UMAX1.GT.ONE ) THEN
+ IF ( ( ABSDMX/ABSDIA ).GT.( BIGNUM/UMAX1 ) ) THEN
+C
+C Error return.
+C
+ INFO = I
+ RETURN
+ END IF
+ END IF
+ ELSE
+ IF ( UMAX1.GT.ONE ) THEN
+ IF ( ABSDMX.GT.( BIGNUM*ABSDIA )/UMAX1 ) THEN
+C
+C Error return.
+C
+ INFO = I
+ RETURN
+ END IF
+ END IF
+ END IF
+ END IF
+C
+C I-th 'non-trivial' sub-vector of A given from coefficients
+C of D:I(s), while I-th row block of B given from this and
+C row I of U(s).
+C
+ DO 10 K = 2, INDCUR + 1
+ IA = IBIAS - K
+ TEMP = -DIAG*DCOEFF(I,K)
+ A(IA,JA) = TEMP
+C
+ CALL DCOPY( MWORK, UCOEFF(I,1,K), LDUCO1, B(IA,1), LDB )
+ CALL DAXPY( MWORK, TEMP, UCOEFF(I,1,1), LDUCO1, B(IA,1),
+ $ LDB )
+ 10 CONTINUE
+C
+ IF ( JA.LT.N ) A(JA+1,JA) = ZERO
+C
+C Finally, I-th 'non-trivial' entry of C and row of D obtained
+C also.
+C
+ C(I,JA) = DIAG
+ END IF
+C
+ CALL DCOPY( MWORK, UCOEFF(I,1,1), LDUCO1, D(I,1), LDD )
+ CALL DSCAL( MWORK, DIAG, D(I,1), LDD )
+ 20 CONTINUE
+C
+ RETURN
+C *** Last line of TD03AY ***
+ END
Property changes on: trunk/octave-forge/extra/control-devel/src/TD03AY.f
___________________________________________________________________
Added: svn:executable
+ *
Added: trunk/octave-forge/extra/control-devel/src/TD04AD.f
===================================================================
--- trunk/octave-forge/extra/control-devel/src/TD04AD.f (rev 0)
+++ trunk/octave-forge/extra/control-devel/src/TD04AD.f 2011-10-19 16:38:34 UTC (rev 8789)
@@ -0,0 +1,425 @@
+ SUBROUTINE TD04AD( ROWCOL, M, P, INDEX, DCOEFF, LDDCOE, UCOEFF,
+ $ LDUCO1, LDUCO2, NR, A, LDA, B, LDB, C, LDC, D,
+ $ LDD, TOL, IWORK, DWORK, LDWORK, INFO )
+C
+C SLICOT RELEASE 5.0.
+C
+C Copyright (c) 2002-2010 NICONET e.V.
+C
+C This program is free software: you can redistribute it and/or
+C modify it under the terms of the GNU General Public License as
+C published by the Free Software Foundation, either version 2 of
+C the License, or (at your option) any later version.
+C
+C This program is distributed in the hope that it will be useful,
+C but WITHOUT ANY WARRANTY; without even the implied warranty of
+C MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+C GNU General Public License for more details.
+C
+C You should have received a copy of the GNU General Public License
+C along with this program. If not, see
+C <http://www.gnu.org/licenses/>.
+C
+C PURPOSE
+C
+C To find a minimal state-space representation (A,B,C,D) for a
+C proper transfer matrix T(s) given as either row or column
+C polynomial vectors over denominator polynomials, possibly with
+C uncancelled common terms.
+C
+C ARGUMENTS
+C
+C Mode Parameters
+C
+C ROWCOL CHARACTER*1
+C Indicates whether the transfer matrix T(s) is given as
+C rows or columns over common denominators as follows:
+C = 'R': T(s) is given as rows over common denominators;
+C = 'C': T(s) is given as columns over common denominators.
+C
+C Input/Output Parameters
+C
+C M (input) INTEGER
+C The number of system inputs. M >= 0.
+C
+C P (input) INTEGER
+C The number of system outputs. P >= 0.
+C
+C INDEX (input) INTEGER array, dimension (porm), where porm = P,
+C if ROWCOL = 'R', and porm = M, if ROWCOL = 'C'.
+C This array must contain the degrees of the denominator
+C polynomials in D(s).
+C
+C DCOEFF (input) DOUBLE PRECISION array, dimension (LDDCOE,kdcoef),
+C where kdcoef = MAX(INDEX(I)) + 1.
+C The leading porm-by-kdcoef part of this array must contain
+C the coefficients of each denominator polynomial.
+C DCOEFF(I,K) is the coefficient in s**(INDEX(I)-K+1) of the
+C I-th denominator polynomial in D(s), where
+C K = 1,2,...,kdcoef.
+C
+C LDDCOE INTEGER
+C The leading dimension of array DCOEFF.
+C LDDCOE >= MAX(1,P) if ROWCOL = 'R';
+C LDDCOE >= MAX(1,M) if ROWCOL = 'C'.
+C
+C UCOEFF (input) DOUBLE PRECISION array, dimension
+C (LDUCO1,LDUCO2,kdcoef)
+C The leading P-by-M-by-kdcoef part of this array must
+C contain the numerator matrix U(s); if ROWCOL = 'C', this
+C array is modified internally but restored on exit, and the
+C remainder of the leading MAX(M,P)-by-MAX(M,P)-by-kdcoef
+C part is used as internal workspace.
+C UCOEFF(I,J,K) is the coefficient in s**(INDEX(iorj)-K+1)
+C of polynomial (I,J) of U(s), where K = 1,2,...,kdcoef;
+C if ROWCOL = 'R' then iorj = I, otherwise iorj = J.
+C Thus for ROWCOL = 'R', U(s) =
+C diag(s**INDEX(I))*(UCOEFF(.,.,1)+UCOEFF(.,.,2)/s+...).
+C
+C LDUCO1 INTEGER
+C The leading dimension of array UCOEFF.
+C LDUCO1 >= MAX(1,P) if ROWCOL = 'R';
+C LDUCO1 >= MAX(1,M,P) if ROWCOL = 'C'.
+C
+C LDUCO2 INTEGER
+C The second dimension of array UCOEFF.
+C LDUCO2 >= MAX(1,M) if ROWCOL = 'R';
+C LDUCO2 >= MAX(1,M,P) if ROWCOL = 'C'.
+C
+C NR (output) INTEGER
+C The order of the resulting minimal realization, i.e. the
+C order of th...
[truncated message content] |
