root/applications/dual/vahala/kim/ctrlLQ4.m @ 1450

function [u_l, S_l] = ctrlLQ4(x, ref_ome, A, B, S, Q, R, iter)
    S_l = S;
    
    %korekce A
    a = 0.9898;
    b = 0.0072;
    c = 0.0361;    
    d = 1.0;
    e = 0.0149;
    dt = 0.000125;
    ial = x(1);
    ibe = x(2);
    ome = x(3);
    the = x(4);
    A(1,15) = b*sin(the)*ref_ome - b*ome*the*cos(the);
    A(2,15) = -b*cos(the)*ref_ome - b*ome*the*sin(the);
    A(3,15) = (d - 1)*ref_ome + e*the*(ial*cos(the)+ibe*sin(the));
    A(4,15) = dt*ref_ome;
    A(5,15) = 1;
    
    %Riccati
%     for i = 1:iter
%        S_l = A'*(S_l - S_l*B/(B'*S_l*B + R)*B'*S_l)*A + Q;
%     end
%     L = (B'*S_l*B + R)\B'*S_l*A;
    
    %QR - !!! do Q a R je treba ted davat odmocniny
    for i = 1:iter
           preQR = [Q*B, Q*A;...
                    R, zeros(2,15);...
                    S_l*B, S_l*A];
           [~, postR] = qr(preQR);
           AA = postR(1:2,1:2);
           BB = postR(1:2,3:end);
           S_l = postR(3:7,3:end);
    end
    L = AA\BB;
    
    y = x;
    y(3) = y(3) - ref_ome;
    u_l = -L*[y;1];
end