1 | function [ulq] = c2008lqcM(A, B, C, Xk, w, N, No, Nu, Qy, Qu, ukm1) |
---|
2 | |
---|
3 | % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % |
---|
4 | % funkce vrac� ��zen� u generovan� LQ ��dic�m algoritmem. /* tvar m - file */ % |
---|
5 | % function [ulq] = c2004lqM(AD, BD, CD, Xk, w, N, No, Nu, Qy, Qu, ukm1); % |
---|
6 | % Out: ugpc ��zen� u(mx1). % |
---|
7 | % In: discr. matice: /AD(stav� [nxn]), BD(vstup� [nxm]), CD(v�stup� [rxn])/, % |
---|
8 | % Xk - stav v k (nx1), w - �adan� v�stupy k+1:k+N-No (((N-No)*r)x1), % |
---|
9 | % horizont: N = nstep (1) /No - necitlivosti (1), Nu - ��zen� (1)/, % |
---|
10 | % penaliza�n� matice: Qy - v�stup� (((N-No)*r)x((N-No)*r)) - (Qy !> 0) % |
---|
11 | % Qu - vstup� (Nu*mxNu*m) % |
---|
12 | % ukm1 - predchozi hodnota ulq tj. ulq(k-1) % |
---|
13 | %=====================================================================================% |
---|
14 | % � Feb 2008, Jednoduch� LQ ��zen� na horizontu N = nstep % |
---|
15 | %=====================================================================================% |
---|
16 | % |
---|
17 | % [rr,ss,kon,clw,clu0,cld,s,ds,du]=lqex2(b,a,k,nstep,qur,d) |
---|
18 | % |
---|
19 | % LQ optimal controller with external disturbance |
---|
20 | % |
---|
21 | % b(z^-1)/(a(z^-1) system discrete transfer function |
---|
22 | % k/(a(z^-1) system offset |
---|
23 | % nstep number of iterations of the Riccati equation (horizon) |
---|
24 | % qur input penalization |
---|
25 | % d(z^-1)/(a(z^-1) disturbance discrete transfer function |
---|
26 | % |
---|
27 | % ss(z^-1)/rr(z^-1) controller feedback part transfer function |
---|
28 | % kon/rr(z^-1) offset term of the controller |
---|
29 | % clw/rr(z^-1) transfer function of the output reference filter |
---|
30 | % clu0/rr(z^-1)transfer function of the input refrence filter |
---|
31 | % cld(z^-1)/rr(z^-1)transfer function of the disturbance filter |
---|
32 | % s,ds Riccati matrix factors s'* ds * s |
---|
33 | % du weight of the control law |
---|
34 | % |
---|
35 | |
---|
36 | echo on |
---|
37 | |
---|
38 | n=size(A,1); m=size(B,2); r=size(C,1); |
---|
39 | |
---|
40 | qy=eye(r)*Qy(1); Qu=eye(m)*Qu(1); qx=C'*qy*C; % do�asn� napln�n� matic penalizac� |
---|
41 | |
---|
42 | if 0, |
---|
43 | if ~isequal(size(qx),[n n]), error('bad dimension of state penalization'); end |
---|
44 | if ~isequal(size(qy),[r r]), error('bad dimension of output penalization'); end |
---|
45 | if ~isequal(size(qu),[m m]), error('bad dimension of input penalization'); end |
---|
46 | if No<0, error('bad initial insensitivity horizon'); end |
---|
47 | if Nu<0, error('bad control horizon'); end |
---|
48 | end |
---|
49 | |
---|
50 | qux=zeros(m,n+2*m+r); % qyx=zeros(r,n+m+r); % pomocn� penalizace % ??? qux=zeros(m,n+2*m+r); qyx=qux; |
---|
51 | qux(:,1:m)=eye(m); |
---|
52 | qux(:,n+m+r+1:end)=-eye(m); |
---|
53 | qux=qux*Qu(1); |
---|
54 | qyx=[C,-eye(r),zeros(r,m)]; |
---|
55 | |
---|
56 | %if size(s)~=size(eye(n+m+r)), s=s*eye(n+m+r); disp('1'), end |
---|
57 | s=1e-5*eye(n+m+r); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
58 | |
---|
59 | pr=[B,A, zeros(n, m+r); zeros(m+r,m+n), eye(m+r)]; % paramentry |
---|
60 | |
---|
61 | % size(pr), |
---|
62 | % size(qyx) |
---|
63 | %h=s*pr; |
---|
64 | %hqx=qx*pr; |
---|
65 | |
---|
66 | % hqy=qyx*pr |
---|
67 | hqy=qy*qyx*pr; |
---|
68 | % keyboard |
---|
69 | |
---|
70 | nstep=N; |
---|
71 | for i=1:nstep, |
---|
72 | h= s*pr; % s1=s; |
---|
73 | % diag(w((N-i)*r+1:(N-i)*r+r)) |
---|
74 | hqy(:,m+n+1:m+n+r)=-qy*diag(w((N-i)*r+1:(N-i)*r+r)); |
---|
75 | % hqy |
---|
76 | % keyboard |
---|
77 | pomqr=[h;hqy;qux]; |
---|
78 | hn=triu(qr(pomqr)); % QR rozklad bez uchov�n� ortogon�ln� matice Q |
---|
79 | s=hn(m+1:2*m+n+r,m+1:2*m+n+r); |
---|
80 | end |
---|
81 | |
---|
82 | ws=hn(1:m,m+1:2*m+n+r); |
---|
83 | wsd=hn(1:m,1:m); |
---|
84 | |
---|
85 | |
---|
86 | % Lklq=-inv(wsd)*ws; % ��dic� z�kon z�skan� z LQ synt�zy |
---|
87 | % ulq=Lklq*[Xk;eye(r,1);eye(m,1)]; % ��zen� u z LQ synt�zy |
---|
88 | |
---|
89 | |
---|
90 | %pom=ws*[Xk;ones(r,1);zeros(m,1)]; |
---|
91 | pom=ws*[Xk;ones(r,1);ukm1]; |
---|
92 | ulq=-inv(wsd)*pom; % ��zen� u na z�lad� LQ synt�zy |
---|
93 | keyboard |
---|
94 | |
---|
95 | % % % uNu=zeros(Nu*m,1); % nezn�m� z maticov� rovnice R*uNu=c |
---|
96 | % % % uNu(Nu*m,1)=c(Nu*m,1)/(R(Nu*m,Nu*m)); |
---|
97 | % % % for i=(Nu*m-1):-1:1, |
---|
98 | % % % uNu(i,1)=(1/(R(i,i)))*(c(i,1)-R(i,i+1:Nu*m)*uNu(i+1:Nu*m,1)); |
---|
99 | % % % end |
---|
100 | % % % |
---|
101 | % % % % V�b�r prvn�ch m ak�n�ch z�sah� (tj. pro �as k*Ts, krok k; m - po�et vstup� - ak�n�ch z�sah�. |
---|
102 | % % % ugpc=uNu(1:m,1); |
---|
103 | % % % |
---|