← Back to branch summary

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

« back to all changes in this revision

Viewing changes to macros/robust/leqr.sci

  • 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:
macros/robust/leqr.sci
 
1
function [k,x,err]=leqr(p12,vx)
 
2
//h-infinity lqr gain for full-state lq problem
 
3
//(discrete or continuous)
 
4
//
 
5
//                discrete                        continuous
 
6
//   |i  -vx  0|   | a    0     b|       |i   0   0|   | a    vx    b  |
 
7
//  z|0   a'  0| - |-c'c  i    -s|      s|0   i   0| - |-c'c  -a'  -s  |
 
8
//   |0   b'  0|   | s'   0   d'd|       |0   0   0|   | s'   -b'   d'd|
 
9
//
 
10
// Copyright INRIA
 
11
[lhs,rhs]=argn(0);
 
12
p121=p12(1);
 
13
if p121(1)<>'lss' then error('leqr: state-space only!');end
 
14
[a,b2,c1,d12]=p12(2:5);
 
15
[n,nu]=size(b2);
 
16
[ny,n]=size(c1);
 
17
dom=p12(7);
 
18
if dom==[] then 
 
19
  dom='c';
 
20
  warning('leqr: time domain (p12(7)) is not defined (assumed continuous)')
 
21
end
 
22
select dom
 
23
//      continuous
 
24
case 'c' then
 
25
  z=0*a;i=eye(a);
 
26
  q=c1'*c1;r=d12'*d12;s=c1'*d12;
 
27
  bige=[i,z,zeros(b2);
 
28
        z,i,zeros(b2);
 
29
        zeros(nu,2*n+nu)];
 
30
  biga=[a,vx,b2;
 
31
       -q,-a',-s;
 
32
        s',b2',r];
 
33
 
 
34
  [bige,biga,dummy,z]=balanc(bige,biga);
 
35
  [w,k]=gschur(biga,bige,'c');
 
36
  if k<>n then warning('leqr: stable subspace too small!');...
 
37
            k=[];w=[];err=[];return;
 
38
  end
 
39
 
 
40
  ws=z*w(:,1:n);
 
41
  x12=ws(1:n,:);
 
42
  if rcond(x12) < 1.d-6 then warning('leqr: bad conditioning!');end
 
43
  k=ws(2*n+1:2*n+nu,:)/x12;
 
44
  x=ws(n+1:2*n,:)/x12;
 
45
  if lhs~=3 then return;end
 
46
  ri=pinv(r);
 
47
  err=norm((a-b2*ri*s')'*x+x*(a-b2*ri*s')-x*(b2*ri*b2'-vx)*x+q-s*ri*s',1)
 
48
//k=-ri*(b2'*x+s')
 
49
//      discrete  time 
 
50
case 'd' then
 
51
  i=eye(a);z=0*i;
 
52
  q=c1'*c1;r=d12'*d12;s=c1'*d12;
 
53
  bige=[i,-vx,zeros(b2);
 
54
        z,a',zeros(b2);
 
55
        zeros(b2'),-b2',zeros(b2'*b2)];
 
56
  biga=[a,z,b2;
 
57
       -q,i, -s;
 
58
        s', 0*b2', r];
 
59
  [bige,biga,dummy,z]=balanc(bige,biga);
 
60
  [w,k]=gschur(biga,bige,'d');
 
61
  if k<>n then warning('leqr: stable subspace too small!');...
 
62
            k=[];w=[];err=[];return;end
 
63
  ws=z*w(:,1:n);
 
64
  x12=ws(1:n,:);
 
65
  if rcond(x12) <1.d-6 then warning('leqr: bad conditioning!');...
 
66
            k=[];w=[];return;end
 
67
 
 
68
  k=ws(2*n+1:2*n+nu,:)/x12;
 
69
  x=ws(n+1:2*n,:)/x12;
 
70
  if norm(x-x',1)>0.0001 then 
 
71
        warning('leqr: x non symmetric!');...
 
72
        k=[];w=[];return;
 
73
  end
 
74
 
 
75
//a'*x*a-(a'*x*b2+c1'*d12)*pinv(b2'*x*b2+d12'*d12)*(b2'*x*a+d12'*c1)+c1'*c1
 
76
  if lhs~=3 then return;end
 
77
  ri=pinv(r);
 
78
  abar=a-b2*ri*s';
 
79
  qbar=q-s*ri*s';
 
80
  err=norm(x-(abar'*inv((inv(x)+b2*ri*b2'-vx))*abar+qbar),1);
 
81
//k=-ri*(b2'*inv(inv(x)+b2*ri*b2'-vx)*abar+s')
 
82
end
 
83