root/applications/dual/vahala/kim/ctrlLQ.m

function [u_l, S_l] = ctrlLQ(x, ref_ome, A, B, S, Q, R, iter)
    S_l = S;
    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;
    y = x;
    y(1) = y(1) - ref_ome;
    u_l = -L*[y;1];
end