A = [0 1 0 0 0
    0 0 0 0 0
    0 0 0 0 0
    -1 0 0 0 0
    0 0 -1 0 0];

B = [0 0
    0 K_1
    K_2 0
    0 0
    0 0];

G = [0 0
    0 0
    0 0
    1 0
    0 1];

Q = diag([5 1 1 2 10]);

R = [1 0
    0 1];

[K, S, P] = lqr(A, B, Q ,R);

F = [K(1,1) K(1,3)
    K(2,1) K(2,3)];