[491] | 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 | % % % |
---|