1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
|
// Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
// Copyright (C) INRIA -
//
// This file must be used under the terms of the CeCILL.
// This source file is licensed as described in the file COPYING, which
// you should have received as part of this distribution. The terms
// are also available at
// http://www.cecill.info/licences/Licence_CeCILL_V2.1-en.txt
function [k,x,err]=leqr(p12,vx)
//h-infinity lqr gain for full-state lq problem
//(discrete or continuous)
//
// discrete continuous
// |i -vx 0| | a 0 b| |i 0 0| | a vx b |
// z|0 a' 0| - |-c'c i -s| s|0 i 0| - |-c'c -a' -s |
// |0 b' 0| | s' 0 d'd| |0 0 0| | s' -b' d'd|
//
[lhs,rhs]=argn(0);
if typeof(p12)<>"state-space" then
error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space expected.\n"),"leqr",1))
end
[a,b2,c1,d12]=p12(2:5);
[n,nu]=size(b2);
[ny,n]=size(c1);
dom=p12(7);
if dom==[] then
dom="c";
warning(msprintf(gettext("%s: Input argument %d is assumed continuous time.\n"),"leqr",1));
end
select dom
// continuous
case "c" then
z=0*a;i=eye(a);
q=c1'*c1;r=d12'*d12;s=c1'*d12;
bige=[i,z,zeros(b2);
z,i,zeros(b2);
zeros(nu,2*n+nu)];
biga=[a,vx,b2;
-q,-a',-s;
s',b2',r];
[bige,biga,dummy,z]=balanc(bige,biga);
[w,k]=schur(biga,bige,"c");
if k<>n then
warning(msprintf(gettext("%s: Stable subspace is too small.\n"),"leqr"));
k=[];w=[];err=[];
return;
end
ws=z*w(:,1:n);
x12=ws(1:n,:);
if rcond(x12) < 1.d-6 then
warning(msprintf(gettext("%s: Bad conditionning.\n"),"leqr"));
end
k=ws(2*n+1:2*n+nu,:)/x12;
x=ws(n+1:2*n,:)/x12;
if lhs~=3 then return;end
ri=pinv(r);
err=norm((a-b2*ri*s')'*x+x*(a-b2*ri*s')-x*(b2*ri*b2'-vx)*x+q-s*ri*s',1)
//k=-ri*(b2'*x+s')
// discrete time
case "d" then
i=eye(a);z=0*i;
q=c1'*c1;r=d12'*d12;s=c1'*d12;
bige=[i,-vx,zeros(b2);
z,a',zeros(b2);
zeros(b2'),-b2',zeros(b2'*b2)];
biga=[a,z,b2;
-q,i, -s;
s', 0*b2', r];
[bige,biga,dummy,z]=balanc(bige,biga);
[w,k]=schur(biga,bige,"d");
if k<>n then
warning(msprintf(gettext("%s: Stable subspace is too small.\n"),"leqr"));
k=[];w=[];err=[];
return;
end
ws=z*w(:,1:n);
x12=ws(1:n,:);
if rcond(x12) <1.d-6 then
warning(msprintf(gettext("%s: Bad conditionning.\n"),"leqr"));
k=[];w=[];
return;
end
k=ws(2*n+1:2*n+nu,:)/x12;
x=ws(n+1:2*n,:)/x12;
if norm(x-x',1)>0.0001 then
warning(msprintf(gettext("%s: %s non symmetric.\n"),"leqr","x"));
k=[];w=[];
return;
end
//a'*x*a-(a'*x*b2+c1'*d12)*pinv(b2'*x*b2+d12'*d12)*(b2'*x*a+d12'*c1)+c1'*c1
if lhs~=3 then return;end
ri=pinv(r);
abar=a-b2*ri*s';
qbar=q-s*ri*s';
err=norm(x-(abar'*inv((inv(x)+b2*ri*b2'-vx))*abar+qbar),1);
//k=-ri*(b2'*inv(inv(x)+b2*ri*b2'-vx)*abar+s')
end
endfunction
|
