clear all; clc;

syms R11 R22 R33
R = [R11 0 0; 
     0 R22 0; 
     0 0 R33];
 
syms I33
Q = eye(6) * I33;

syms F11 F22 F33
F = [F11   0   0;
       0 F22   0
       0   0 F33];
   
syms G13 G31
G = [0   0 G13;
     0   0   0;
     G31 0   0];
 
syms I11 I22 d
I = [I11   0   0;
       0 I22   0;
       0   0 I33];
   
A = [zeros(3,3), d * eye(3); F, G];
B = [zeros(3,3); inv(I)];
P = [-Q, -A'] * pinv([A, -B*inv(R)*B']);

K = simplify((R^-1) * B' * P)