← Back to branch summary

~ubuntu-branches/ubuntu/karmic/scilab/karmic

« back to all changes in this revision

Viewing changes to routines/control/sszer.f

  • Committer: Bazaar Package Importer
  • Author(s): Torsten Werner
  • Date: 2002-03-21 16:57:43 UTC
  • Revision ID: james.westby@ubuntu.com-20020321165743-e9mv12c1tb1plztg
Tags: upstream-2.6
Import upstream version 2.6

Show diffs side-by-side

added added

removed removed

Lines of Context:
routines/control/sszer.f
 
1
      subroutine sszer(n,m,p,a,na,b,c,nc,d,eps,zeror,zeroi,nu,irank,af,
 
2
     &                 naf,bf,mplusn,wrka,wrk1,nwrk1,wrk2,nwrk2,ierr)
 
3
C
 
4
C! calling sequence
 
5
C
 
6
C        subroutine sszer(n,m,p,a,na,b,c,nc,d,zeror,zeroi,nu,irank,
 
7
C     1       af,naf,bf,mplusn,wrka,wrk1,nwrk1,wrk2,nwrk2,ierr)
 
8
C
 
9
C        integer n,m,p,na,nc,nu,irank,nabf,mplusn,nwrk1,nwrk2,ierr
 
10
C
 
11
C        double precision a(na,n),b(na,m),c(nc,n),d(nc,m),wrka(na,n)
 
12
C        double precision af(naf,mplusn),bf(naf,mplusn)
 
13
C        double precision wrk1(nwrk1),wrk2(nwrk2)
 
14
C        double precision zeror(n),zeroi(n)
 
15
C
 
16
C arguments in
 
17
C
 
18
C        n       integer
 
19
C                -the number of state variables in the system
 
20
C
 
21
C        m       integer
 
22
C                -the number of inputs to the system
 
23
C
 
24
C        p       integer
 
25
C                -the number of outputs from the system
 
26
C
 
27
C        a       double precision (n,n)
 
28
C                -the state dynamics matrix of the system
 
29
C
 
30
C        na      integer
 
31
C                -the declared first dimension of matrices a and b
 
32
C
 
33
C        b       double precision (n,m)
 
34
C                -the  input/state  matrix of the system
 
35
C
 
36
C        c       double precision (p,n)
 
37
C                -the  state/output  matrix of the system
 
38
C
 
39
C        nc      integer
 
40
C                -the declared first dimension of matrices  c and d
 
41
C
 
42
C        d       double precision (p,m)
 
43
C                -the  input/output  matrix of the system
 
44
C
 
45
C        naf     integer
 
46
C                -the declared first dimension of matrices  af and bf
 
47
C                 naf must be at least  n + p
 
48
C
 
49
C        mplusn  integer
 
50
C                -the second dimension of  af and bf.  mplusn  must be
 
51
C                at least  m + n .
 
52
C
 
53
C        nwrk1   integer
 
54
C                -the length of work vector wrk1.
 
55
C                nwrk1  must be at least  max(m,p)
 
56
C
 
57
C        nwrk2   integer
 
58
C                -the length of work vector  wrk2.
 
59
C                nwrk2  must be at least  max(n,m,p)+1
 
60
C
 
61
C arguments out
 
62
C
 
63
C        nu      integer
 
64
C                -the number of (finite) invariant zeros
 
65
C
 
66
C        irank   integer
 
67
C                -the normal rank of the transfer function
 
68
C
 
69
C        zeror   double precision (n)
 
70
C        zeroi   double precision (n)
 
71
C                -the real  and imaginary parts of the zeros
 
72
C
 
73
C        af      double precision ( n+p , m+n )
 
74
C        bf      double precision ( n+p , m+n )
 
75
C                -the coefficient matrices of the reduced pencil
 
76
C
 
77
C        ierr    integer
 
78
C                -error indicator
 
79
C
 
80
C                ierr = 0        successful return
 
81
C
 
82
C                ierr = 1        incorrect dimensions of matrices
 
83
C
 
84
C                ierr = 2        attempt to divide by zero
 
85
C
 
86
C                ierr = i > 2    ierr value i-2 from qitz (eispack)
 
87
C
 
88
C!working space
 
89
C
 
90
C        wrka    double precision (na,n)
 
91
C
 
92
C        wrk1    double precision (nwrk1)
 
93
C
 
94
C        wrk2    double precision (nwrk2)
 
95
C
 
96
C!purpose
 
97
C
 
98
C        to compute the invariant zeros of a linear multivariable
 
99
C        system given in state space form.
 
100
C
 
101
C!method
 
102
C
 
103
C        this routine extracts from the system matrix of a state-space
 
104
C        system  a,b,c,d  a regular pencil   lambda * bf  -  af
 
105
C        which has the invariant zeros of the system as generalized
 
106
C        eigenvalues.
 
107
C
 
108
C!reference
 
109
C
 
110
C        emami-naeini, a. and van dooren, p.
 
111
C        'computation of zeros of linear multivariable systems'
 
112
C        report na-80-03, computer science department, stanford univ.
 
113
C
 
114
C!originator
 
115
C
 
116
C                a.emami-naeini, computer science department,
 
117
C                stanford university.
 
118
C     Copyrigth SLICE
 
119
C
 
120
      integer n,m,p,na,nc,nu,irank,naf,mplusn,nwrk1,nwrk2,ierr
 
121
C
 
122
      double precision a(na,n),b(na,m),c(nc,n),d(nc,m)
 
123
      double precision wrka(na,n),zeror(n),zeroi(n)
 
124
      double precision af(naf,mplusn),bf(naf,mplusn),wrk1(nwrk1),
 
125
     &                 wrk2(nwrk2)
 
126
      double precision eps,sum,heps,xxx(1,1)
 
127
C
 
128
C       local variables:
 
129
C
 
130
      logical zero,matq,matz
 
131
C
 
132
      integer mm,nn,pp,mu,iro,isigma,numu,mnu,numu1,mnu1,i,j,j1
 
133
      integer mj,ni,nu1
 
134
C
 
135
      double precision s
 
136
      ierr = 1
 
137
      if (na .lt. n) return
 
138
      if (nc .lt. p) return
 
139
      if (naf .lt. n+p) return
 
140
      if (nwrk1 .lt. m) return
 
141
      if (nwrk1 .lt. p) return
 
142
      if (nwrk2 .lt. n) return
 
143
      if (nwrk2 .lt. m) return
 
144
      if (nwrk2 .lt. p) return
 
145
      if (mplusn .lt. m+n) return
 
146
      ierr = 0
 
147
C       construct the compound matrix (b      a) of dimension
 
148
C                                     (d      c)
 
149
C       (n + p) * (m + n)
 
150
C
 
151
      sum = 0.0d+0
 
152
      do 30 i = 1,n
 
153
        do 10 j = 1,m
 
154
          bf(i,j) = b(i,j)
 
155
          sum = sum + (b(i,j)*b(i,j))
 
156
 10     continue
 
157
        do 30 j = 1,n
 
158
          mj = m + j
 
159
          bf(i,mj) = a(i,j)
 
160
          sum = sum + (a(i,j)*a(i,j))
 
161
 30   continue
 
162
C
 
163
      do 60 i = 1,p
 
164
        ni = n + i
 
165
        do 40 j = 1,m
 
166
          bf(ni,j) = d(i,j)
 
167
          sum = sum + (d(i,j)*d(i,j))
 
168
 40     continue
 
169
        do 60 j = 1,n
 
170
          mj = m + j
 
171
          bf(ni,mj) = c(i,j)
 
172
          sum = sum + (c(i,j)*c(i,j))
 
173
 60   continue
 
174
C
 
175
      heps = eps * sqrt(sum)
 
176
C
 
177
C       reduce this system to one with the same invariant zeros and with
 
178
C       d full row rank mu (the normal rank of the original system)
 
179
C
 
180
      iro = p
 
181
      isigma = 0
 
182
C
 
183
      call preduc(bf,naf,mplusn,m,n,p,heps,iro,isigma,mu,nu,wrk1,nwrk1,
 
184
     &            wrk2,nwrk2)
 
185
C
 
186
      irank = mu
 
187
      if (nu .eq. 0) return
 
188
C
 
189
C       pertranspose the system
 
190
C
 
191
      numu = nu + mu
 
192
      mnu = m + nu
 
193
      numu1 = numu + 1
 
194
      mnu1 = mnu + 1
 
195
      do 70 i = 1,numu
 
196
        ni = numu1 - i
 
197
        do 70 j = 1,mnu
 
198
          mj = mnu1 - j
 
199
          af(mj,ni) = bf(i,j)
 
200
 70   continue
 
201
C
 
202
      mm = m
 
203
      nn = n
 
204
      pp = p
 
205
      if (mu .eq. mm) goto 80
 
206
      pp = mm
 
207
      nn = nu
 
208
      mm = mu
 
209
C
 
210
C       reduce the system to one with the same invariant zeros and with
 
211
C       d square and of full rank
 
212
C
 
213
      iro = pp - mm
 
214
      isigma = mm
 
215
C
 
216
      call preduc(af,naf,mplusn,mm,nn,pp,heps,iro,isigma,mu,nu,wrk1,
 
217
     &            nwrk1,wrk2,nwrk2)
 
218
C
 
219
      if (nu .eq. 0) return
 
220
      mnu = mm + nu
 
221
 80   continue
 
222
      do 100 i = 1,nu
 
223
        ni = mm + i
 
224
        do 90 j = 1,mnu
 
225
          bf(i,j) = 0.0d+0
 
226
 90     continue
 
227
        bf(i,ni) = 1.0d+0
 
228
 100  continue
 
229
C
 
230
      if (irank .eq. 0) return
 
231
      nu1 = nu + 1
 
232
      numu = nu + mu
 
233
      j1 = mm
 
234
      do 120 i = 1,mm
 
235
        j1 = j1 - 1
 
236
        do 110 j = 1,nu1
 
237
          mj = j1 + j
 
238
          wrk2(j) = af(numu,mj)
 
239
 110    continue
 
240
C
 
241
        call house(wrk2,nu1,nu1,heps,zero,s)
 
242
        call tr2(af,naf,mplusn,wrk2,s,1,numu,j1,nu1)
 
243
        call tr2(bf,naf,mplusn,wrk2,s,1,nu,j1,nu1)
 
244
C
 
245
        numu = numu - 1
 
246
 120  continue
 
247
      matz = .false.
 
248
      matq = .false.
 
249
Cc
 
250
      call qhesz(naf,nu,af,bf,matq,xxx,matz,wrka)
 
251
      call qitz(naf,nu,af,bf,eps,matq,xxx,matz,wrka,ierr)
 
252
      if (ierr .ne. 0) goto 150
 
253
Cc
 
254
      call qvalz(naf,nu,af,bf,eps,zeror,zeroi,wrk2,matq,xxx,matz,wrka)
 
255
Cc
 
256
C         do 130 i = 1,nu
 
257
C            if (wrk2(i) .eq. 0.0d+0) go to 140
 
258
C            zeror(i) = zeror(i)/wrk2(i)
 
259
C            zeroi(i) = zeroi(i)/wrk2(i)
 
260
C  130       continue
 
261
Cc
 
262
Cc       successful completion
 
263
Cc
 
264
      ierr = 0
 
265
      return
 
266
Cc
 
267
Cc       attempt to divide by zero
 
268
Cc
 
269
C  140    ierr = 2
 
270
C         return
 
271
Cc
 
272
Cc       failure in subroutine qzit
 
273
Cc
 
274
 150  ierr = ierr + 2
 
275
      return
 
276
      end
 
277
        subroutine preduc(abf,naf,mplusn,m,n,p,heps,iro,isigma,mu,nu,
 
278
     1                    wrk1,nwrk1,wrk2,nwrk2)
 
279
c%calling sequence
 
280
c       subroutine preduc(abf,naf,mplusn,m,n,p,heps,iro,isigma,mu,nu,
 
281
c    1                    wrk1,nwrk1,wrk2,nwrk2)
 
282
c       integer naf,mplusn,m,n,p,iro,isigma,mu,nu,nwrk1,nwrk2
 
283
c       double precision abf(naf,mplusn),wrk1(nwrk1),wrk2(nwrk2)
 
284
c
 
285
c%purpose
 
286
c
 
287
c     this routine is only to be called from slice routine sszer
 
288
c%
 
289
        integer naf,mplusn,m,n,p,iro,isigma,mu,nu,nwrk1,nwrk2
 
290
c
 
291
        double precision abf(naf,mplusn),wrk1(nwrk1),wrk2(nwrk2)
 
292
c
 
293
c       local variables:
 
294
c
 
295
        integer i,j,i1,m1,n1,i2,mm1,mn1,irj,itau,iro1,icol
 
296
        integer ibar,numu,irow
 
297
c
 
298
        logical zero
 
299
c
 
300
        double precision s,temp
 
301
c
 
302
        double precision sum,heps
 
303
c
 
304
c
 
305
        mu = p
 
306
        nu = n
 
307
 10     if (mu .eq. 0) return
 
308
        iro1 = iro
 
309
        mnu = m + nu
 
310
        numu = nu + mu
 
311
        if (m .eq. 0) go to 120
 
312
        iro1 = iro1 + 1
 
313
        irow = nu
 
314
        if (isigma .le. 1) go to 40
 
315
c
 
316
c       compress rows of d: first exploit triangular shape
 
317
c
 
318
        m1 = isigma - 1
 
319
        do 30 icol = 1,m1
 
320
           do 20 j = 1,iro1
 
321
              irj = irow + j
 
322
              wrk2(j) = abf(irj,icol)
 
323
 20           continue
 
324
c
 
325
           call house(wrk2,iro1,1,heps,zero,s)
 
326
c
 
327
           call tr1(abf,naf,mplusn,wrk2,s,irow,iro1,icol,mnu)
 
328
c
 
329
           irow = irow + 1
 
330
 30        continue
 
331
c
 
332
c       continue with householder transformation with pivoting
 
333
c
 
334
 40     if (isigma .ne. 0) go to 45
 
335
        isigma = 1
 
336
        iro1 = iro1 - 1
 
337
 45     if (isigma .eq. m) go to 60
 
338
        do 55 icol = isigma,m
 
339
           sum = 0.0d+0
 
340
           do 50 j = 1,iro1
 
341
              irj = irow + j
 
342
              sum = sum + (abf(irj,icol) * abf(irj,icol) )
 
343
 50           continue
 
344
           wrk1(icol) = sum
 
345
 55        continue
 
346
c
 
347
 60     continue
 
348
        do 100 icol = isigma,m
 
349
c
 
350
c          pivot if necessary
 
351
c
 
352
           if (icol .eq. m) go to 80
 
353
c
 
354
           call pivot(wrk1,temp,ibar,icol,m)
 
355
c
 
356
           if (ibar .eq. icol) go to 80
 
357
           wrk1(ibar) = wrk1(icol)
 
358
           wrk1(icol) = temp
 
359
           do 70 i = 1,numu
 
360
              temp = abf(i,icol)
 
361
              abf(i,icol) = abf(i,ibar)
 
362
 70           abf(i,ibar) = temp
 
363
c
 
364
c          perform householder transformation
 
365
c
 
366
 80        continue
 
367
           do 90 i = 1,iro1
 
368
              irj = irow + i
 
369
 90           wrk2(i) = abf(irj,icol)
 
370
c
 
371
           call house(wrk2,iro1,1,heps,zero,s)
 
372
c
 
373
           if (zero) go to 120
 
374
           if (iro1 .eq. 1) return
 
375
c
 
376
           call tr1(abf,naf,mplusn,wrk2,s,irow,iro1,icol,mnu)
 
377
c
 
378
           irow = irow + 1
 
379
           iro1 = iro1 - 1
 
380
           do 100 j = icol,m
 
381
 100          wrk1(j) = wrk1(j) - (abf(irow,j) * abf(irow,j) )
 
382
c
 
383
 120    itau = iro1
 
384
        isigma = mu - itau
 
385
c
 
386
c       compress the columns of c
 
387
c
 
388
        i1 = nu + isigma
 
389
        mm1 = m + 1
 
390
        n1 = nu
 
391
        if (itau .eq. 1) go to 140
 
392
        do 135 i = 1,itau
 
393
           irj = i1 + i
 
394
           sum = 0.0d+0
 
395
           do 130 j = mm1,mnu
 
396
 130          sum = sum + (abf(irj,j) * abf(irj,j) )
 
397
 135       wrk1(i) = sum
 
398
c
 
399
 140    continue
 
400
        do 200 iro1 = 1,itau
 
401
           iro = iro1 - 1
 
402
           i = itau - iro
 
403
           i2 = i + i1
 
404
c
 
405
c          pivot if necessary
 
406
c
 
407
           if (i .eq. 1) go to 160
 
408
c
 
409
           call pivot(wrk1,temp,ibar,1,i)
 
410
c
 
411
           if (ibar .eq. i) go to 160
 
412
           wrk1(ibar) = wrk1(i)
 
413
           wrk1(i) = temp
 
414
           irj = ibar + i1
 
415
           do 150 j = mm1,mnu
 
416
              temp = abf(i2,j)
 
417
              abf(i2,j) = abf(irj,j)
 
418
 150          abf(irj,j) = temp
 
419
c
 
420
c          perform householder transformation
 
421
c
 
422
 160       do 170 j = 1,n1
 
423
              irj = m + j
 
424
 170          wrk2(j) = abf(i2,irj)
 
425
c
 
426
           call house(wrk2,n1,n1,heps,zero,s)
 
427
c
 
428
           if (zero) go to 210
 
429
           if (n1 .eq. 1) go to 220
 
430
c
 
431
           call tr2(abf,naf,mplusn,wrk2,s,1,i2,m,n1)
 
432
c
 
433
           mn1 = m + n1
 
434
c
 
435
           call tr1(abf,naf,mplusn,wrk2,s,0,n1,1,mn1)
 
436
c
 
437
           do 190 j = 1,i
 
438
              irj = i1 + j
 
439
 190          wrk1(j) = wrk1(j) - (abf(irj,mn1) * abf(irj,mn1) )
 
440
           mnu = mnu - 1
 
441
 200       n1 = n1 - 1
 
442
c
 
443
        iro = itau
 
444
 210    nu = nu - iro
 
445
        mu = isigma + iro
 
446
        if (iro .eq. 0) return
 
447
        go to 10
 
448
c
 
449
 220    mu = isigma
 
450
        nu = 0
 
451
c
 
452
        return
 
453
        end
 
454
        subroutine house(wrk2,k,j,heps,zero,s)
 
455
c
 
456
c  warning - this routine is only to be called from slice routine
 
457
c            sszer
 
458
c
 
459
c% purpose
 
460
c       this routine constructs a householder transformation h = i-s.uu
 
461
c       that 'mirrors' a vector wrk2(1,...,k) to the j-th unit vector.
 
462
c       if norm(wrk2) < heps, zero is put equal to .true.
 
463
c       upon return, u is stored in wrk2
 
464
c
 
465
c%
 
466
        integer k,j
 
467
c
 
468
        double precision wrk2(k),heps,s
 
469
c
 
470
        logical zero
 
471
c
 
472
c       local variables:
 
473
c
 
474
        integer i
 
475
c
 
476
        double precision alfa,dum1
 
477
c
 
478
        double precision sum
 
479
c
 
480
c
 
481
        zero = .true.
 
482
        sum = 0.0d+0
 
483
        do 10 i = 1,k
 
484
 10        sum = sum + (wrk2(i) * wrk2(i) )
 
485
c
 
486
        alfa = sqrt(sum)
 
487
        if (alfa .le. heps) return
 
488
c
 
489
        zero = .false.
 
490
        dum1 = wrk2(j)
 
491
        if (dum1 .gt. 0.0d+0) alfa = -alfa
 
492
        wrk2(j) = dum1 - alfa
 
493
        s = 1.0d+0 / (sum - (alfa * dum1) )
 
494
c
 
495
        return
 
496
        end
 
497
 
 
498
        subroutine tr1(a,na,n,u,s,i1,i2,j1,j2)
 
499
c% calling sequence
 
500
c
 
501
c        subroutine tr1(a,na,n,u,s,i1,i2,j1,j2)
 
502
c
 
503
c%purpose
 
504
c
 
505
c       this subroutine performs the householder transformation
 
506
c                       h = i - s.uu
 
507
c       on the rows i1 + 1 to i1 + i2 of a, this from columns j1 to j2.
 
508
c% comments
 
509
c
 
510
c  warning - this routine is only to be called from slice routine
 
511
c            sszer
 
512
c
 
513
c%
 
514
        integer na,n,i1,i2,j1,j2
 
515
c
 
516
        double precision a(na,n),u(i2),s
 
517
c
 
518
c       local variables:
 
519
c
 
520
        integer i,j,irj
 
521
c
 
522
        double precision y
 
523
c
 
524
        double precision sum
 
525
c
 
526
c
 
527
        do 20 j = j1,j2
 
528
           sum = 0.0d+0
 
529
           do 10 i = 1,i2
 
530
              irj = i1 + i
 
531
 10           sum = sum + (u(i) * a(irj,j) )
 
532
c
 
533
           y = sum * s
 
534
c
 
535
           do 20 i = 1,i2
 
536
              irj = i1 + i
 
537
 20           a(irj,j) = a(irj,j) - (u(i) * y)
 
538
c
 
539
        return
 
540
        end
 
541
 
 
542
        subroutine tr2(a,na,n,u,s,i1,i2,j1,j2)
 
543
c% calling sequence
 
544
c
 
545
c        subroutine tr2(a,na,n,u,s,i1,i2,j1,j2)
 
546
c%purpose
 
547
c
 
548
c       this routine performs the householder transformation h = i-s.uu
 
549
c       on the columns j1 + 1 to j1 + j2 of a, this from rows i1 to i2.
 
550
c
 
551
c% comments
 
552
c
 
553
c  warning - this routine is only to be called from slice routine
 
554
c            sszer
 
555
c%
 
556
        integer na,n,i1,i2,j1,j2
 
557
c
 
558
        double precision a(na,n),u(j2),s
 
559
c
 
560
c       local variables:
 
561
c
 
562
        integer i,j,irj
 
563
c
 
564
        double precision y
 
565
c
 
566
        double precision sum
 
567
c
 
568
c
 
569
        do 20 i = i1,i2
 
570
          sum = 0.0d+0
 
571
           do 10 j = 1,j2
 
572
              irj = j1 + j
 
573
 10           sum = sum + (u(j) * a(i,irj) )
 
574
c
 
575
           y = sum * s
 
576
c
 
577
           do 20 j = 1,j2
 
578
              irj = j1 + j
 
579
 20           a(i,irj) = a(i,irj) - (u(j) * y)
 
580
c
 
581
        return
 
582
        end
 
583
 
 
584
        subroutine pivot(vec,vmax,ibar,i1,i2)
 
585
c% calling sequence
 
586
c       subroutine pivot(vec,vmax,ibar,i1,i2)
 
587
c       integer ibar,i1,i2
 
588
c       double precision vec(i2),vmax
 
589
c
 
590
c% purpose
 
591
c
 
592
c       this subroutine computes the maximal norm element (vthe max)
 
593
c       of the vector vec(i1,...,i2), and its location ibar
 
594
c
 
595
c       this routine is only to be called from slice routine sszer
 
596
c
 
597
c%
 
598
        integer ibar,i1,i2
 
599
c
 
600
        double precision vec(i2),vmax
 
601
c
 
602
c       local variables:
 
603
c
 
604
        integer i,i11
 
605
c
 
606
c
 
607
        ibar = i1
 
608
        vmax = vec(i1)
 
609
        if (i1 .ge. i2) go to 20
 
610
        i11 = i1 + 1
 
611
        do 10 i = i11,i2
 
612
           if (abs(vec(i) ) .lt. vmax) go to 10
 
613
           vmax = abs (vec(i) )
 
614
           ibar = i
 
615
  10       continue
 
616
c
 
617
  20    if (vec(ibar) .lt. 0.0d+0) vmax = -vmax
 
618
c
 
619
        return
 
620
        end