gain_final.m

%% Define Gain Matrix from Embry-Riddle three-axis control paper
        
sigma1 = (Jy-Jz)/Jx;
sigma2 = (Jx-Jz)/Jy;
sigma3 = (Jy-Jx)/Jz;
H = diag([2, 2.5, 0.5]);

mu_m=10^16;
r=a;
A1 = [-4*(n^2)*sigma1, 0, 0;
      0, -3*(n^2)*sigma2, 0;
      0, 0, -(n^2)*sigma3];
  
A2 = [0, 0, n*(1-sigma1);
        0, 0, 0;
        n*(sigma3-1), 0, 0];

A = [zeros(3), eye(3);A1, A2];
B = ((mu_m^2)/(2*r^6))*[zeros(3);(inv(J_c)*H)];

Q=[eye(3)/(n^2), zeros(3);zeros(3), eye(3)/n];
R=eye(3);
K=lqr(A,B,Q,R)
eigen = eigs(A-B*K);

%% Dynamic H -- magnetic model
% H(1,1)=4*sin(inc)^2*sin(n*t)+cos(inc)^2;
% H(2,2)=(cos(n*t)^2+ 4*sin(n*t)^2)*sin(inc)^2;
% H(3,3)=sin(inc)^2*cos(n*t)^2+cos(inc)^2;
% H(1,2)=-0.5*sin(2*inc)*cos(n*t);
% H(2,1)=H(1,2);
% H(1,3)=-sin(inc)^2*sin(2*n*t);
% H(3,1)=H(1,3);
% H(2,3)=-sin(2*inc)*sin(n*t);

% H(1,1)=4*sin(inc)^2*.5+cos(inc)^2;
% H(2,2)=(.5+ 4*.5)*sin(inc)^2;
% H(3,3)=sin(inc)^2*.5+cos(inc)^2;
% H(1,2)=-0.5*sin(2*inc)*0;
% H(2,1)=H(1,2);
% H(1,3)=-sin(inc)^2*0;
% H(3,1)=H(1,3);
% H(2,3)=-sin(2*inc)*0;

Kfinal=[1015.81637123020,8.04611892244142e-14,-453.958044168676,384003.046505704,1.44940442674718e-11,-187075.587093135;7.60272150440681e-14,1186.23970592244,-2.35978593865247e-13,1.08705332006039e-11,448289.486640409,9.49012273578315e-11;453.958044168675,-1.67413778929006e-14,600.766687834337,-20043.8129028358,1.35573181939759e-11,795987.850800695]
Afinal=[0,0,0,1,0,0;0,0,0,0,1,0;0,0,0,0,0,1;3.26658658709613e-06,0,0,0,0,0.00184463935812897;0,2.93992792838652e-06,0,0,0,0;0,0,-3.49991420046014e-07,-0.000790559724912417,0,0]
Bfinal=[0,0,0;0,0,0;0,0,0;1.57407015004541e-08,0,0;0,1.18055261253406e-08,0;0,0,1.68650373219151e-09]