I = [310 1.11 1.01 ; 1.11 360 -0.35 ; 1.01 -0.35 530.7];

I_w = [0.01911 0 0 ; 0 0.01911 0; 0 0 0.01911];

I_b = I - I_w;

I_b_inverse = inv(I_b);

w = [0;0;0.024];





A = [0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; 0 0 0 0 0 0; 0 0 0 0 0 0; 0 0 0 0 0 0];
B = [ 0 0 0; 0 0 0; 0 0 0; -I_b_inverse];
%%B = [ 0 0 0; 0 0 0; 0 0 0; -0.0032 0 0; 0 -0.0028 0; 0 0 -0.0019];
C= [ 1 1 1 1 1 1];
D = [0 0 0];

R= [1 0 0; 0 1 0; 0 0 1];
Q = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1];

s = ss(A,B,C,D);

c = ctrb(A,B);
o = obsv(A,C);

k = lqr(A,B,Q,R);