PageRenderTime 62ms CodeModel.GetById 25ms RepoModel.GetById 0ms app.codeStats 1ms

/trunk/octave-forge/extra/control-devel/devel/dksyn/muHopt.f

#
FORTRAN Legacy | 882 lines | 445 code | 0 blank | 437 comment | 0 complexity | 6a7875deac50430f23a71c6af5faf9ae MD5 | raw file
Possible License(s): GPL-2.0, BSD-3-Clause, LGPL-2.1, GPL-3.0, LGPL-3.0
  1. C MUHOPT.f - Gateway function to compute the mu optimal or H_inf
  2. C controller using SLICOT routines SB10AD, SB10MD, AB04MD,
  3. C AB05MD, and AB07ND.
  4. C
  5. C RELEASE 2.0 of SLICOT Robust Control Toolbox.
  6. C Based on SLICOT RELEASE 5.0, Copyright (c) 2004-2010 NICONET e.V.
  7. C
  8. C Matlab call:
  9. C If mu optimal controller is desired (job > 0):
  10. C
  11. C [AK,BK,CK,DK(,mju,RCOND)] = muHopt(job,discr,A,B,C,D,ncon,nmeas,
  12. C gamma,omega,nblock,itype,ord(,qutol(,gtol(,actol))))
  13. C
  14. C If H_inf optimal controller is only desired (job <= 0):
  15. C
  16. C [AK,BK,CK,DK(,gammin,RCOND)] = muHopt(job,discr,A,B,C,D,ncon,
  17. C nmeas,gamma(,gtol(,actol)))
  18. C
  19. C Purpose:
  20. C To compute the matrices of the mu optimal or H_inf optimal
  21. C controller given the model in a state space. It also outputs the
  22. C mu norm of the closed loop system, if mu optimal controller is
  23. C desired, or the value of gamma reached in the H_inf synthesis
  24. C problem, if H_inf controller is only desired.
  25. C The discrete-time systems are handled via bilinear transformation
  26. C to continuous-time and then the controller obtained is discretised.
  27. C For the K step the SB10AD subroutine is employed, and the SB10MD
  28. C subroutine performs the D step.
  29. C
  30. C Input parameters:
  31. C job - indicates the type of the controller as well as the strategy
  32. C for reducing the gamma value:
  33. C > 0 : mu optimal controller is desired;
  34. C <= 0 : H_inf optimal controller only is desired.
  35. C Specifically, job
  36. C = 0 : find suboptimal controller only;
  37. C and abs(job) specifies the strategy for reducing gamma:
  38. C = 1 : use bisection method for decreasing gamma from gamma
  39. C to gammin until the closed-loop system leaves
  40. C stability;
  41. C = 2 : scan from gamma to 0 trying to find the minimal gamma
  42. C for which the closed-loop system retains stability;
  43. C = 3 : first bisection, then scanning.
  44. C discr - indicates the type of the system, as follows:
  45. C = 0 : continuous-time system;
  46. C = 1 : discrete-time system.
  47. C A - the n-by-n system state matrix A of the plant.
  48. C B - the n-by-m system input matrix B of the plant.
  49. C C - the p-by-n system output matrix C of the plant.
  50. C D - the p-by-m system input/output matrix D of the plant.
  51. C ncon - the number of control inputs.
  52. C p-nmeas >= ncon >= 0.
  53. C nmeas - the number of measurements.
  54. C p-nmeas = m-ncon >= nmeas >= 0.
  55. C gamma - the initial value of gamma on input. It is assumed that
  56. C gamma is sufficiently large so that the controller is
  57. C admissible. gamma >= 0.
  58. C omega - the vector of length lendat >= 2 with the frequencies.
  59. C They must be nonnegative, in increasing order, and
  60. C for discrete-time systems between 0 and pi.
  61. C nblock - the vector with the block structure of the uncertainty.
  62. C nblock(I) is the size of each block.
  63. C itype - the vector of the same length as nblock indicating
  64. C the type of each block.
  65. C itype(I) = 1 indicates that the corresponding block is a
  66. C real block. THIS OPTION IS NOT SUPPORTED NOW.
  67. C itype(I) = 2 indicates that the corresponding block is a
  68. C complex block. THIS IS THE ONLY ALLOWED VALUE NOW!
  69. C ord - the maximum order of each block in the D-fitting procedure.
  70. C 1 <= ord <= lendat-1.
  71. C qutol - (optional) the acceptable mean relative error between
  72. C the D(jw) and the frequency response of the estimated block
  73. C [ADi,BDi;CDi,DDi]. When it is reached, the result is
  74. C taken as good enough.
  75. C Default: qutol = 2.
  76. C gtol - (optional) tolerance used for controlling the accuracy
  77. C of gamma and its distance to the estimated minimal possible
  78. C value of gamma.
  79. C If gtol <= 0, then sqrt(EPS) is used, where EPS is the
  80. C relative machine precision.
  81. C Default: gtol = 0.01.
  82. C actol - (optional) upper bound for the poles of the closed-loop
  83. C system used for determining if it is stable.
  84. C actol <= 0 for stable systems.
  85. C Default: actol = 0.
  86. C
  87. C Output parameters:
  88. C AK - the n-by-n controller state matrix AK.
  89. C BK - the n-by-nmeas controller input matrix BK.
  90. C CK - the ncon-by-n controller output matrix CK.
  91. C DK - the ncon-by-nmeas controller input/output matrix DK.
  92. C mju - (optional) the vector with the estimated upper bound of
  93. C the structured singular value for each frequency in omega
  94. C for the closed-loop system.
  95. C gammin - (optional) the estimated minimal admissible gamma.
  96. C RCOND - (optional) for each successful J-th K step:
  97. C RCOND(J) contains the reciprocal condition number of the
  98. C control transformation matrix;
  99. C RCOND(J+1) contains the reciprocal condition number of the
  100. C measurement transformation matrix;
  101. C RCOND(J+2) contains an estimate of the reciprocal condition
  102. C number of the X-Riccati equation;
  103. C RCOND(J+3) contains an estimate of the reciprocal condition
  104. C number of the Y-Riccati equation.
  105. C If job = 0, only RCOND(1:4) are set by the first K step.
  106. C
  107. C Contributor:
  108. C A. Markovski, Technical University of Sofia, October 2003.
  109. C
  110. C Revisions:
  111. C V. Sima, April 2004, Sept. 2004, Mar. 2005, Apr. 2009.
  112. C
  113. C***********************************************************************
  114. C
  115. SUBROUTINE MEXFUNCTION( NLHS, PLHS, NRHS, PRHS )
  116. C
  117. C .. Parameters ..
  118. INTEGER MAXIT, HNPTS
  119. PARAMETER ( MAXIT = 15, HNPTS = 2048 )
  120. DOUBLE PRECISION ZERO, ONE, TWO, P01
  121. PARAMETER ( ZERO = 0.0D0, ONE = 1.0D0, TWO = 2.0D0,
  122. $ P01 = 0.01D0 )
  123. C
  124. C .. Mex-file interface parameters ..
  125. INTEGER NLHS, PLHS(*), NRHS, PRHS(*)
  126. C
  127. C .. Mex-file integer functions ..
  128. INTEGER mxCreateDoubleMatrix, mxGetPr, mxGetM, mxGetN
  129. $ mxIsNumeric, mxIsComplex
  130. C
  131. C .. Parameters used by SLICOT subroutines ..
  132. INTEGER F, INFO, JOB, LBWORK, LDA, LDAC, LDAD, LDAE,
  133. $ LDAK, LDB, LDBC, LDBD, LDBE, LDBK, LDC, LDCC,
  134. $ LDCD, LDCE, LDCK, LDD, LDDC, LDDD, LDDE, LDDK,
  135. $ LDWORK, LENDAT, LIWORK, LZWORK, M, M2, MNB, N,
  136. $ N2E, NE, NEB, NP, NP1, NP2, NTEMP, ORD, TOTORD
  137. DOUBLE PRECISION ACTOL, GAMMA, GTOL, QUTOL, TEMP
  138. DOUBLE PRECISION RCOND(4*MAXIT)
  139. C
  140. C .. Allocatable arrays ..
  141. C !Fortran 90/95 (Fixed dimensions should be used with Fortran 77.)
  142. LOGICAL, ALLOCATABLE :: BWORK(:)
  143. INTEGER, ALLOCATABLE :: ITYPE(:), IWORK(:), NBLOCK(:)
  144. DOUBLE PRECISION, ALLOCATABLE :: A(:), AC(:), AD(:), AE(:), AK(:),
  145. $ AKB(:), B(:), BC(:), BD(:),
  146. $ BE(:), BK(:), BKB(:), C(:),
  147. $ CC(:), CD(:), CE(:), CK(:),
  148. $ CKB(:), D(:), DC(:), DD(:),
  149. $ DE(:), DK(:), DKB(:), DWORK(:),
  150. $ MJU(:), OMEGA(:), PMJU(:),
  151. $ RITYPE(:), RNBLCK(:)
  152. COMPLEX*16, ALLOCATABLE :: ZWORK(:)
  153. C
  154. C .. Local variables and constant dimension arrays ..
  155. CHARACTER CONJOB
  156. CHARACTER*120 TEXT
  157. INTEGER DISCR, I, IP, ITER, ITERB, LD1, LD2, LD3, LD4,
  158. $ LI1, LI2, LI3, LI4, LW1, LW2, LW3, LW4, LW5,
  159. $ LW6, LW7, LWA, LWB, LZD, LZM, M1, M11, MD, MIT,
  160. $ MN, NA, NB, NC, ND, NIT, NNB, NP11
  161. DOUBLE PRECISION PMPEAK, MUPEAK, TOL
  162. C
  163. C .. External functions ..
  164. DOUBLE PRECISION DLAMCH
  165. EXTERNAL DLAMCH
  166. C
  167. C .. External subroutines ..
  168. EXTERNAL AB04MD, AB05MD, AB07ND, DCOPY, SB10AD, SB10MD
  169. C
  170. C ..Intrinsic functions..
  171. INTRINSIC ABS, COS, INT, MAX, MIN, SQRT
  172. C
  173. C Check for proper number of arguments.
  174. C
  175. IF ( NRHS.EQ.0 ) THEN
  176. CALL mexErrMsgTxt( 'Matlab call: [AK,BK,CK,DK,MJU,RCOND]='//
  177. $ 'MUHOPT(JOB,DISCR,A,B,C,D,NCON,NMEAS,GAMMA,OMEGA,NBLOCK,ITYPE,'//
  178. $ 'ORD[,QUTOL[,GTOL[,ACTOL]]]) or [AK,BK,CK,DK,GAMMIN,RCOND]='//
  179. $ 'MUHOPT(JOB,DISCR,A,B,C,D,NCON,NMEAS,GAMMA[,GTOL[,ACTOL]])')
  180. ELSE IF ( NRHS.LT.9 ) THEN
  181. CALL mexErrMsgTxt
  182. $ ( 'MUHOPT requires at least 9 input arguments' )
  183. ELSE IF ( NRHS.GT.16 ) THEN
  184. CALL mexErrMsgTxt
  185. $ ( 'MUHOPT requires at most 16 input arguments' )
  186. ELSE IF ( NLHS.LT.4 ) THEN
  187. CALL mexErrMsgTxt
  188. $ ( 'MUHOPT requires at least 4 output arguments' )
  189. END IF
  190. C
  191. C job
  192. C
  193. IF ( mxGetM( PRHS(1) ).NE.1 .OR. mxGetN( PRHS(1) ).NE.1 ) THEN
  194. CALL mexErrMsgTxt( 'JOB must be a scalar' )
  195. ELSE IF ( mxIsNumeric( PRHS(1) ).EQ.0 .OR.
  196. $ mxIsComplex( PRHS(1) ).EQ.1 ) THEN
  197. CALL mexErrMsgTxt( 'JOB must be an integer scalar' )
  198. END IF
  199. C
  200. CALL mxCopyPtrToReal8( mxGetPr( PRHS(1) ), TEMP, 1 )
  201. JOB = INT( TEMP )
  202. C
  203. IF ( ABS( JOB ).GT.3 ) THEN
  204. CALL mexErrMsgTxt( 'JOB has -3, -2, -1, 0, 1, 2, or 3 '//
  205. $ 'the only admissible values' )
  206. END IF
  207. C
  208. C Determine the job.
  209. C
  210. IF ( JOB.GT.0 ) THEN
  211. C mu controller desired.
  212. CONJOB = 'M'
  213. ELSE
  214. C H_inf controller desired.
  215. CONJOB = 'H'
  216. IF ( JOB.EQ.0 ) THEN
  217. JOB = 4
  218. ELSE
  219. JOB = ABS( JOB )
  220. END IF
  221. END IF
  222. C
  223. C Recheck for proper number of arguments.
  224. C
  225. IF ( CONJOB.EQ.'M' .AND. NRHS.LT.13 ) THEN
  226. CALL mexErrMsgTxt( 'MUHOPT requires at least 13 input '//
  227. $ 'arguments if mu optimal controller is desired')
  228. ELSE IF ( CONJOB.EQ.'H' .AND. NRHS.GT.11 ) THEN
  229. CALL mexErrMsgTxt( 'MUHOPT requires at most 11 input '//
  230. $ 'arguments if H_inf optimal controller is desired')
  231. END IF
  232. C
  233. C discr
  234. C
  235. IF ( mxGetM( PRHS(2) ).NE.1 .OR. mxGetN( PRHS(2) ).NE.1 ) THEN
  236. CALL mexErrMsgTxt( 'DISCR must be a scalar' )
  237. ELSE IF ( mxIsNumeric( PRHS(2) ).EQ.0 .OR.
  238. $ mxIsComplex( PRHS(2) ).EQ.1 ) THEN
  239. CALL mexErrMsgTxt( 'DISCR must be an integer scalar' )
  240. END IF
  241. C
  242. CALL mxCopyPtrToReal8( mxGetPr( PRHS(2) ), TEMP, 1 )
  243. DISCR = INT( TEMP )
  244. C
  245. IF ( DISCR.LT.0 .OR. DISCR.GT.1 ) THEN
  246. CALL mexErrMsgTxt( 'DISCR must be 0 or 1' )
  247. END IF
  248. C
  249. C A, B, C, D
  250. C
  251. IF ( mxIsNumeric( PRHS(3) ).EQ.0 .OR.
  252. $ mxIsComplex( PRHS(3) ).EQ.1 ) THEN
  253. CALL mexErrMsgTxt( 'A must be a numeric matrix' )
  254. END IF
  255. C
  256. N = mxGetM( PRHS(3) )
  257. NA = mxGetN( PRHS(3) )
  258. C
  259. IF ( NA.NE.N ) THEN
  260. CALL mexErrMsgTxt( 'A must be a square matrix' )
  261. END IF
  262. C
  263. IF ( mxIsNumeric( PRHS(4) ).EQ.0 .OR.
  264. $ mxIsComplex( PRHS(4) ).EQ.1 ) THEN
  265. CALL mexErrMsgTxt( 'B must be a numeric matrix' )
  266. END IF
  267. C
  268. NB = mxGetM( PRHS(4) )
  269. M = mxGetN( PRHS(4) )
  270. C
  271. IF ( NB.NE.N ) THEN
  272. CALL mexErrMsgTxt( 'B must have the same row dimension as A' )
  273. END IF
  274. C
  275. IF ( mxIsNumeric( PRHS(5) ).EQ.0 .OR.
  276. $ mxIsComplex( PRHS(5) ).EQ.1 ) THEN
  277. CALL mexErrMsgTxt( 'C must be a numeric matrix' )
  278. END IF
  279. C
  280. NP = mxGetM( PRHS(5) )
  281. NC = mxGetN( PRHS(5) )
  282. C
  283. IF ( NC.NE.N ) THEN
  284. CALL mexErrMsgTxt
  285. $ ( 'C must have the same column dimension as A' )
  286. END IF
  287. C
  288. IF ( mxIsNumeric( PRHS(6) ).EQ.0 .OR.
  289. $ mxIsComplex( PRHS(6) ).EQ.1 ) THEN
  290. CALL mexErrMsgTxt( 'D must be a numeric matrix' )
  291. END IF
  292. C
  293. ND = mxGetM( PRHS(6) )
  294. MD = mxGetN( PRHS(6) )
  295. C
  296. IF ( ND.NE.NP ) THEN
  297. CALL mexErrMsgTxt( 'D must have the same row dimension as C' )
  298. ELSE IF ( MD.NE.M ) THEN
  299. CALL mexErrMsgTxt
  300. $ ( 'D must have the same column dimension as B' )
  301. END IF
  302. C
  303. C ncon (M2), nmeas (NP2)
  304. C
  305. IF ( mxGetM( PRHS(7) ).NE.1 .OR. mxGetN( PRHS(7) ).NE.1 ) THEN
  306. CALL mexErrMsgTxt( 'NCON must be a scalar' )
  307. ELSE IF ( mxIsNumeric( PRHS(7) ).EQ.0 .OR.
  308. $ mxIsComplex( PRHS(7) ).EQ.1 ) THEN
  309. CALL mexErrMsgTxt( 'NCON must be an integer scalar' )
  310. END IF
  311. C
  312. CALL mxCopyPtrToReal8( mxGetPr( PRHS(7) ), TEMP, 1 )
  313. M2 = INT( TEMP )
  314. C
  315. IF ( mxGetM( PRHS(8) ).NE.1 .OR. mxGetN( PRHS(8) ).NE.1 ) THEN
  316. CALL mexErrMsgTxt( 'NMEAS must be a scalar' )
  317. ELSE IF ( mxIsNumeric( PRHS(8) ).EQ.0 .OR.
  318. $ mxIsComplex( PRHS(8) ).EQ.1 ) THEN
  319. CALL mexErrMsgTxt( 'NMEAS must be an integer scalar' )
  320. END IF
  321. C
  322. CALL mxCopyPtrToReal8( mxGetPr( PRHS(8) ), TEMP, 1 )
  323. NP2 = INT( TEMP )
  324. M1 = M - M2
  325. NP1 = NP - NP2
  326. C
  327. IF ( M1.NE.NP1 ) THEN
  328. CALL mexErrMsgTxt( 'M - NCON must be equal to P - NMEAS' )
  329. END IF
  330. C
  331. C gamma
  332. C
  333. IF ( mxGetM( PRHS(9) ).NE.1 .OR. mxGetN( PRHS(9) ).NE.1 ) THEN
  334. CALL mexErrMsgTxt( 'GAMMA must be a scalar' )
  335. ELSE IF ( mxIsNumeric( PRHS(9) ).EQ.0 .OR.
  336. $ mxIsComplex( PRHS(9) ).EQ.1 ) THEN
  337. CALL mexErrMsgTxt( 'GAMMA must be a real scalar' )
  338. END IF
  339. C
  340. CALL mxCopyPtrToReal8( mxGetPr( PRHS(9) ), GAMMA, 1 )
  341. C
  342. IF ( CONJOB.EQ.'M' ) THEN
  343. C
  344. C The mu controller case.
  345. C
  346. C omega
  347. C
  348. IF ( mxIsNumeric( PRHS(10) ).EQ.0 .OR.
  349. $ mxIsComplex( PRHS(10) ).EQ.1 ) THEN
  350. CALL mexErrMsgTxt( 'OMEGA must be a numeric vector' )
  351. ELSE IF ( MIN( mxGetM( PRHS(10) ), mxGetN( PRHS(10) ) ).NE.1 )
  352. $ THEN
  353. CALL mexErrMsgTxt( 'OMEGA must be a vector' )
  354. END IF
  355. C
  356. LENDAT = mxGetM( PRHS(10) )*mxGetN( PRHS(10) )
  357. IF ( LENDAT.LE.1 ) THEN
  358. CALL mexErrMsgTxt( 'OMEGA must have at least 2 elements' )
  359. END IF
  360. C
  361. C nblock
  362. C
  363. IF ( mxIsNumeric( PRHS(11) ).EQ.0 .OR.
  364. $ mxIsComplex( PRHS(11) ).EQ.1 ) THEN
  365. CALL mexErrMsgTxt( 'NBLOCK must be a numeric vector' )
  366. END IF
  367. C
  368. MNB = mxGetM( PRHS(11) )
  369. NNB = mxGetN( PRHS(11) )
  370. MN = MIN( MNB, NNB )
  371. MNB = MAX( MNB, NNB )
  372. NNB = MN
  373. IF ( NNB.NE.1 ) THEN
  374. CALL mexErrMsgTxt( 'NBLOCK must be a vector' )
  375. END IF
  376. C
  377. C itype
  378. C
  379. IF ( mxIsNumeric( PRHS(12) ).EQ.0 .OR.
  380. $ mxIsComplex( PRHS(12) ).EQ.1 ) THEN
  381. CALL mexErrMsgTxt( 'ITYPE must be a numeric vector' )
  382. END IF
  383. C
  384. MIT = mxGetM( PRHS(12) )
  385. NIT = mxGetN( PRHS(12) )
  386. MN = MIN( MIT, NIT )
  387. MIT = MAX( MIT, NIT )
  388. NIT = MN
  389. C
  390. IF ( NIT.NE.1 ) THEN
  391. CALL mexErrMsgTxt( 'ITYPE must be a vector' )
  392. ELSE IF ( MNB.NE.MIT ) THEN
  393. CALL mexErrMsgTxt
  394. $ ( 'ITYPE must have the same size as NBLOCK' )
  395. END IF
  396. C
  397. C ord
  398. C
  399. IF ( mxGetM( PRHS(13) ).NE.1 .OR.
  400. $ mxGetN( PRHS(13) ).NE.1 ) THEN
  401. CALL mexErrMsgTxt( 'ORD must be a scalar' )
  402. ELSE IF ( mxIsNumeric( PRHS(13) ).EQ.0 .OR.
  403. $ mxIsComplex( PRHS(13) ).EQ.1 ) THEN
  404. CALL mexErrMsgTxt( 'ORD must be an integer scalar' )
  405. END IF
  406. C
  407. CALL mxCopyPtrToReal8( mxGetPr( PRHS(13) ), TEMP, 1 )
  408. ORD = INT( TEMP )
  409. C
  410. IF ( ORD.LT.1 ) THEN
  411. CALL mexErrMsgTxt( 'ORD must be at least 1' )
  412. ELSE IF ( ORD.GE.LENDAT ) THEN
  413. WRITE( TEXT, '(''ORD must be less than LENDAT - 1 = '',
  414. $ I7)' ) LENDAT - 1
  415. CALL mexErrMsgTxt( TEXT )
  416. END IF
  417. C
  418. C qutol
  419. C
  420. IF ( NRHS.GE.14 ) THEN
  421. IF ( mxGetM( PRHS(14) ).NE.1 .OR.
  422. $ mxGetN( PRHS(14) ).NE.1 ) THEN
  423. CALL mexErrMsgTxt( 'QUTOL must be a scalar' )
  424. ELSE IF ( mxIsNumeric( PRHS(14) ).EQ.0 .OR.
  425. $ mxIsComplex( PRHS(14) ).EQ.1 ) THEN
  426. CALL mexErrMsgTxt( 'QUTOL must be a real scalar' )
  427. END IF
  428. CALL mxCopyPtrToReal8( mxGetPr( PRHS(14) ), QUTOL, 1 )
  429. ELSE
  430. QUTOL = TWO
  431. END IF
  432. IP = 15
  433. ELSE
  434. IP = 10
  435. END IF
  436. C
  437. C gtol
  438. C
  439. IF ( NRHS.GE.IP ) THEN
  440. IF ( mxGetM( PRHS(IP) ).NE.1 .OR.
  441. $ mxGetN( PRHS(IP) ).NE.1 ) THEN
  442. CALL mexErrMsgTxt( 'GTOL must be a scalar' )
  443. ELSE IF ( mxIsNumeric( PRHS(IP) ).EQ.0 .OR.
  444. $ mxIsComplex( PRHS(IP) ).EQ.1 ) THEN
  445. CALL mexErrMsgTxt( 'GTOL must be a real scalar' )
  446. END IF
  447. CALL mxCopyPtrToReal8( mxGetPr( PRHS(IP) ), GTOL, 1 )
  448. IP = IP + 1
  449. ELSE
  450. GTOL = P01
  451. END IF
  452. C
  453. C actol
  454. C
  455. IF ( NRHS.EQ.IP ) THEN
  456. IF ( mxGetM( PRHS(IP) ).NE.1 .OR.
  457. $ mxGetN( PRHS(IP) ).NE.1 ) THEN
  458. CALL mexErrMsgTxt( 'ACTOL must be a scalar' )
  459. ELSE IF ( mxIsNumeric( PRHS(IP) ).EQ.0 .OR.
  460. $ mxIsComplex( PRHS(IP) ).EQ.1 ) THEN
  461. CALL mexErrMsgTxt( 'ACTOL must be a real scalar' )
  462. END IF
  463. CALL mxCopyPtrToReal8( mxGetPr(PRHS(IP) ), ACTOL, 1 )
  464. ELSE
  465. ACTOL = ZERO
  466. END IF
  467. C
  468. C Set the default tolerance.
  469. C
  470. TOL = SQRT( DLAMCH( 'Epsilon' ) )
  471. C
  472. C Determine the lenghts of working arrays.
  473. C
  474. C The original system.
  475. C
  476. LDA = MAX( 1, N )
  477. LDB = LDA
  478. LDC = MAX( 1, NP )
  479. LDD = LDC
  480. C
  481. IF ( CONJOB.EQ.'M' ) THEN
  482. C
  483. C The scaling system.
  484. C
  485. TOTORD = NP1*ORD
  486. C
  487. F = MAX( M2, NP2 )
  488. LDAD = MAX( 1, TOTORD )
  489. LDBD = LDAD
  490. LDCD = MAX( 1, NP1 + F )
  491. LDDD = LDCD
  492. C
  493. C The extended system.
  494. C
  495. NE = N + 2*TOTORD
  496. LDAE = MAX( 1, NE )
  497. LDBE = LDAE
  498. LDCE = LDCD
  499. LDDE = LDCE
  500. ELSE
  501. NE = N
  502. LDAE = LDA
  503. END IF
  504. C
  505. C The closed-loop system.
  506. C
  507. N2E = 2*NE
  508. LDAC = MAX( 1, N2E )
  509. LDBC = LDAC
  510. LDCC = MAX( 1, NP1 )
  511. LDDC = LDCC
  512. C
  513. C The controller.
  514. C
  515. LDAK = LDAE
  516. LDBK = LDAK
  517. LDCK = MAX( 1, M2 )
  518. LDDK = LDCK
  519. C
  520. C LBWORK
  521. C
  522. C ..SB10AD..
  523. LBWORK = N2E
  524. C
  525. C LIWORK
  526. C
  527. C ..SB10AD..
  528. LI1 = MAX( 2*MAX( NE, M - M2, NP - NP2, M2, NP2 ), NE*NE )
  529. C
  530. IF ( CONJOB.EQ.'M' ) THEN
  531. C ..SB10MD..
  532. LI2 = MAX( N2E, 4*MNB - 2, NP1 )
  533. IF ( QUTOL.GE.ZERO )
  534. $ LI2 = MAX( LI2, 2*ORD + 1 )
  535. C ..AB07ND..
  536. LI3 = 2*M
  537. ELSE
  538. LI2 = 0
  539. LI3 = 0
  540. END IF
  541. C
  542. C ..AB04MD..
  543. LI4 = NE
  544. C
  545. LIWORK = MAX( LI1, LI2, LI3, LI4 )
  546. C
  547. C LDWORK
  548. C
  549. C ..AB04MD..
  550. LD1 = LDAK
  551. C
  552. C ..SB10AD..
  553. NP11 = NP1 - M2
  554. M11 = M1 - NP2
  555. LW1 = NE*M + NP*NE + NP*M + M2*M2 + NP2*NP2
  556. LW2 = MAX( ( NE + NP1 + 1 )*( NE + M2 ) +
  557. $ MAX( 3*( NE + M2 ) + NE + NP1, 5*( NE + M2 ) ),
  558. $ ( NE + NP2 )*( NE + M1 + 1 ) +
  559. $ MAX( 3*( NE + NP2 ) + NE + M1, 5*( NE + NP2 ) ),
  560. $ M2 + NP1*NP1 + MAX( NP1*MAX( NE, M1 ), 3*M2 + NP1,
  561. $ 5*M2 ),
  562. $ NP2 + M1*M1 + MAX( MAX( NE, NP1 )*M1, 3*NP2 + M1,
  563. $ 5*NP2 ) )
  564. LW3 = MAX( NP11*M1 + MAX( 4*MIN( NP11, M1 ) + MAX( NP11, M1 ),
  565. $ 6*MIN( NP11, M1 ) ),
  566. $ NP1*M11 + MAX( 4*MIN( NP1, M11 ) + MAX( NP1, M11 ),
  567. $ 6*MIN( NP1, M11 ) ) )
  568. LW4 = 2*M*M + NP*NP + 2*M*NE + M*NP + 2*NE*NP
  569. LW5 = 2*NE*NE + M*NE + NE*NP
  570. LW6 = MAX( M*M + MAX( 2*M1, 3*NE*NE +
  571. $ MAX( NE*M, 10*NE*NE + 12*NE + 5 ) ),
  572. $ NP*NP + MAX( 2*NP1, 3*NE*NE +
  573. $ MAX( NE*NP, 10*NE*NE + 12*NE + 5 ) ) )
  574. LW7 = M2*NP2 + NP2*NP2 + M2*M2 +
  575. $ MAX( NP11*NP11 + MAX( 2*NP11, ( NP11 + M11 )*NP2 ),
  576. $ M11*M11 + MAX( 2*M11, M11*M2 ), 3*NE,
  577. $ NE*( 2*NP2 + M2 ) +
  578. $ MAX( 2*NE*M2, M2*NP2 +
  579. $ MAX( M2*M2 + 3*M2, NP2*( 2*NP2 +
  580. $ M2 + MAX( NP2, NE ) ) ) ) )
  581. LD2 = LW1 + MAX( 1, LW2, LW3, LW4, LW5 + MAX( LW6, LW7 ) )
  582. C
  583. IF ( CONJOB.EQ.'M' ) THEN
  584. C
  585. C ..SB10MD..
  586. MN = MIN( 2*LENDAT, 2*ORD + 1 )
  587. LWA = NP1*LENDAT + 2*MNB + NP1 - 1
  588. LWB = LENDAT*( NP1 + 2 ) + ORD*( ORD + 2 ) + 1
  589. LW1 = 2*LENDAT + 4*HNPTS
  590. LW2 = LENDAT + 6*HNPTS
  591. LW3 = 2*LENDAT*( 2*ORD + 1 ) + MAX( 2*LENDAT, 2*ORD + 1 ) +
  592. $ MAX( MN + 6*ORD + 4, 2*MN + 1 )
  593. LW4 = MAX( ORD*ORD + 5*ORD, 6*ORD + 1 + MIN( 1, ORD ) )
  594. C
  595. LD3 = LWA + MAX( N2E + MAX( N2E, NP1 - 1 ),
  596. $ 2*NP1*NP1*MNB - NP1*NP1 + 9*MNB*MNB +
  597. $ NP1*MNB + 11*NP1 + 33*MNB - 11 )
  598. IF ( QUTOL.GE.ZERO ) THEN
  599. LD4 = LWB + MAX( LW1, LW2, LW3, LW4 )
  600. ELSE
  601. LD4 = 0
  602. END IF
  603. C .. AB05MD..
  604. LD4 = MAX( LD4, MAX( M, NP )*MAX( N + TOTORD, M, NP ) )
  605. C .. AB07ND..
  606. LD4 = MAX( LD4, 4*M )
  607. ELSE
  608. LD3 = 0
  609. LD4 = 0
  610. END IF
  611. C
  612. LDWORK = MAX( LD1, LD2, LD3, LD4 )
  613. C
  614. C LZWORK
  615. C
  616. IF ( CONJOB.EQ.'M' ) THEN
  617. C ..SB10MD..
  618. LZM = MAX( NP1*NP1 + N2E*NP1 + N2E*N2E + 2*N2E,
  619. $ 6*NP1*NP1*MNB + 13*NP1*NP1 + 6*MNB + 6*NP1 - 3 )
  620. IF ( QUTOL.GE.ZERO ) THEN
  621. LZD = MAX( LENDAT*( 2*ORD + 3 ), ORD*ORD + 3*ORD + 1 )
  622. ELSE
  623. LZD = 0
  624. END IF
  625. LZWORK = MAX( LZM, LZD )
  626. END IF
  627. C
  628. C Allocate variable dimension local arrays.
  629. C !Fortran 90/95
  630. C
  631. ALLOCATE ( A(LDA*N), AC(LDAC*N2E), AK(LDAK*NE),
  632. $ B(LDB*M), BC(LDBC*M1), BK(LDBK*NP2), BWORK(LBWORK),
  633. $ C(LDC*N), CC(LDCC*N2E), CK(LDCK*NE),
  634. $ D(LDD*M), DC(LDDC*M1), DK(LDDK*NP2), DWORK(LDWORK),
  635. $ IWORK(LIWORK) )
  636. C
  637. IF ( CONJOB.EQ.'M' ) THEN
  638. ALLOCATE ( AD(LDAD*TOTORD), AE(LDAE*NE), AKB(LDAK*NE),
  639. $ BD(LDBD*(M1+F)), BE(LDBE*(M1+F)), BKB(LDBK*NP2),
  640. $ CD(LDCD*TOTORD), CE(LDCE*NE), CKB(LDCK*NE),
  641. $ DD(LDDD*(M1+F)), DE(LDDE*(M1+F)), DKB(LDDK*NP2),
  642. $ ITYPE(MNB), MJU(LENDAT), NBLOCK(MNB), OMEGA(LENDAT),
  643. $ PMJU(LENDAT), RITYPE(MNB), RNBLCK(MNB),
  644. $ ZWORK(LZWORK) )
  645. END IF
  646. C
  647. C Copy right hand side arguments to local arrays.
  648. C
  649. CALL mxCopyPtrToReal8( mxGetPr( PRHS(3) ), A, N*N )
  650. CALL mxCopyPtrToReal8( mxGetPr( PRHS(4) ), B, N*M )
  651. CALL mxCopyPtrToReal8( mxGetPr( PRHS(5) ), C, NP*N )
  652. CALL mxCopyPtrToReal8( mxGetPr( PRHS(6) ), D, NP*M )
  653. C
  654. IF ( CONJOB.EQ.'M' ) THEN
  655. CALL mxCopyPtrToReal8( mxGetPr( PRHS(10) ), OMEGA, LENDAT )
  656. CALL mxCopyPtrToReal8( mxGetPr( PRHS(11) ), RNBLCK, MNB )
  657. CALL mxCopyPtrToReal8( mxGetPr( PRHS(12) ), RITYPE, MNB )
  658. C
  659. DO 10 I = 1, MNB
  660. NBLOCK(I) = INT( RNBLCK(I) )
  661. ITYPE(I) = INT( RITYPE(I) )
  662. 10 CONTINUE
  663. END IF
  664. C
  665. C Do the actual computations.
  666. C
  667. C Transform to continuous-case, if needed.
  668. C
  669. IF ( DISCR.EQ.1 ) THEN
  670. CALL AB04MD( 'D', N, M, NP, ONE, ONE, A, LDA, B, LDB, C, LDC,
  671. $ D, LDD, IWORK, DWORK, LDWORK, INFO )
  672. C
  673. IF ( INFO.NE.0 ) THEN
  674. WRITE( TEXT, '('' INFO = '',I4,'' ON EXIT FROM AB04MD'')' )
  675. $ INFO
  676. GOTO 60
  677. END IF
  678. C
  679. IF ( CONJOB.EQ.'M' ) THEN
  680. C
  681. DO 20 I = 1, LENDAT
  682. OMEGA(I) = SQRT( ( ONE - COS( OMEGA(I) ) ) /
  683. $ ( ONE + COS( OMEGA(I) ) + TOL ) )
  684. 20 CONTINUE
  685. C
  686. END IF
  687. C
  688. END IF
  689. C
  690. C Set parameters for the first K step.
  691. C
  692. N2E = 2*N
  693. NE = N
  694. NEB = N
  695. ITER = 1
  696. C
  697. C First K step - makes use of the original system.
  698. C
  699. CALL SB10AD( JOB, N, M, NP, M2, NP2, GAMMA, A, LDA, B, LDB, C,
  700. $ LDC, D, LDD, AK, LDA, BK, LDB, CK, LDCK, DK, LDDK,
  701. $ AC, LDAC, BC, LDBC, CC, LDCC, DC, LDDC, RCOND, GTOL,
  702. $ ACTOL, IWORK, LIWORK, DWORK, LDWORK, BWORK, LBWORK,
  703. $ INFO )
  704. C
  705. IF ( INFO.NE.0 ) THEN
  706. WRITE( TEXT, '('' INFO = '',I4,'' ON EXIT FROM SB10AD'')' )
  707. $ INFO
  708. GOTO 60
  709. END IF
  710. C
  711. C Skip the D step if H_inf controller only desired.
  712. C
  713. IF ( CONJOB.EQ.'H' )
  714. $ GOTO 50
  715. C
  716. PMPEAK = GAMMA + P01
  717. C
  718. C Start the iteration process
  719. C --------- D step -------------------------------------------------
  720. C
  721. 30 CONTINUE
  722. C
  723. CALL SB10MD( N2E, NP1, LENDAT, F, ORD, MNB, NBLOCK, ITYPE,
  724. $ QUTOL, AC, LDAC, BC, LDBC, CC, LDCC, DC, LDDC,
  725. $ OMEGA, TOTORD, AD, LDAD, BD, LDBD, CD, LDCD, DD,
  726. $ LDDD, MJU, IWORK, LIWORK, DWORK, LDWORK, ZWORK,
  727. $ LZWORK, INFO )
  728. C
  729. IF ( INFO.NE.0 ) THEN
  730. WRITE( TEXT, '('' INFO = '',I4,'' ON EXIT FROM SB10MD'')' )
  731. $ INFO
  732. GOTO 60
  733. END IF
  734. C
  735. C Check mu.
  736. C
  737. MUPEAK = ZERO
  738. C
  739. DO 40 I = 1, LENDAT
  740. MUPEAK = MAX( MUPEAK, MJU(I) )
  741. 40 CONTINUE
  742. C
  743. IF ( MUPEAK.GT.PMPEAK ) THEN
  744. IF ( ITER.NE.1 )
  745. $ CALL DCOPY( LENDAT, PMJU, 1, MJU, 1 )
  746. GOTO 50
  747. ELSE
  748. C
  749. C Save the best controller.
  750. C
  751. PMPEAK = MUPEAK
  752. ITERB = ITER
  753. NEB = NE
  754. C
  755. CALL DCOPY( LENDAT, MJU, 1, PMJU, 1 )
  756. IF ( ITER.NE.1 ) THEN
  757. CALL DCOPY( NE*NE, AKB, 1, AK, 1 )
  758. CALL DCOPY( NE*NP2, BKB, 1, BK, 1 )
  759. CALL DCOPY( M2*NE, CKB, 1, CK, 1 )
  760. CALL DCOPY( M2*NP2, DKB, 1, DK, 1 )
  761. END IF
  762. END IF
  763. C
  764. ITER = ITER + 1
  765. IF ( ITER.GT.MAXIT )
  766. $ GOTO 50
  767. C
  768. C Dl*P.
  769. C
  770. CALL AB05MD( 'U', 'N', N, M, NP, TOTORD, NP, A, LDA, B, LDB,
  771. $ C, LDC, D, LDD, AD, LDAD, BD, LDBD, CD, LDCD, DD,
  772. $ LDDD, NTEMP, AE, LDAE, BE, LDBE, CE, LDCE, DE,
  773. $ LDDE, DWORK, LDWORK, INFO )
  774. C
  775. C inv(Dr).
  776. C
  777. CALL AB07ND( TOTORD, M, AD, LDAD, BD, LDBD, CD, LDCD, DD, LDDD,
  778. $ TEMP, IWORK, DWORK, LDWORK, INFO )
  779. C
  780. IF ( INFO.NE.0 ) THEN
  781. WRITE( TEXT, '('' INFO = '',I4,'' ON EXIT FROM AB07ND'')' )
  782. $ INFO
  783. GOTO 60
  784. END IF
  785. C
  786. C Dl*P*inv(Dr).
  787. C
  788. CALL AB05MD( 'U', 'O', TOTORD, M, M, NTEMP, NP, AD, LDAD, BD,
  789. $ LDBD, CD, LDCD, DD, LDDD, AE, LDAE, BE, LDBE,
  790. $ CE, LDCE, DE, LDDE, NE, AE, LDAE, BE, LDBE, CE,
  791. $ LDCE, DE, LDDE, DWORK, LDWORK, INFO )
  792. N2E = 2*NE
  793. LDAK = MAX( 1, NE )
  794. LDBK = LDAK
  795. C
  796. C --------- K step ----------------------------------------------
  797. C
  798. CALL SB10AD( JOB, NE, M, NP, M2, NP2, GAMMA, AE, LDAE, BE,
  799. $ LDBE, CE, LDCE, DE, LDDE, AKB, LDAK, BKB, LDBK,
  800. $ CKB, LDCK, DKB, LDDK, AC, LDAC, BC, LDBC, CC,
  801. $ LDCC, DC, LDDC, RCOND( 4*ITER-3 ), GTOL, ACTOL,
  802. $ IWORK, LIWORK, DWORK, LDWORK, BWORK, LBWORK,
  803. $ INFO )
  804. C
  805. IF ( INFO.NE.0 ) THEN
  806. INFO = 0
  807. GOTO 50
  808. END IF
  809. C
  810. GOTO 30
  811. C
  812. C ---- End of the mu synthesis -------------------------------------
  813. C
  814. 50 CONTINUE
  815. C
  816. C Transform back to discrete time if needed.
  817. C
  818. IF ( DISCR.EQ.1 ) THEN
  819. CALL AB04MD( 'C', NEB, NP2, M2, ONE, ONE, AK, LDAK, BK, LDBK,
  820. $ CK, LDCK, DK, LDDK, IWORK, DWORK, LDWORK, INFO )
  821. C
  822. IF ( INFO.NE.0 ) THEN
  823. WRITE( TEXT, '('' INFO = '',I4,'' ON EXIT FROM AB04MD'')' )
  824. $ INFO
  825. GOTO 60
  826. END IF
  827. END IF
  828. C
  829. C Copy output to MATLAB workspace.
  830. C
  831. PLHS(1) = mxCreateDoubleMatrix( NEB, NEB, 0 )
  832. PLHS(2) = mxCreateDoubleMatrix( NEB, NP2, 0 )
  833. PLHS(3) = mxCreateDoubleMatrix( M2, NEB, 0 )
  834. PLHS(4) = mxCreateDoubleMatrix( M2, NP2, 0 )
  835. CALL mxCopyReal8ToPtr( AK, mxGetPr( PLHS(1) ), NEB*NEB )
  836. CALL mxCopyReal8ToPtr( BK, mxGetPr( PLHS(2) ), NEB*NP2 )
  837. CALL mxCopyReal8ToPtr( CK, mxGetPr( PLHS(3) ), M2*NEB )
  838. CALL mxCopyReal8ToPtr( DK, mxGetPr( PLHS(4) ), M2*NP2 )
  839. C
  840. IF ( NLHS.GE.5 ) THEN
  841. IF ( CONJOB.EQ.'M' ) THEN
  842. C mu
  843. PLHS(5) = mxCreateDoubleMatrix( LENDAT, 1, 0 )
  844. CALL mxCopyReal8ToPtr( MJU, mxGetPr( PLHS(5) ), LENDAT )
  845. IF ( NLHS.GE.6 ) THEN
  846. PLHS(6) = mxCreateDoubleMatrix( ITERB*4, 1, 0 )
  847. CALL mxCopyReal8ToPtr( RCOND, mxGetPr( PLHS(6) ),
  848. $ ITERB*4 )
  849. END IF
  850. ELSE
  851. C H_inf
  852. PLHS(5) = mxCreateDoubleMatrix( 1, 1, 0 )
  853. CALL mxCopyReal8ToPtr( GAMMA, mxGetPr( PLHS(5) ), 1 )
  854. IF ( NLHS.GE.6 ) THEN
  855. PLHS(6) = mxCreateDoubleMatrix( 4, 1, 0 )
  856. CALL mxCopyReal8ToPtr( RCOND, mxGetPr( PLHS(6) ), 4 )
  857. END IF
  858. END IF
  859. END IF
  860. C
  861. C Deallocate local arrays.
  862. C !Fortran 90/95
  863. C
  864. 60 CONTINUE
  865. C
  866. DEALLOCATE ( A, AC, AK, B, BC, BK, BWORK, C, CC, CK, D, DC, DK,
  867. $ DWORK, IWORK )
  868. C
  869. IF ( CONJOB.EQ.'M' )
  870. $ DEALLOCATE ( AD, AE, AKB, BD, BE, BKB, CD, CE, CKB, DD, DE,
  871. $ DKB, ITYPE, MJU, NBLOCK, OMEGA, PMJU, RITYPE,
  872. $ RNBLCK, ZWORK )
  873. C
  874. C Error and warning handling ..
  875. C
  876. IF ( INFO.NE.0 )
  877. $ CALL mexErrMsgTxt( TEXT )
  878. C
  879. RETURN
  880. C
  881. C *** Last line of MUHOPT ***
  882. END