%------------------SIMULINK SIMULATION DATA LOADING------------------------

clc;clear all;close all;
DT=1/10; %simulation time step
Tend=1600; %[s]
m=176;
UP_T=1;%state updating period
T_T=1; %control torque updating period

Ksole=1.327e20;     %[m^3/s^2]
AU=1.4962e11;     %1AU [m]

mu=3.98e14;
Rterra=6371.2e3; 
iT=23.45*pi/180;

days=25;%number of days after the vernal equinox 20/3
teta_terra=days/365*2*pi;

Ix=30.1;  %kg.m^2
Iy=18;  % -y is firing axis!!!!
Iz=27.8;
Ix_c=Ix*1.1;
Iy_c=Iy*1.1;  
Iz_c=Iz*1.1;

%------------------------------WHEELS DATA----------------------------------
Irx=10e-4; %[kg.m^2]
Iry=10e-4; 
Irz=10e-4; 
wrx=0;
wry=0; 
wrz=0;
hrx=Irx*wrx;
hry=Iry*wry;
hrz=Irz*wrz;
wr0x= wrx *[1 0 0]'; %rad/sec
wr0y= wry *[0 1 0]'; 
wr0z= wrz *[0 0 1]';
Aw=eye(3);  %matrix defining wheels orientation (body frame) HP wheels axes coincident with body axis!  
Ahrx=Aw*Irx*wr0x;
Ahry=Aw*Iry*wr0y;
Ahrz=Aw*Irz*wr0z;

%------------------------INITIAL CONDITION (PERIGEEEEE)--------------------------

%initial orbit keplerian parameters
W=0;
om=0;
I=0;
an=0*pi/180;%true anomaly (teta)
sma=24582e3; 
ec=0.723;
s15=sin(15*pi/180);
c15=cos(15*pi/180);
pv=9.88e3; %perigee velocity [m/s] 
pr=sma*(1-ec);
pw=pv/pr;   %perigee angular rate

w0x=0;
w0y=0;
w0z=0;

%orbital/inertial frame
R1=[cos(W) sin(W) 0;-sin(W) cos(W) 0;0 0 1];%Mi porto sull'asse dei nodi
R2=[1 0 0;0 cos(I) sin(I);0 -sin(I) cos(I)];%Mi inclino sul piano orbitale
R3=[cos(om+an) sin(om+an) 0;-sin(om+an) cos(om+an) 0;0 0 1];%Mi porto sul versore di pericentro( no sulla radiale)
R=R3*R2*R1;%la R porta dalla terna inerziale alla terna orbitale
r_Tsat=sma*(1-ec^2)/(1+ec*cos(an));

r_Ts=R'*[r_Tsat  ;  0  ; 0  ]; %Earth-s/c vector at t=0 (inertial frame)
r_TS=[AU*cos(teta_terra) ; -AU*sin(teta_terra)*sin(iT) ; AU*sin(teta_terra)*cos(iT) ];%Erth-Sun
r_sS=r_TS-r_Ts; % s/c to sun vector in the inertial frame (non sarebbe costanteeeeeeeeeee)
sS=r_sS/norm(r_sS);
sS=[0  0  1]'; %togli
MV=[1/sqrt(2)  0  1/sqrt(2)]'; %vettore proveniente dal secondo sensore

q4= 0.5 * (1+R(1,1)+R(2,2)+R(3,3))^0.5; %at t=0 body axes meet orbital axes!!
q1=(R(2,3)-R(3,2))/4/q4;
q2=(R(3,1)-R(1,3))/4/q4;
q3=(R(1,2)-R(2,1))/4/q4;

b1=.01145;b2=.01145;b3=.01145; %initial gyro bias % an initial manoeuver is performed to estimate bias


T1=0;T2=0;T3=0;
x0=[q1 q2 q3 q4 w0x w0y w0z]';

qm1=-0.5*[0 w0z -w0y w0x ; -w0z 0 w0x w0y ; w0y -w0x 0 w0z ; -w0x -w0y -w0z 0]*[q1 q2 q3 q4]'*DT+[q1 q2 q3 q4]';
dqm1=0.5*[0 w0z -w0y w0x ; -w0z 0 w0x w0y ; w0y -w0x 0 w0z ; -w0x -w0y -w0z 0]*qm1;
qm2=qm1-dqm1*DT;
dqm2=0.5*[0 w0z -w0y w0x ; -w0z 0 w0x w0y ; w0y -w0x 0 w0z ; -w0x -w0y -w0z 0]*qm2;
dwm1=[2.222e-5 0 0]'; 
dwm2=[2.222e-5 0 0]';

%--------------------------------DISTURBANCES------------------------------

%Magnetic field
MFD_amp=4.813e-5; %[Nm]
D_v=rand(3,1)-.5*ones(3,1); %Magnetic moment vector [A*m^2]
D_v=[-4 ; 1 ; -8]; %togli
D_v=D_v/norm(D_v);  %D is assumed to be 1 A*m^2
B_v=rand(3,1)-.5*ones(3,1); %Magnetic field vector [Tesla] 
B_v=[0 ; 0 ; 1]; %togli
MFD=cross(D_v,B_v)*MFD_amp;%orbital frame!
%Drag
rho550min=3.63e-14;
rho550mean=2.21e-13;
rho550max=9.25e-13; %max value [Kg/m3]
arm=.339;  %thrust-CG arm %[m]
Drag_arm=arm/2; %[m]
face=pi*arm^2; %s/c surface (normal to rolling axis) [m2]
CD=2.3; % change
bz=arm/3;
bx=arm/4;
Dz=.5*pv^2*face*CD*bz;
Dx=.5*pv^2*face*CD*bx;

%------------------------COLD GAS THRUSTERS--------------------------------
Tmen=22;
Tmend=22*cos(10*pi/180);
Arot=[1 0 0; 0 0 -1; 0 1 0]; %Yaxis becomes Zaxis. Zaxis becomes -Yaxis
XCogWet = Arot*[0.0,-0.0,0.4455]'; % m
CG_shift=[.0392;.1212;-.0847];
XCogWet2=XCogWet+CG_shift;% m

XCogWet_dist = Arot*[0.0377,-0.0687,0.3842]';

%XCogWet2=XCogWet_dist*1.15;%


[off_T ,men_theo_T , men_eff_T]=est_T(Arot , XCogWet ,XCogWet_dist, Tmen);
[off_T2 ,men_theo_T2 , men_eff_T2]=est_T2(Arot , XCogWet2 ,XCogWet_dist, Tmen);

%off_T = each column is the THEORICAL torque (in body axes) introduced by each thruster STOPPING 
%men_theo_T = each column is the THEORICAL torque (in body axes) introduced by each thruster FIRING
%men_eff_T = each column is the REAL torque (in body axes) introduced by each thruster FIRING 
%off_eff_T = matrix : each column is the REAL torque (in body axes) introduced by each thruster STOPPING 
%ON_dist_torque = torque introduced by all the engines firing 
Brot=pinv(off_T);

Gamma=.65;
mot=.02;
T_min_t=mot;
ON_dist_torque=men_eff_T2(:,1)+men_eff_T2(:,2)+men_eff_T2(:,3)+men_eff_T2(:,4);
ON_dist_torque_e=men_theo_T2(:,1)+men_theo_T2(:,2)+men_theo_T2(:,3)+men_theo_T2(:,4);

%------------------------------CONTROL-------------------------------------

KKx=1;KKy=1;KKz=1;
Td=.7;

%X-axis
Kix=.2;  Kx=1.55; 
phi_d=(37+.7*.7*180/pi)*pi/180; %desired phase margin [rad]
a=(1+sin(phi_d/2))/(1-sin(phi_d/2));
omx=sqrt(Kx*(a)^(3/2)/Ix);
Tx=1/omx/(a)^.25;
Cx=tf([(a*Tx)^2 2*a*Tx 1]*Kx,[Tx^2 2*Tx 1]);
[numx,denx]=c2dm([(a*Tx)^2 2*a*Tx 1]*Kx,[Tx^2 2*Tx 1],.1);
a2x=numx(1); a1x=numx(2) ; a0x=numx(3);
b1x=denx(2) ; b0x=denx(3);


%Y-axis
Kiy=.1;  Ky=.9; 
phi_d=(37+.7*.7*180/pi)*pi/180; %desired phase margin [rad]
a=(1+sin(phi_d/2))/(1-sin(phi_d/2));
omy=sqrt(Ky*(a)^(3/2)/Iy);
Ty=1/omy/(a)^.25;
Cy=tf([(a*Ty)^2 2*a*Ty 1]*Ky,[Ty^2 2*Ty 1]);
[numy,deny]=c2dm([(a*Ty)^2 2*a*Ty 1]*Ky,[Ty^2 2*Ty 1],.1);
a2y=numy(1); a1y=numy(2) ; a0y=numy(3);
b1y=deny(2) ; b0y=deny(3);


%Z-axis
Kiz=.19;  Kz=1.47; 
phi_d=(37+.7*.7*180/pi)*pi/180; %desired phase margin [rad]
a=(1+sin(phi_d/2))/(1-sin(phi_d/2));
omz=sqrt(Kz*(a)^(3/2)/Iz);
Tz=1/omz/(a)^.25;
Cz=tf([(a*Tz)^2 2*a*Tz 1]*Kz,[Tz^2 2*Tz 1]);
[numz,denz]=c2dm([(a*Tz)^2 2*a*Tz 1]*Kz,[Tz^2 2*Tz 1],.1);
a2z=numz(1); a1z=numz(2) ; a0z=numz(3);
b1z=denz(2) ; b0z=denz(3);


q_t=[q1 q2 q3 q4]';%maintain the initial attitude, which was previously achieved



%--------------------------ESTIMATOR PARAMETERS---------------------------
%covariance of error in the state estimate (initial guess)
SD=(9e-13);
Q(1:3,1:3)=(eye(3)*SD*DT^3)/12;
Q(4:6,4:6)=eye(3)*SD*DT;
Q(1:3,4:6)=eye(3)*.25*SD*DT^2;  Q(4:6,1:3)=eye(3)*.25*SD*DT^2;

Pfg=diag([1e-9 1e-9 1e-9 1e-9 1e-9 1e-9]);

scs_acc=(3e-1)*pi/180;%scs accuracy [rad]
SS_acc=(3e-1)*pi/180;
sig_SS=((scs_acc)^2);
sig_g=1e-6;
RK=[];
RK(1:3,1:3)=eye(3)*3.0462e-014; RK(4:6,4:6)=eye(3)*3.0462e-014; 
RK(7:9,7:9)=eye(3)*sig_g;