RAW CODE

%% For VT 2013 Microgravity Control Design

% I found an error I have been making repeatedly : I write over h , w , and

% d variables used to characterize satellite later when I program 3 x 3

% matrix inversion

clear all

clc

hs = sym('hs','real');

ws = sym('ws','real');

ds = sym('ds','real');

ma1 = sym('ma1','real');

ma2 = sym('ma2','real');

ma3 = sym('ma3','real');

m4 = sym('m4','real');

mB = sym('mB','real');

wcx = sym('wcx','real');

wcy = sym('wcy','real');

wcz = sym('wcz','real');

wci = [wcx;wcy;wcz];

ddxm1 = sym('ddxm1','real');

dxm1 = sym('dxm1','real');

xm1 = sym('xm1','real');

a_a1 = [ddxm1;0;0];

v_a1 = [dxm1;0;0];

r_a1 = [xm1;0;(hs/2)];

ddym2 = sym('ddym2','real');

dym2 = sym('dym2','real');

ym2 = sym('ym2','real');

a_a2 = [0;ddym2;0];

v_a2 = [0;dym2;0];

r_a2 = [0;ym2;(-hs/2)];

M_T = ma1 + ma2 + mB ; % total mass

%%

% Fixed payload inertia matrix: NOTE - we will need to have accurate

% weights and dimensions of all of the hardware and the satellite structure

% to generate an accurate payload fixed inertia matrix. The starting point

% listed below is too simple and will need adjustment for accurate

% maneuvers

%%

Ib = [(1/12)*(mB*(hs^2 + ws^2)), 0, 0;

0 , (1/12)*(mB*(hs^2 + ds^2)), 0;

0 , 0, (1/12)*(mB*(ds^2 + ws^2))];

% Adding an offset constant postion mass to Ib

IB = Ib ;

% position of moving mass a1 relative to " b frame "

r_a1b_b = [r_a1(1);r_a1(2);r_a1(3)];

% position of moving mass a2 relative to " b frame "

r_a2b_b = [r_a2(1);r_a2(2);r_a2(3)];

% position of the primary rigid body center of mass relative to the

% instantaneous center of mass

r_c_bc = -(1/M_T)*(ma1*r_a1b_b + ma2*r_a2b_b);

% position of moving mass a1 relative to " c frame "

r_a1c = r_a1b_b + r_c_bc;

% position of moving mass a2 relative to " c frame "

r_a2c = r_a2b_b + r_c_bc;

% skew matrix of primary rigid body position vector expressed in " c frame "

TILDE_r_bc = [0,-r_c_bc(3),r_c_bc(2);r_c_bc(3),0,-r_c_bc(1);-r_c_bc(2),r_c_bc(1),0];

% skew matrix of a1 position vector expressed in " c frame "

TILDE_r_a1c = [0,-r_a1c(3),r_a1c(2);r_a1c(3),0,-r_a1c(1);-r_a1c(2),r_a1c(1),0] ;

% skew matrix of a2 position vector expressed in " c frame "

TILDE_r_a2c = [0,-r_a2c(3),r_a2c(2);r_a2c(3),0,-r_a2c(1);-r_a2c(2),r_a2c(1),0];

% velocity of moving mass a1 rel. to " b frame "

v_a1b_b =v_a1;

% velocity of moving mass a2 rel. to " b frame "

v_a2b_b = v_a2;

% velocity of primary rigid body rel to " c frame "

v_bc = -(1/M_T)*(ma1*v_a1b_b + ma2*v_a2b_b);

% velocity of moving mass a1 rel to " c frame "

v_a1c = v_a1b_b + v_bc;

% velocity of moving mass a2 rel to "b frame" expressed in " c frame "

v_a2c = v_a2b_b + v_bc;

% acceleration of moving mass a1 rel. to " b frame "

DOT_v_a1b = a_a1;

% acceleration of moving mass a1 rel. to " b frame "

DOT_v_a2b = a_a2;

% acceleration of rigid body B rel. to " c frame "

DOT_vbc = -(1/M_T)*(ma1*DOT_v_a1b + ma2*DOT_v_a2b);

% acceleration of moving mass a1 rel. to " c frame "

DOT_v_a1c = DOT_v_a1b + DOT_vbc;

% acceleration of moving mass a2 rel. to " c frame "

DOT_v_a2c = DOT_v_a2b + DOT_vbc;

wcix=wci(1); % components of wci input

wciy=wci(2);

wciz=wci(3);

% skew form of angular velocity

TILDE_w_ci = [0 -wciz wciy

wciz 0 -wcix

-wciy wcix 0 ];

% Time Varying Inertia matrix of composite body for treatement of moving

% masses as point-masses instead of rigid bodies

% no need for change of basis : R_cb*(IB)*(R_bc) becuase for concept the

% "b frame" and "c frame" have identity alignment

Ic = (IB) ...

+ mB*((r_c_bc)'*(r_c_bc)*(eye(3))-(r_c_bc)*(r_c_bc)') ...

+ ma1*((r_a1c)'*(r_a1c)*(eye(3)) -(r_a1c)*(r_a1c)') ...

+ ma2*((r_a2c)'*(r_a2c)*(eye(3)) -(r_a2c)*(r_a2c)');

% First derivative of composite body inertia matrix

Dot_Ic_MT= mB*((2*(r_c_bc)'*(v_bc)*(eye(3))-(r_c_bc)*(v_bc)'-(v_bc)*(r_c_bc)'))...

+ ma1*((2*(r_a1c)'*(v_a1c)*(eye(3))-(r_a1c)*(v_a1c)'-(v_a1c)*(r_a1c)')) ...

+ ma2*((2*(r_a2c)'*(v_a2c)*(eye(3))-(r_a2c)*(v_a2c)'-(v_a2c)*(r_a2c)'));

torque =-(TILDE_w_ci)*(Ic)*(wci) -(Dot_Ic_MT)*(wci) + ...

- mB*(TILDE_r_bc)*(DOT_vbc)...

- ma1*(TILDE_r_a1c)*(DOT_v_a1c)...

- ma2*(TILDE_r_a2c)*(DOT_v_a2c)...

- mB*(TILDE_w_ci)*(TILDE_r_bc)*(v_bc) ...

- ma1*(TILDE_w_ci)*(TILDE_r_a1c)*(v_a1c) ...

- ma2*(TILDE_w_ci)*(TILDE_r_a2c)*(v_a2c);

% ---------- computing inverse of Ic : --------------------------%

a = Ic(1,1);

b = Ic(1,2);

c = Ic(1,3);

d = Ic(2,1);

e = Ic(2,2);

f = Ic(2,3);

g = Ic(3,1);

h = Ic(3,2);

k = Ic(3,3);

A = (e*k -f*h);

B = (f*g-d*k);

C = (d*h - e*g);

D = (c*h -b*k);

E = (a*k -c*g);

F = (g*b - a*h);

G = (b*f -c*e);

H = (c*d - a*f);

K = (a*e - b*d);

det_Ic = det(Ic);

transp_cof_mat = [A,D,G;

B,E,H;

C,F,K];

inv_Ic = (1/det_Ic)*transp_cof_mat;

dwdt= inv_Ic*torque;

% nonlinear system of equations

f = [dwdt(1);dwdt(2)];

%% Linearization

xhat = [wcx;wcy];

uhat = [xm1;ym2];

A = zeros(2,2);

A = sym(A);

for i = 1:length(f)

for j = 1:length(f)

A(i,j) = diff(f(i),xhat(j));

end

end

% Linear Control Influence Matrix

B = zeros(2,2);

B = sym(B);

for i = 1:length(f)

for j = 1:length(uhat)

B(i,j) = diff(f(i),uhat(j));

end

end

% equilbirum values

gamma = sym('gamma','real');

subvals_sym = [0,0,gamma,0,0,0,0,0,0,ma1,ma2,mB,hs,ws,ds,0,0] ; % [wcx,wcy,wcz,xm1,dxm1,ddxm1,ym2,dym2,ddym2,ma1,ma2,mB,h,w,d,u1,u2]

[wcx,wcy,wcz,xm1,dxm1,ddxm1,ym2,dym2,ddym2,ma1,ma2,mB,hs,ws,ds,u1,u2] = exchangeValues_sym(subvals_sym);

Asym = subs(A)

Bsym = subs(B)

%Numerical substitution as part of linearization

gamma = 2*1.57; % 180 degree per seconds yaw spin rate

ma1 = 1.36; % 3 pounds for mass 1

ma2 = 1.36; % 3 pounds for mass 2

mB = 13.6; % 30 pounds for base body

hs = 0.4572; % height of satelltie sructure = 1.5 feet

ws = 0.4572; % width of satellite structure = 1.5 feet

ds = 0.4572; % depth of satellite strucutre = 1.5 feet

subvals_num = [0,0,gamma,0,0,0,0,0,0,ma1,ma2,mB,hs,ws,ds,0,0] % [wcx,wcy,wcz,xm1,dxm1,ddxm1,ym2,dym2,ddym2,ma1,ma2,mB,h,w,d,u1,u2]

[wcx,wcy,wcz,xm1,dxm1,ddxm1,ym2,dym2,ddym2,ma1,ma2,mB,hs,ws,ds,u1,u2] = exchangeValues_sym(subvals_num);

A = double(subs(A))

B = double(subs(B))

% Save A, B, and K matrices

save('Amat_2m.mat','A')

save('Bmat_2m.mat','B')

% save static parameters

save('static_params.mat','hs','ws','ds','ma1','ma2','mB')

% hs = sym('hs','real');

% ws = sym('ws','real');

% ds = sym('ds','real');

% ma1 = sym('ma1','real');

% ma2 = sym('ma2','real');

% mB = sym('mB','real');

%% Trying multiple feedback gains

% K1

Q = 1000*eye(2,2);

R = 9000*eye(2,2);

[K1,Acl,E] = lqr(A,B,Q,R)

save('K1.mat','K1')

% K2

Q = [10, 0;

0, 100];

R = [500,0;

0, 1000];

[K2,Acl,E] = lqr(A,B,Q,R)

save('K2.mat','K2')

% K3

Q = [100, 0;

0, 1000];

R = [5000,0;

0, 1000];

[K3,Acl,E] = lqr(A,B,Q,R)

save('K3.mat','K3')

% K4

Q = [100, 0;

0, 1000];

R = [50000,0;

0, 10000];

[K4,Acl,E] = lqr(A,B,Q,R)

save('K4.mat','K4')

% K5

Q = [100, 0;

0, 100];

R = [50000,0;

0, 50000];

[K5,Acl,E] = lqr(A,B,Q,R)

save('K5.mat','K5')

%K6

Q = [1000, 0;

0, 10000];

R = [50000,0;

0, 100000];

[K6,Acl,E] = lqr(A,B,Q,R)

save('K6.mat','K6')

% K7

Q = [1000, 0;

0, 10000];

R = [500000,0;

0, 100000];

[K7,Acl,E] = lqr(A,B,Q,R)

save('K7.mat','K7')

% K8

Q = [10000, 0;

0, 10000];

R = [500000,0;

0, 100000];

[K8,Acl,E] = lqr(A,B,Q,R)

save('K8.mat','K8')

% K9

Q = [10000, 0;

0, 100000];

R = [500000,0;

0, 100000];

[K9,Acl,E] = lqr(A,B,Q,R)

save('K9.mat','K9')

% K10

Q = [10000, 0;

0, 100000];

R = [5000000,0;

0, 100000];

[K10,Acl,E] = lqr(A,B,Q,R)

save('K10.mat','K10')