# Spacecraft attitude control using MPC
## Simple Elliptical Orbits
### Nonlinear system dynamic equations

Nonlinear system dynamic equations:
$\ddot{x}=-\mu\frac{x}{r^3}$
$\ddot{y}=-\mu\frac{y}{r^3}$
$\ddot{z}=-\mu\frac{z}{r^3}$


## Simple Elliptical Orbits with J2 perturbation
## Add Drag Effect

#### 阻力計算

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;

#### 大氣密度

function rho = atmospheric_density(altitude)
    [T,rho] = atmosnrlmsise00(altitude*1000,45,-50,2007,4,0);
    rho = rho(6);
end

## 初始TLE
2023-12-18T21:15:43.169Z

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

## 模擬結果-MATLAB(ode45)
## 模擬結果-GMAT(RK89)
## 長週期模擬結果-GMAT(RK89)
## 長週期模擬結果-STK
## 長週期模擬結果-MATLAB(ODE45)

## 完整程式碼

% 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
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*365; % seconds, period of one orbit
time_step = 60;  % specify the time step in seconds
options = odeset('RelTol', 1e-9, 'AbsTol', 1e-9, 'MaxStep', time_step);
[t, Y] = ode45(@(t, Y) relative_motion_drag_J2(t, Y, mu, Cd, A, m2), t0:time_step:tf, Y0, options);



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

rmag = vecnorm(rvec, 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)
    % Earth's J2 coefficient and equatorial radius
    J2 = 1.08263e-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
    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];

    % 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_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,2007,4,0);
    rho = rho(6);
end

TLE(name='FORMOSAT-5', norad='42920', classification='U', int_desig='17049A', epoch_year=2023, epoch_day=347.57530379, dn_o2=1.98e-06, ddn_o6=0.0, bstar=6.1613e-05, set_num=999, inc=98.3095, raan=62.8656, ecc=0.001116, argp=27.6807, M=332.4979, n=14.50773973, rev_num=33376)
7101.2587517904385

## ode45 but only J2

% 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;
format long
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 = 0.001082626724393;
    %0.001082626724393
    
    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];
    disp(size(Ydot))
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

## ode4 but only J2

% FORMOSAT 5 TLE to Propagate Satellite Orbit with J2
% 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;
format long
warning('off','all')

% Constants
G = 6.67430e-20; % Gravitational constant in km^3/(kg * s^2)
m1 = 5.97219e24; % Mass of the Earth in kg
RE = 6378.12; % Earth's radius in km
mu = 398600.44189; % Earth's gravitational parameter in km^3/s^2
Cd = 2.2; % Drag coefficient
A = 2.120773439350248; % Cross-sectional area of the satellite in square meters

% Initial conditions
r0 = [2646.803786119759 6581.416179767096 14.64870964671942]; % Initial position vector in km
v0 = [0.9992138358820861 -0.4201725262436247 7.421268328550636]; % Initial velocity vector in km/s
Y0 = [r0 v0]; % Initial state vector

% Time settings
t0 = 0; % Initial time in seconds
tf = 86400*14; % Final time in seconds (14 days)
N = 20161; % Number of time steps
h = (tf-t0)/(N-1); % Time step size

% Propagate the orbit using 4th order Runge-Kutta method
Y = ode4(@relative_motion_drag_J2, t0, 60, tf, Y0);

% Extract position vectors
rvec = Y(:, 1:3);

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


% 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)
    % Earth's J2 coefficient and gravitational parameter
    J2 = 0.001082626724393;
    mu = 398600.44189; % km^3/s^2
    RE = 6378.12; % Earth's radius in km
    
    % Extract position and velocity vectors
    rvector = Y(1:3);
    vvector = Y(4:6);

    % J2 perturbation calculation
    r = norm(rvector);
    Z = rvector(3);
    Z2 = Z^2;
    r2 = r^2;
    r5 = r^5;
    ax_J2 = (-mu* rvector(1) / r^3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    ay_J2 = (-mu* rvector(2) / r^3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    az_J2 = (-mu* rvector(3) / r^3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 3));
    a_J2 = [ax_J2; ay_J2; az_J2];

    % Total acceleration
    a_total =  a_J2;
    
    % Output derivative of state vector
    Ydot = [vvector a_total'];
end

## 在飛行方向加delta V

% FORMOSAT 5 TLE to Propagate Satellite Orbit with J2
% 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;
format long
warning('off','all')

% Constants
G = 6.67430e-20; % Gravitational constant in km^3/(kg * s^2)
m1 = 5.97219e24; % Mass of the Earth in kg
RE = 6378.12; % Earth's radius in km
mu = 398600.44189; % Earth's gravitational parameter in km^3/s^2
Cd = 2.2; % Drag coefficient
A = 2.120773439350248; % Cross-sectional area of the satellite in square meters

% Initial conditions
r0 = [2646.803786119759 6581.416179767096 14.64870964671942]; % Initial position vector in km
v0 = [0.9992138358820861 -0.4201725262436247 7.421268328550636]; % Initial velocity vector in km/s
Y0 = [r0 v0]; % Initial state vector

% Time settings
t0 = 0; % Initial time in seconds
tf = 86400*14; % Final time in seconds (14 days)
N = 20161; % Number of time steps
h = (tf-t0)/(N-1); % Time step size

% Propagate the orbit using 4th order Runge-Kutta method
[t, Y] = ode4(@relative_motion_drag_J2, t0, 60, tf, Y0);
% Conditionally print a string when t == 86400
% Find the index where the time is just over one day (86400 seconds)



% Extract position vectors
rvec = Y(:, 1:3);
vvec = Y(:, 4:6);
% Calculate altitude
rmag = vecnorm(rvec, 2, 2);
vmag = vecnorm(vvec, 2, 2);
altitude = rmag - RE;


% 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)
    
    % Earth's J2 coefficient and gravitational parameter
    J2 = 0.001082626724393;
    mu = 398600.44189; % km^3/s^2
    RE = 6378.12; % Earth's radius in km
    
    % Extract position and velocity vectors
    rvector = Y(1:3);
    vvector = Y(4:6);
    
    % J2 perturbation calculation
    r = norm(rvector);
    Z = rvector(3);
    Z2 = Z^2;
    r2 = r^2;
    r5 = r^5;
    ax_J2 = (-mu* rvector(1) / r^3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    ay_J2 = (-mu* rvector(2) / r^3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    az_J2 = (-mu* rvector(3) / r^3 ) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 3));
    a_J2 = [ax_J2; ay_J2; az_J2];
    a_thrust = [0; 0; 0];

    thrust = 0.01; % Constant thrust in Newtons
    burn_duration = 60000; % Duration of the burn in seconds
    satellite_mass = 475; % Mass of the satellite in kg (you will need the actual mass here)
    thrust_acceleration = thrust / satellite_mass*1e-3; % in km/s^2, make sure to convert units if necessary
    persistent burnStartTime; % Initialize a persistent variable to record the start time of the burn
    
    if isempty(burnStartTime) && t >= 86400*7
        burnStartTime = t; % Record the start time of the burn
    end
    if ~isempty(burnStartTime) && t >= burnStartTime && t <= burnStartTime + burn_duration
        vvector_normalized = vvector / norm(vvector);
        a_thrust = (thrust_acceleration * vvector_normalized)';
       
        %disp(t)
    end
    % Total acceleration
    a_total =  a_J2 + a_thrust;
    
    % Output derivative of state vector
    Ydot = [vvector a_total'];
end