← Back to branch summary

~ubuntu-branches/ubuntu/hoary/scilab/hoary

« back to all changes in this revision

Viewing changes to routines/control/polmc.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/polmc.f
 
1
      subroutine polmc(nm,ng,n,m,a,b,g,wr,wi,z,inc,invr,ierr,jpvt,
 
2
     x                  rm1,rm2,rv1,rv2,rv3,rv4)
 
3
c
 
4
      double precision a(nm,n),b(nm,m),g(ng,n),wr(n),wi(n),z(nm,n),
 
5
     x     rm1(m,m),rm2(m,*),rv1(n),rv2(n),rv3(m),rv4(m)
 
6
      double precision p,q,r,s,t,zz
 
7
      integer invr(n),jpvt(m)
 
8
      logical complx
 
9
c!purpose
 
10
c     this subroutine determines the state feedback matrix  g  of the
 
11
c     linear time-invariant multi-input system
 
12
c
 
13
c        dx / dt = a * x + b * u,
 
14
c
 
15
c     where  a  is a  nxn  and  b  is a  nxm  matrix, such that the
 
16
c     closed-loop system
 
17
c
 
18
c        dx / dt = (a - b * g) * x
 
19
c
 
20
c     has desired poles. the system must be preliminary reduced into
 
21
c     orthogonal canonical form using the subroutine  trmcf.
 
22
c!calling sequence
 
23
c
 
24
c     subroutine polmc(nm,ng,n,m,a,b,g,wr,wi,z,inc,invr,ierr,jpvt,
 
25
c    x                  rm1,rm2,rv1,rv2,rv3,rv4)
 
26
c
 
27
c     on input-
 
28
c
 
29
c        nm    is an integer variable set equal to the row dimension
 
30
c              of the two-dimensional arrays  a,  b  and  z  as
 
31
c              specified in the dimension statements for  a,  b  and  z
 
32
c              in the calling program,
 
33
c
 
34
c        ng    is an integer variable set equal to the row dimension
 
35
c              of the two-dimensional array  g  as specified in the
 
36
c              dimension statement for  g  in the calling program,
 
37
c
 
38
c        n     is an integer variable set equal to the order of the
 
39
c              matrices  a  and  z.  n  must be not greater than  nm,
 
40
c
 
41
c        m     is an integer variable set equal to the number of the
 
42
c              columns of the matrix  b.  m  must be not greater than
 
43
c              ng,
 
44
c
 
45
c        a     is a working precision real two-dimensional variable with
 
46
c              row dimension  nm  and column dimension at least  n
 
47
c              containing the block-hessenberg canonical form of the
 
48
c              matrix  a.  the elements below the subdiagonal blocks
 
49
c              must be equal to zero,
 
50
c
 
51
c        b     is a working precision real two-dimensional variable with
 
52
c              row dimension  nm  and column dimension at least  m
 
53
c              containing the canonical form of the matrix  b. the
 
54
c              elements below the  invr(1)-th row must be equal to zero,
 
55
c
 
56
c        wr,wi are working precision real one-dimensional variables
 
57
c              of dimension at least  n  containing the real and
 
58
c              imaginery parts, respectively, of the desired poles,
 
59
c              the poles can be unordered except that the complex
 
60
c              conjugate pairs of poles must appfar consecutively.
 
61
c              note that on output the imaginery parts of the poles
 
62
c              may be modified,
 
63
c
 
64
c        z     is a working precision real two-dimensonal variale with
 
65
c              row dimension  nm  and column dimension at least  n
 
66
c              containing the orthogonal transformation matrix produced
 
67
c              in  trmcf  which reduces the system into canonical form,
 
68
c
 
69
c        inc   is an integer variable set equal to the controllability
 
70
c              index of the system,
 
71
c
 
72
c        invr  is an integer one-dimensional variable of dimension at
 
73
c              least  inc  containing the dimensons of the
 
74
c              controllable subsystems in the canonical form.
 
75
c
 
76
c     on output-
 
77
c
 
78
c        a     contains the upper quast-triangular form of the closed-
 
79
c              loop system matrix  a - b * g,  that is triangular except
 
80
c              of possible  2x2  blocks on the diagonal,
 
81
c
 
82
c        b     contains the transformed matrix  b,
 
83
c
 
84
c        g     is a working precision real two-dimensional variable with
 
85
c              row dimension  ng  and column dimension at least  n
 
86
c              containing the state feedback matrix  g  of the original
 
87
c              system,
 
88
c
 
89
c        z     contains the orthogonal matrix which reduces the closed-
 
90
c              loop system matrix  a - b * g  to the upper quasi-
 
91
c              triangular form,
 
92
c
 
93
c        ierr  is an integer variable set equal to
 
94
c              zero  for normal return,
 
95
c              1     if the system is not completely controllable,
 
96
c
 
97
c        jpvt  is an integer temporary one-dimensonal array of
 
98
c              dimension at least  m  used in the solution of linear
 
99
c              equations,
 
100
c
 
101
c        rm1   is a working precision real temporary two-dimensonal
 
102
c              array of dimension at least  mxm  used in the solution
 
103
c              of linear equations,
 
104
c
 
105
c        rm2   is a working precision real temporary two-dimensional
 
106
c              array od dimension at least  mxmax(2,m)  used in the
 
107
c              solution of linear equations,
 
108
c
 
109
c        rv1,  are working precision real temporary one-dimensional
 
110
c          rv2 arrays of dimension at least  n  used to hold the
 
111
c              real and imaginery parts, respectively, of the
 
112
c              eigenvectors during the reduction,
 
113
c
 
114
c        rv3,  are working precision real temporary one-dimensional
 
115
c          rv4 arrays of dimension at least  m  used in the solution
 
116
c              of linear equations.
 
117
c
 
118
c!auxiliary routines
 
119
c
 
120
c        sqrsm
 
121
c        fortran  abs,min,sqrt
 
122
c!originator
 
123
c     p.hr.petkov, higher institute of mechanical and electrical
 
124
c     engineering, sofia, bulgaria.
 
125
c     modified by serge Steer INRIA
 
126
c     Copyright SLICOT
 
127
c!
 
128
c
 
129
      ierr = 0
 
130
      m1 = invr(1)
 
131
      l = 0
 
132
   10    l = l + 1
 
133
         mr = invr(inc)
 
134
         if (inc .eq. 1) go to 350
 
135
         lp1 = l + m1
 
136
         inc1 = inc - 1
 
137
         mr1 = invr(inc1)
 
138
         nr = n - mr + 1
 
139
         nr1 = nr - mr1
 
140
         complx = wi(l) .ne. 0.0d+0
 
141
        do 15 i = nr, n
 
142
           rv1(i) = 0.0d+0
 
143
           if (complx) rv2(i) = 0.0d+0
 
144
   15   continue
 
145
c
 
146
        rv1(nr) = 1.0d+0
 
147
        if (.not. complx) go to 20
 
148
        if (mr .eq. 1) rv2(nr) = 1.0d+0
 
149
        if (mr .gt. 1) rv2(nr+1) = 1.0d+0
 
150
        t = wi(l)
 
151
        wi(l) = 1.0d+0
 
152
        wi(l+1) = t * wi(l+1)
 
153
c
 
154
c       compute and transform eigenvector
 
155
c
 
156
   20   do 200 ip = 1, inc
 
157
           if (ip .eq. inc .and. inc .eq. 2) go to 200
 
158
           if (ip .eq. inc) go to 120
 
159
c
 
160
           do 40 ii = 1, mr
 
161
              i = nr + ii - 1
 
162
c
 
163
              do 30 jj = 1, mr1
 
164
                 j = nr1 + jj - 1
 
165
                 rm1(ii,jj) = a(i,j)
 
166
   30         continue
 
167
c
 
168
   40      continue
 
169
c
 
170
           if (ip .eq. 1) go to 70
 
171
c
 
172
c          scaling
 
173
c
 
174
           s = 0.0d+0
 
175
           mp1 = mr + 1
 
176
           np1 = nr + mp1
 
177
c
 
178
           do 50 ii = 1, mp1
 
179
              i = nr + ii - 1
 
180
              s = s + abs(rv1(i))
 
181
              if (complx) s = s + abs(rv2(i))
 
182
   50      continue
 
183
c
 
184
           do 60 ii = 1, mp1
 
185
              i = nr + ii - 1
 
186
              rv1(i) = rv1(i) / s
 
187
              if (complx) rv2(i) = rv2(i) / s
 
188
   60      continue
 
189
c
 
190
           if (complx .and. np1 .le. n) rv2(np1) = rv2(np1) / s
 
191
   70      if (ip .eq. 1) mp1 = 1
 
192
           np1 = nr + mp1
 
193
c
 
194
           do 100 ii = 1, mr
 
195
              i = nr + ii - 1
 
196
              s = wr(l) * rv1(i)
 
197
c
 
198
              do 80 jj = 1, mp1
 
199
                 j = nr + jj - 1
 
200
                 s = s - a(i,j) * rv1(j)
 
201
   80         continue
 
202
c
 
203
              rm2(ii,1) = s
 
204
              if (.not. complx) go to 100
 
205
              rm2(ii,1) = rm2(ii,1) + wi(l+1) * rv2(i)
 
206
              s = wr(l+1) * rv2(i) + wi(l) * rv1(i)
 
207
c
 
208
              do 90 jj = 1, mp1
 
209
c la ligne suivante a ete rajoutee par mes soins
 
210
                 j = nr + jj - 1
 
211
                 s = s - a(i,j) * rv2(j)
 
212
   90         continue
 
213
c
 
214
              if (np1 .le. n) s = s - a(i,np1) * rv2(np1)
 
215
              rm2(ii,2) = s
 
216
  100      continue
 
217
c
 
218
c          solving linear equations for the eigenvector elements
 
219
c
 
220
           nc = 1
 
221
           if (complx) nc = 2
 
222
           call dqrsm(rm1,m,mr,mr1,rm2,m,nc,rm2,m,ir,jpvt,
 
223
     x                             rv3,rv4)
 
224
           if (ir .lt. mr) go to 600
 
225
c
 
226
           do 110 ii = 1, mr1
 
227
              i = nr1 + ii - 1
 
228
              rv1(i) = rm2(ii,1)
 
229
              if (complx) rv2(i) = rm2(ii,2)
 
230
  110      continue
 
231
c
 
232
           if (ip .eq. 1 .and. inc .gt. 2) go to 195
 
233
  120      nj = nr
 
234
           if (ip .lt. inc) nj = nr1
 
235
           ni = nr + mr - 1
 
236
           inc2 = inc - ip + 2
 
237
           if (ip .gt. 1) ni = ni + invr(inc2)
 
238
           if (ip .gt. 2) ni = ni + 1
 
239
           if (complx .and. ip .gt. 2) ni = min(ni+1,n)
 
240
           kmr = mr1
 
241
           if (ip .gt. 1) kmr = mr
 
242
c
 
243
           do 190 kk = 1, kmr
 
244
              ll = 1
 
245
              k = nr + mr - kk
 
246
              if (ip .eq. 1) k = nr - kk
 
247
  130         p = rv1(k)
 
248
              if (ll .eq. 2) p = rv2(k)
 
249
              q = rv1(k+1)
 
250
              if (ll .eq. 2) q = rv2(k+1)
 
251
              s = abs(p) + abs(q)
 
252
              p = p / s
 
253
              q = q / s
 
254
              r = sqrt(p*p+q*q)
 
255
              t = s * r
 
256
              rv1(k) = t
 
257
              if (ll .eq. 2) rv2(k) = t
 
258
              rv1(k+1) = 0.0d+0
 
259
              if (ll .eq. 2) rv2(k+1) = 0.0d+0
 
260
              p = p / r
 
261
              q = q / r
 
262
c
 
263
c             transform  a
 
264
c
 
265
              do 140 j = nj, n
 
266
                 zz = a(k,j)
 
267
                 a(k,j) = p * zz + q * a(k+1,j)
 
268
                 a(k+1,j) = p * a(k+1,j) - q * zz
 
269
  140         continue
 
270
c
 
271
              do 150 i = 1, ni
 
272
                 zz = a(i,k)
 
273
                 a(i,k) = p * zz + q * a(i,k+1)
 
274
                 a(i,k+1) = p * a(i,k+1) - q * zz
 
275
  150         continue
 
276
c
 
277
              if (k .eq. lp1 .and. ll .eq. 1 .or. k .gt. lp1) go to 170
 
278
c
 
279
c        transform b
 
280
c
 
281
             do 160 j = 1, m
 
282
                zz = b(k,j)
 
283
                b(k,j) = p * zz + q * b(k+1,j)
 
284
                b(k+1,j) = p * b(k+1,j) - q * zz
 
285
  160         continue
 
286
c
 
287
c             accumulate transformations
 
288
c
 
289
  170          do 180 i = 1, n
 
290
                  zz = z(i,k)
 
291
                  z(i,k) = p * zz + q * z(i,k+1)
 
292
                  z(i,k+1) = p * z(i,k+1) - q * zz
 
293
  180          continue
 
294
c
 
295
               if (.not. complx .or. ll .eq. 2) go to 190
 
296
               zz = rv2(k)
 
297
               rv2(k) = p * zz + q * rv2(k+1)
 
298
               rv2(k+1) = p * rv2(k+1) - q * zz
 
299
               if (k + 2 .gt. n) go to 190
 
300
               k = k + 1
 
301
               ll = 2
 
302
               go to 130
 
303
  190       continue
 
304
c
 
305
            if (ip .eq. inc) go to 200
 
306
  195       mr = mr1
 
307
            nr = nr1
 
308
            if (ip .eq. inc1) go to 200
 
309
            inc2 = inc - ip - 1
 
310
            mr1 = invr(inc2)
 
311
            nr1 = nr1 - mr1
 
312
  200    continue
 
313
c
 
314
         if (complx) go to 250
 
315
c
 
316
c        find one column of  g
 
317
c
 
318
         do 220 ii = 1, m1
 
319
            i = l + ii
 
320
c
 
321
            do 210 j = 1, m
 
322
  210       rm1(ii,j) = b(i,j)
 
323
c
 
324
            rm2(ii,1) = a(i,l)
 
325
  220    continue
 
326
c
 
327
         call dqrsm(rm1,m,m1,m,rm2,m,1,g(1,l),ng,ir,jpvt,rv3,rv4)
 
328
         if (ir .lt. m1) go to 600
 
329
c
 
330
         do 240 i = 1, lp1
 
331
c
 
332
            do 230 j = 1, m
 
333
  230       a(i,l) = a(i,l) - b(i,j) * g(j,l)
 
334
c
 
335
  240    continue
 
336
c
 
337
         go to 330
 
338
c
 
339
c        find two columns of  g
 
340
c
 
341
  250    l = l + 1
 
342
         if (lp1 .lt. n) lp1 = lp1 + 1
 
343
c
 
344
         do 270 ii = 1, m1
 
345
              i = l + ii
 
346
              if (l + m1 .gt. n) i = i - 1
 
347
c
 
348
c la ligne suivante a ete rajoutee par mes soins
 
349
              do 260 j = 1 , m
 
350
cxxx        if(abs(b(i,j)).le.abs(b(l,j))) i=i-1
 
351
  260         rm1(ii,j) = b(i,j)
 
352
c
 
353
              p = a(i,l-1)
 
354
              if (i .eq. l) p = p - (rv2(i) / rv1(i-1)) * wi(i)
 
355
              rm2(ii,1) = p
 
356
              q = a(i,l)
 
357
              if (i .eq. l) q = q - wr(i) + (rv2(i-1) / rv1(i-1)) *wi(i)
 
358
              rm2(ii,2) = q
 
359
  270      continue
 
360
c
 
361
           call dqrsm(rm1,m,m1,m,rm2,m,2,rm2,m,ir,jpvt,rv3,rv4)
 
362
           if (ir .lt. m1) go to 600
 
363
c
 
364
           do 290 i = 1, m
 
365
c
 
366
              do 280 jj = 1, 2
 
367
                 j = l + jj - 2
 
368
                 g(i,j) = rm2(i,jj)
 
369
  280         continue
 
370
c
 
371
  290      continue
 
372
c
 
373
           do 320 i = 1, lp1
 
374
c
 
375
              do 310 jj = 1, 2
 
376
                 j = l + jj - 2
 
377
c
 
378
                 do 300 k = 1, m
 
379
  300            a(i,j) = a(i,j) - b(i,k)*g(k,j)
 
380
c
 
381
  310         continue
 
382
c
 
383
  320      continue
 
384
c
 
385
           if (l .eq. n) go to 500
 
386
  330      invr(inc) = invr(inc) - 1
 
387
           if (invr(inc) .eq. 0) inc = inc - 1
 
388
           if (complx) invr(inc) = invr(inc) - 1
 
389
           if (invr(inc) .eq. 0) inc = inc - 1
 
390
        go to 10
 
391
c
 
392
c       find the rest columns of  g
 
393
c
 
394
  350   do 370 ii = 1, mr
 
395
           i = l + ii - 1
 
396
c
 
397
           do 360 j = 1, m
 
398
  360   rm1(ii,j) = b(i,j)
 
399
c
 
400
  370 continue
 
401
c
 
402
      do 400 ii = 1, mr
 
403
         i = l + ii - 1
 
404
c
 
405
         do 380 jj = 1, mr
 
406
            j = l + jj - 1
 
407
            if (ii .lt. jj) rm2(ii,jj) = 0.0d+0
 
408
            if (ii .gt. jj) rm2(ii,jj) = a(i,j)
 
409
  380    continue
 
410
c
 
411
  400 continue
 
412
c
 
413
      ii = 0
 
414
  410    ii = ii + 1
 
415
         i = l + ii - 1
 
416
         if (wi(i) .ne. 0.0d+0) go to 420
 
417
         rm2(ii,ii) = a(i,i) - wr(i)
 
418
         if (ii .eq. mr) go to 430
 
419
c la ligne suivante a ete rajoutee par mes soins
 
420
      goto 410
 
421
  420    rm2(ii,ii) = a(i,i) - wr(i)
 
422
         rm2(ii,ii+1) = a(i,i+1) - wi(i)
 
423
         rm2(ii+1,ii) = a(i+1,i) - wi(i+1)
 
424
         rm2(ii+1,ii+1) = a(i+1,i+1) - wr(i+1)
 
425
         ii = ii + 1
 
426
      if (ii .lt. mr) go to 410
 
427
  430 call dqrsm(rm1,m,mr,m,rm2,m,m,rm2,m,ir,jpvt,rv3,rv4)
 
428
      if (ir .lt. mr) go to 600
 
429
c
 
430
      do 450 i = 1, m
 
431
c
 
432
         do 440 jj = 1, mr
 
433
            j = l + jj - 1
 
434
            g(i,j) = rm2(i,jj)
 
435
  440    continue
 
436
c
 
437
  450 continue
 
438
c
 
439
      do 480 i = 1, n
 
440
c
 
441
         do 470 j = l, n
 
442
c
 
443
            do 460 k = 1, m
 
444
  460       a(i,j) = a(i,j) - b(i,k) * g(k,j)
 
445
c
 
446
  470    continue
 
447
c
 
448
  480 continue
 
449
c
 
450
c     transform  g
 
451
c
 
452
  500 do 540 i = 1, m
 
453
c
 
454
         do 520 j = 1, n
 
455
            s = 0.0d+0
 
456
c
 
457
            do 510 k = 1, n
 
458
  510       s = s + g(i,k) * z(j,k)
 
459
c
 
460
            rv1(j) = s
 
461
  520    continue
 
462
c
 
463
         do 530 j = 1, n
 
464
  530    g(i,j) = rv1(j)
 
465
c
 
466
  540 continue
 
467
c
 
468
      go to 610
 
469
c
 
470
c     set error -- the system is not completely controllable
 
471
c
 
472
  600 ierr = 1
 
473
  610 return
 
474
c
 
475
c     last card of subroutine polmc
 
476
c
 
477
      end