Revision 1436, 1.0 kB
(checked in by vahalam, 12 years ago)
|
pridani a uprava lqg s hyperstavem viz clanek Kim2006
|
Rev | Line | |
---|
[1436] | 1 | function [u_l, S_l] = ctrlLQ4(x, ref_ome, A, B, S, Q, R, iter) |
---|
| 2 | S_l = S; |
---|
| 3 | |
---|
| 4 | %korekce A |
---|
| 5 | a = 0.9898; |
---|
| 6 | b = 0.0072; |
---|
| 7 | c = 0.0361; |
---|
| 8 | d = 1.0; |
---|
| 9 | e = 0.0149; |
---|
| 10 | dt = 0.000125; |
---|
| 11 | ial = x(1); |
---|
| 12 | ibe = x(2); |
---|
| 13 | ome = x(3); |
---|
| 14 | the = x(4); |
---|
| 15 | A(1,15) = b*sin(the)*ref_ome - b*ome*the*cos(the); |
---|
| 16 | A(2,15) = -b*cos(the)*ref_ome - b*ome*the*sin(the); |
---|
| 17 | A(3,15) = (d - 1)*ref_ome + e*the*(ial*cos(the)+ibe*sin(the)); |
---|
| 18 | A(4,15) = dt*ref_ome; |
---|
| 19 | A(5,15) = 1; |
---|
| 20 | |
---|
| 21 | %Riccati |
---|
| 22 | % for i = 1:iter |
---|
| 23 | % S_l = A'*(S_l - S_l*B/(B'*S_l*B + R)*B'*S_l)*A + Q; |
---|
| 24 | % end |
---|
| 25 | % L = (B'*S_l*B + R)\B'*S_l*A; |
---|
| 26 | |
---|
| 27 | %QR - !!! do Q a R je treba ted davat odmocniny |
---|
| 28 | for i = 1:iter |
---|
| 29 | preQR = [Q*B, Q*A;... |
---|
| 30 | R, zeros(2,15);... |
---|
| 31 | S_l*B, S_l*A]; |
---|
| 32 | [~, postR] = qr(preQR); |
---|
| 33 | AA = postR(1:2,1:2); |
---|
| 34 | BB = postR(1:2,3:end); |
---|
| 35 | S_l = postR(3:7,3:end); |
---|
| 36 | end |
---|
| 37 | L = AA\BB; |
---|
| 38 | |
---|
| 39 | y = x; |
---|
| 40 | y(3) = y(3) - ref_ome; |
---|
| 41 | u_l = -L*[y;1]; |
---|
| 42 | end |
---|