# 0130實驗結果
## 只有考慮J2影響，無阻力
### 原本程式J2公式與計算方式

% J2 perturbation
    z2 = rvector(3)^2;
    r2 = r^2;
    factor = 1.5 * J2 * mu * RE^2 / r2^2;
    ax_J2 = rvector(1) / r * (5 * z2 / r2 - 1) * factor;
    ay_J2 = rvector(2) / r * (5 * z2 / r2 - 1) * factor;
    az_J2 = rvector(3) / r * (5 * z2 / r2 - 3) * factor;
    a_J2 = [ax_J2; ay_J2; az_J2];

### 更新後程式J2公式與計算方式
公式來源:Spacecraft Formation Flying: Dynamics, Control and Navigation

% J2 perturbation
    Z = rvector(3);
    Z2 = Z^2;
    r2 = r^2;
    r3 = r^3;
    r5 = r^5;
    
    % Calculate the J2 perturbation for each component
    ax_J2 = (-mu* rvector(1) / r3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    ay_J2 = (-mu* rvector(2) / r3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    az_J2 = (-mu* rvector(3) / r3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 3));

    
    % The total gravitational acceleration including J2 perturbation
    a_J2 = [ax_J2; ay_J2; az_J2];

### 兩個數學式展開
只是差在有沒有先加入重力影響，其他數學式基本上是相同的
### 比較新、舊 a_J2的差異
平均誤差1.12995E-08

### 主程式

% FORMOSAT 5
% 1 42920U 17049A   23352.88591631  .00000349  00000-0  99784-4 0  9991
% 2 42920  98.3089  68.1091 0010834  12.0422 348.1030 14.50778162334536
clc;
clear;
close all;
warning('off','all')
G = 6.67430e-20; % km^3/(kg * s^2)
m1 = 5.97219e24; % kg
m2 = 475; %Mass of the satellite kg
%mu = G * m1; % km^3/s^2
mu = 398600.44189; % km^3/s^2
RE = 6378.12; % km
utc = [2000 1 1 11 59 28];

Cd = 2.2; % Example value for drag coefficient
A = 2.120773439350248; % Example value for cross-sectional area in square meters
r0 = [2646.803786119759 6581.416179767096 14.64870964671942]; % km
v0 = [0.9992138358820861 -0.4201725262436247 7.421268328550636]; % km/s
Y0 = [r0 v0];


t0 = 0; % seconds
tf = 86400*14; % seconds, period of one orbit
time_step = 60;  % specify the time step in seconds

num_steps = length(t0:time_step:tf);


global a_J2_values;
a_J2_values = zeros(num_steps, 3); % Assuming 'num_steps' is the total number of time steps

options = odeset('RelTol', 1e-13, 'AbsTol', 1e-13, 'MaxStep', time_step);
[t, Y] = ode89(@(t, Y) relative_motion_drag_J2(t, Y, mu, Cd, A, m2, round(t/time_step) + 1), t0:time_step:tf, Y0, options);




rvec = Y(:, 1:3);
vvec = Y(:, 4:6);

rmag = vecnorm(rvec, 2, 2);
%vmag = vecnorm(vvec, 2, 2);
altitude = rmag - RE;



% Plot altitude as a function of time
figure;
plot(t/86400, altitude);
xlabel('Time (day)');
ylabel('Altitude (km)');
title('Altitude vs. Time');

% Create a new figure for the 3D orbit
figure;
[xx, yy, zz] = sphere(200);
surf(RE * xx, RE * yy, RE * zz)
colormap(light_blue)
clim([-RE / 100 RE / 100])
shading interp

% Draw and label the X, Y, and Z axes
line([0 2 * RE], [0 0], [0 0]);
text(2 * RE + RE / 10, 0, 0, 'x')
line([0 0], [0 2 * RE], [0 0]);
text(0, 2 * RE + RE / 10, 0, 'y')
line([0 0], [0 0], [0 2 * RE]);
text(0, 0, 2 * RE + RE / 10, 'z')

hold on
plot3(rvec(:, 1), rvec(:, 2), rvec(:, 3), 'k')
plot3(rvec(1, 1), rvec(1, 2), rvec(1, 3), 'go'); % Starting point
plot3(rvec(end, 1), rvec(end, 2), rvec(end, 3), 'ro'); % Ending point

% Specify some properties of the graph
grid on
axis equal
xlabel('km')
ylabel('km')
zlabel('km')
title('3D Orbit');

function map = light_blue
    r = 0.8;
    g = r;
    b = 1.0;
    map = [r g b; 0 0 0; r g b];
end



function Ydot = relative_motion_drag_J2(t, Y, mu, Cd, A, m,step_index)
    global a_J2_values;
    % Earth's J2 coefficient and equatorial radius
    J2 = 1.08262668e-3;
    RE = 6378.12; % km
    Omega = 7.2921*10^-5;
    Omega = [0.0, 0.0, Omega]';
    % Extract position and velocity vectors
    rvector = Y(1:3);
    vvector = Y(4:6);

    % Standard gravitational acceleration
    r = norm(rvector);
    a_gravity = -mu * rvector / r^3;

    % J2 perturbation
    Z = rvector(3);
    Z2 = Z^2;
    r2 = r^2;
    r3 = r^3;
    r5 = r^5;
    
    % Calculate the J2 perturbation for each component
    ax_J2 = (-mu* rvector(1) / r3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    ay_J2 = (-mu* rvector(2) / r3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    az_J2 = (-mu* rvector(3) / r3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 3));

    
    % The total gravitational acceleration including J2 perturbation
    a_J2 = [ax_J2; ay_J2; az_J2];
    %a_J2_values(t, :) = a_J2;
    a_J2_values(step_index, :) = a_J2;

    % Calculate the relative velocity
    v_icrf = Y(4:6);
    r_icrf = Y(1:3);
    if isrow(r_icrf)
        r_icrf = r_icrf';
    end
    if isrow(v_icrf)
        v_icrf = v_icrf';
    end
    % Extracted mean motion from TLE (revolutions per day)
    n_TLE = 14.50778162; 
    
    % Convert mean motion to radians per second
    n = n_TLE * (2 * pi) / 86400; 

    % Rotation angle about the Z-axis
    % fprintf("time:%d\n",t);
    theta = n * t;
    % fprintf("theta:%d\n",theta);
    % Rotation matrix about the Z-axis
    Rz = [cos(theta), -sin(theta), 0;
          sin(theta),  cos(theta), 0;
                  0,           0, 1];
    
    % Now you can use Rz to rotate your r_icrf vector from body to ECI frame
    r_icrf_eci = Rz * r_icrf;  % Replace r_icrf_body with your vector
    % Calculate the relative velocity
    v_rel = (v_icrf - cross(Omega, r_icrf_eci));
    
    % Calculate the drag acceleration
    v_rel_mag = norm(v_rel);
    
     % Call atmospheric density function with additional arguments
    x = r_icrf(1);
    y = r_icrf(2);
    z = r_icrf(3);
    simulationTime = t; % Assuming 't' is your simulation time in seconds since start
    rho = atmospheric_density(norm(r_icrf) - RE); 
    %fprintf("%f kg/m^3\n", rho);
    %a_drag = -0.5 *( (Cd * A*10^-6 )/ m )* (1.68e-11*10^9 )* (v_rel_mag)^2 * ((v_rel) / (v_rel_mag));
    %disp(a_drag);
    a_drag = -0.5 *( (Cd * A*10^-6 )/ m )* (rho*10^9 )* (v_rel_mag)^2 * ((v_rel) / (v_rel_mag));
    % Total acceleration 
    %a_total = a_gravity + a_J2 + a_drag;
    a_total =  a_J2;
    
    %a_total = a_gravity + a_J2;
    %disp(a_total);
    % Output derivative of state vector
    Ydot = [vvector; a_total];
end

function rho = atmospheric_density(altitude)
    % Placeholder values - you need to replace these with actual calculations or inputs
    

    % atmosnrlmsise00 function call for accurate atmospheric density
    %[T, rho] = atmosnrlmsise00(altitude * 1000, 45,-50,2007,4,0);
    %[T,a,P,rho,nu] = atmosisa(altitude*1000);
    [T,rho] = atmosnrlmsise00(altitude*1000,45,-50,2023,360,0);
    rho = rho(6);
end

## result
## 對STK比較
#### STK(HPOP、EGM2008 重力模型) 與MATLAB誤差
#### STK(HPOP、JGM2重力模型) 與MATLAB誤差
#### STK(J2 disturbance) 與MATLAB誤差
#### 放大檢視200個資料點週期

## 結論
* SKT裡面的J2長怎樣(J2公式)還不確定
* JGM2 &EGM2008 看起來差不多
* STK裡面的J2 跟我自己寫的MATLAB 有個每日循環回去0誤差之現象，需要再探討

https://help.agi.com/stk/12.4.0/index.htm#gator/ab-engine.htm?TocPath=Capabilities%257CAstrogator%257CAstrogator%2520Components%257C_____6
J2000
2682.4882685250249779 km
6566.9639001485847984 km
8.1007149823713345 km
1.0143378782946217 km/sec
-0.4253020701657230 km/sec
7.4189240233167526 km/sec
https://help.agi.com/stk/#gator/eq-diffcorr.htm?TocPath=Capabilities%257CAstrogator%257CTechnical%2520Notes%257C_____1
https://help.agi.com/stk/#training/Astro_LowThrust.htm?TocPath=Training%257CLevel%25203%2520-%2520Focused%257CFeature%2520Specific%257CAstrogator%257C_____12