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
106
107
108
|
// 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]=lqr(P12)
//lqr gain for full-state LQ problem
//(discrete or continuous)
// discrete continuous
// |I 0 0| | A 0 B | |I 0 0| | A 0 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|
if typeof(P12)<>"state-space" then
error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space expected.\n"),"lqr",1))
end
[A,B2,C1,D12]=P12(2:5);
Q=C1'*C1;R=D12'*D12;S=D12'*C1;
[n,nu]=size(B2);
[ny,n]=size(C1);
if P12(7) == [] then
error(msprintf(gettext("%s: Wrong value for input argument #%d: Time domain must be ''c'' or ''d''.\n"),"lqr",1))
elseif P12(7) == "c"
Z=0*A;I=eye(A);O=zeros(n,nu);
bigE=[I,Z,O; ...
Z,I,O; ...
zeros(nu,2*n+nu)];
bigA=[A ,Z, B2; ...
-Q ,-A',-S'; ...
S ,B2', R];
Ri=inv(R);
Left=[I, Z, -B2*Ri;
Z, I, S'*Ri;
zeros(nu,2*n), Ri];
LA=Left*bigA;LE=Left*bigE;N=1:2*n;
//[wsmall,ks1]=schur(LA(N,N),LE(N,N),'c');
[wsmall,ks1]=schur(LA(N,N),"c");
if ks1<>n then
error(msprintf(gettext("%s: Stable subspace is too small.\n"),"lqr"));
end
X12=wsmall(1:n,1:n);phi12=wsmall(n+1:$,1:n);X=phi12/X12;
if rcond(X12)< 1.d-5 then
warning(msprintf(gettext("%s: Bad conditionning.\n"),"lqr"));
end
K=-Ri*(B2'*X+S);
return;
//////////////////////////////
// Other implementation ... //
//////////////////////////////
//[Q,Z,Qd,Zd,numbeps,numbeta]=kroneck(bigE,bigA);
//[w,ks]=schur(bigA,bigE,'c');
//if ks<>n then error('lqr: stable subspace too small!');end
//ws=w(:,1:n);
//X12=ws(1:n,:);
//phi12=ws(n+1:2*n,:);
//u12=ws(2*n+1:2*n+nu,:);
//if rcond(X12)< 1.d-5 then warning('lqr: bad conditionning!');end
//K=u12/X12;
//X=phi12/X12;
//return
elseif P12(7) == "d" | type(P12(7))==1
I=eye(A);Z=0*I;
Q=C1'*C1;R=D12'*D12;S=D12'*C1;
bigE=[I,Z,0*B2; ...
Z,A',0*B2; ...
0*B2',-B2',0*B2'*B2];
bigA=[A,Z, B2; ...
-Q ,I, -S'; ...
S, 0*B2', R];
Ri=inv(R);
Left=[I, Z, -B2*Ri; ...
Z, I, S'*Ri; ...
zeros(nu,2*n), Ri];
LA=Left*bigA;LE=Left*bigE;N=1:2*n;
[wsmall,ks1]=schur(LA(N,N),LE(N,N),"d");
if ks1<>n then
error(msprintf(gettext("%s: Stable subspace is too small.\n"),"lqr"));
end
X12=wsmall(1:n,1:n);phi12=wsmall(n+1:$,1:n);X=phi12/X12;
if rcond(X12)< 1.d-5 then
warning(msprintf(gettext("%s: Bad conditionning.\n"),"lqr"));
end
K=-pinv(B2'*X*B2+R)*(B2'*X*A+S);
return
////////////////////
// Other form ... //
////////////////////
//[w,ks]=schur(bigA,bigE,'d');
//if ks<>n then error('lqr: stable subspace too small!');end
//ws=w(:,1:n);
//X12=ws(1:n,:);
//phi12=ws(n+1:2*n,:);
//u12=ws(2*n+1:2*n+nu,:);
//if rcond(X12)< 1.d-5 then warning('lqr: bad conditionning!');end
//K=u12/X12;
//X=phi12/X12;
//return
end
endfunction
|
