https://drive.google.com/file/d/12otZAVXIasgQcCCzFlmTI3J2DzZ9yMAO/view?usp=sharing
https://drive.google.com/drive/folders/19iHv0DyPddHOw_1snCHtfeVBBQaaQSc-?usp=sharing
https://mega.nz/folder/0DBRnTrR#hHjiZFmU0VtvQHD3tnPa4Q
2685.334933997698
6573.932784081527
8.109311488062492
1.01389663443199
0.4233007520875216
7.411065644732161
# 摘要
為了探討衛星最佳化變軌設計的可行性，我們以以FORMOSAT-5為例進行模擬和分析。
分為以下幾大部分:

簡單橢圓軌道：僅考慮重力影響下的衛星運動動力學，並使用MATLAB進行了模擬。模擬結果顯示了不同的ODE解算器（如ODE23s、ODE4、ODE45）在模擬簡單橢圓軌道時的差異。

J2擾動下的FORMOSAT-5軌道：討論了在J2擾動（地球赤道隆起）影響下的FORMOSAT-5軌道，使用MATLAB和GMAT進行模擬，並比較了兩者的模擬結果。

低推力下的FORMOSAT-5軌道：在低推力推進系統下的FORMOSAT-5軌道變軌，模擬了推力為32mN、比衝為1244s、燃燒時間為86400s的情況，並比較了MATLAB和GMAT的模擬結果。

Edelbaum低推力轉移解：探討了低推力推進方式的優化策略，包括半長軸、偏心率和軌道傾角的變化。提出了最佳低推力修正方法。

# 目的
衛星精確軌道控制的研究旨在開發和實施一種高度精確的衛星軌道控制系統，以確保衛星在軌道上的位置和運動狀態能夠得到精確控制。這對於許多衛星任務都是至關重要的，特別是對於需要精確定位和運動補償的應用，如合成孔徑雷達（SAR）成像。精確的軌道控制可以保證衛星成像的精度和圖像質量，防止圖像變形和失真。此外，精確軌道控制還可以確保衛星的運行穩定，減少衛星運行中出現的問題，延長衛星的使用壽命，降低運營成本。因此，衛星精確軌道控制的研究具有非常重要的意義。

# Simple Elliptical Orbits
為了驗證基於牛頓運動定律的二體運動方程式，我們先以圓形軌道之參數於MATLAB內建模並模擬軌道運動。
* Satellite motion dynamic
* Elliptical orbit
* Only consider gravity effect
* r0 = [8000 0 6000]; % km
* v0 = [0 7 0]; % km/s
* time_step = 60;
* ODE23s
**Nonlinear system dynamic equations:**

# FORMOSAT-5 Orbits with J2 perturbation

而為了更進一步精確模擬太空中的環進，我們加入的重力梯度(J2)所造成的擾動，並於MATLAB內建模並模擬軌道運動。
* 起始條件精度:小數點後16位
* TLE 轉換之坐標系:TEME OF DATE
* J2的設定 (max order = 0; max degree = 2)
* J2 = 0.001082626724393;(利用JGM2重力模型計算得到)
* ODE4

## Spherical Harmonic

The spherical harmonic of degree 2 and order 0 - C(2,0) - is due to the flattening of the Earth (Earth’s dynamic oblateness)
C(2,0) (also known as 'J2', but they differ by a constant factor: J2 = -C(2,0)*sqrt(5))


## MATLAB 模擬結果

可以看到加入J2擾動後，軌道倾角與RAAN皆有明顯變化，為了驗證結果是否正確我們與STK進行比較。

### ODE4
### ODE45
### ODE45 vs ODE4

從上面三張圖表我們可以看出，雖然ODE45與STK的平均軌道高度誤差較小，但是散布明顯大ODE4許多。
而我們希望積分求解器是FIXED-TIMESTEP，因此最終採用ODE4。

# FORMOSAT-5 Orbits with Low-Thrust

此計畫是為了讓在低軌道的LEO衛星能維持軌道高度，而採用的方式就是利用Low-Thrust的電力推進來維持，為了實驗我們的模型，因此先在飛行方向加delta V，在模擬時間第七天時開啟引擎持續燃燒，推進至第八天。分別以MATLAB與GMAT進行模擬。

## 引擎設定
* 推力:32 mN
* ISP 1244
* 燃燒時間:86400 S

## MATLAB 模擬結果
## MATLAB-低推力模擬

上圖我們可以很明顯的看出軌道在低七天時開始抬升至第八天，符合我們的預期結果。
## GMAT 模擬結果

## GMAT-低推力模擬

## GMAT與MATLAB結果之差異

從結果來看，我們可以我們在MATLAB內的模擬，軌道抬升的幅度明顯高於GMAT將近20公里，這部分還在除錯，詳情請看 附錄

# Edelbaum’s Low-Thrust Transfer Solution

原先的加速直接相加後積分，應該是不可行，未在文獻中發現相似作法
而目前主流低推力推進方式之優化有許多不同的方式，需要找尋最適合本案的控制器演算法，
因此在文獻回朔的過程中，我們找到一篇論文*Edelbaum, T. N. (1961). Propulsion requirements for controllable satellites. ARS Journal, 31(8)*，他探討了基於低推力衛星最佳變軌的方式。

## Optimum Low Thrust Corrections:
* For changes in the semimajor axis, thrust should be directed tangentially.
* For changes in eccentricity, thrust should be directed normal to the line of apsides.
* For changes in inclination, thrust should be directed normal to the plane of the orbit for half a cycle, then reversed.
* Changes in position are made by applying tangential thrust in one direction, then reversing it.

## Modification of Circular Orbits
### Large Changes in Orbit Radius:
The classic Hohmann transfer is optimal for most practical cases.
For transfers to orbits more than twelve times larger or smaller, a three-impulse bi-elliptic transfer can save fuel.
Low thrust transfers between circular orbits are optimized by maintaining a mean path with a slope twice the local thrust-weight ratio.

### Changes in Inclination:
Efficient maneuvers involve a tangential impulse to establish an elliptic orbit, a normal impulse at apogee to change inclination, and a tangential impulse at perigee to re-establish a circular orbit.
For large changes, escaping to "infinity" and applying an infinitesimal impulse is theoretically optimal but impractical.

## Orbit Maintenance
### Atmospheric Drag:
At altitudes above 300 miles(482.8032 Km), atmospheric drag is negligible.
Below this, continuous thrust is required to maintain orbit, typically ranging from 10^-5 to 10^-4 times the vehicle weight.
### Gravitational Perturbations:
The Earth's equatorial bulge causes changes in the period, rotation of the orbital plane, and the line of apsides.
The sun and moon have similar effects.
For a stationary 24-hour equatorial satellite, continuous or periodic thrust is needed to maintain position against these perturbations.

## Optimum Maneuvers and Characteristic Velocity
* High Thrust Systems: Characterized by low specific impulse and high fuel consumption. Maneuvers are optimized to minimize fuel consumption.
* Low Thrust Systems: Have high specific impulse and low fuel consumption but long maneuver times. For these systems, minimum-time maneuvers are equivalent to minimum-fuel maneuvers.
* Combination Maneuvers
* High Thrust: Combining changes in semimajor axis and eccentricity can be achieved without increasing total velocity increment. For three-dimensional maneuvers, generally three or four impulses are required.
* Low Thrust: Optimum steering programs for combinations of changes in semimajor axis, eccentricity, and inclination are derived.

## Equations of Motion of Orbit Elements


Orbital elements
## Lagrange's planetary equations VOP

## The full set of the Gaussian form of the Lagrange planetary equations

## Optimum Steering Programs

## Conclusion

嘗試將Edelbaum’s Low-Thrust Transfer Solution的結果，基於6個Lagrange‘s planetary equations，運用在當前的MATLAB模型上，進行軌道維持模擬並與STK做驗證比較。

# 附錄
完整程式碼

% 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 = [2682.4882685250249779 6566.9639001485847984 8.1007149823713345]; % Initial position vector in km
v0 = [1.0143378782946217 -0.4253020701657230 7.4189240233167526]; % 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);
amag = diff(vmag);
altitude = rmag - RE;
figure;
plot(t/86400,altitude);

% 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];
    %32 mN  ISP 1244
    thrust = 0.032; % Constant thrust in Newtons
    burn_duration = 86400; % 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)';
       
        
    end
    % Total acceleration
    a_total =  a_J2 + a_thrust;
    
    % Output derivative of state vector
    Ydot = [vvector a_total'];
end

# Hohmann Transfer
## 目標
以霍曼轉移為例，練習從完整理論公式推導到利用模擬來驗證我的理論。
### 理論公式

### $r_{\text{initial}}, r_{\text{final}} \gg a_{\text{trans}}, \Delta v_a, \Delta v_b$

$a_{\text{trans}} = \frac{r_{\text{initial}} + r_{\text{final}}}{2}$

$r_{\text{final}} = 20000 \, \text{km}$

$v_{\text{initial}} = \sqrt{\frac{\mu}{r_{\text{initial}}}}$

$v_{\text{trans}a} = \sqrt{\frac{2\mu}{r_{\text{initial}}} - \frac{\mu}{a_{\text{trans}}}}$

$v_{\text{final}} = \sqrt{\frac{\mu}{r_{\text{final}}}}$

$v_{\text{trans}b} = \sqrt{\frac{2\mu}{r_{\text{final}}} - \frac{\mu}{a_{\text{trans}}}}$

$\Delta v_a = v_{\text{trans}a} - v_{\text{initial}}$

$\Delta v_b = v_{\text{final}} - v_{\text{trans}b}$

$\Delta v = |\Delta v_a| + |\Delta v_b|$

*Vallado, D. A. (2001). Fundamentals of Astrodynamics and Applications.*

找到兩次推進所需要的 Delta-V，並且計算兩次分別所需要的燃燒時間，這邊是固定推力以及ISP,所以時間計算的方式是先找到推力造成的加速度量後再計算時間。

### 利用Matlab計算所需要的Delta-V

* R1:715.597 Km
* R2:20000.000 Km
* a_trans:16735.919 Km
* V1:7.500013 Km/s
* V2:3.887290 Km/s
* Va:9.410855 Km/s
* Vb:9.410855 Km/s
* **delta_v1:1910.842152 m/s**
* **delta_v2:1356.482663 m/s**
* **burn_time1:30255.000744 s (504.25min)**
* **burn_time2:21477.642172 s (357.9607min)**

在這裡我們的V1，也就是初始軌道速度，得到的數值與我們利用公式
$v_{\text{initial}} = \sqrt{\frac{\mu}{r_{\text{initial}}}}$
所計算得到的7.496041 Km/s有所不同，是因為初始福衛五號的軌道並不是完美的圓形(ECC=0)，而該公式是設計計算正圓軌道的，因而有些許上的速度差異。

### Matlab 計算施加推力點

> The idealized Hohmann transfer has two instantaneous burns, one at the periapsis, one at the apoapsis.
>
因此我們必須透過計算當前衛星所在位置的True anomaly(TA)來得知衛星是否處於近地點(TA=0)或遠地點(TA=180)。

True anomaly:
$\nu = \cos^{-1} \left( \frac{\mathbf{e} \cdot \mathbf{r}}{|\mathbf{e}||\mathbf{r}|} \right)$

(if $\mathbf{r} \cdot \mathbf{v} < 0$ then replace $\nu$ by $2\pi - \nu$)

Eccentricity vector:
$\mathbf{e} = \frac{\mathbf{v} \times \mathbf{h}}{\mu} - \frac{\mathbf{r}}{|\mathbf{r}|}$

Specific angular momentum:
$\mathbf{r} \times \mathbf{v}$

得到當前的TA後，我們會將開啟推力的時間點限制在最近近TA=0與TA=180的時刻，以確保加速度施加符合霍曼轉移，

*Vallado, D. A. (2001). Fundamentals of Astrodynamics and Applications. *

### Matlab模擬結果(thrust = 30N)
由上面的模擬結果我們可以觀察到，軌道呈現螺旋高度提升的狀態，是因為我們兩次推進的時間都非常長，分別長達504.25min與357.9607min，以至於變成了finite thrust maneuver，而不是或曼轉移所需要的impulse thrust maneuver。
因此針對這個問題，我們提出了以下的修正方式:
* R2改為800公里 (target_altitude = 800; % Target orbit altitude in km)
* 只在近地和遠地點有推力(burn_time1:2.464674 s, burn_time2:2.994881 s)
thrust = 3500N
True Anomaly (Burn 1): 9.99921 (degree) 接近近地點 (TA = 0)
True Anomaly (Burn 2): 189.99977(degree) 接近遠地點 (TA = 180)
* 改sampling time為1秒 ([t, Y] = ode4(@relative_motion_drag_J2, t0, 1, tf, Y0);


### Matlab模擬結果(thrust = 3500N)
大致上有大幅修正，但是最終軌道結果看起來仍然不是正圓，判斷是因為兩次加速的時間點仍然不是在True Anomaly = 0 與 True Anomaly = 180的位置，因此所造成的些微差異，這部分屬於程式問題，我將盡速修正。

### 最終修正與模擬結果
發現先前之軌道起始條件非正圓軌道，且模擬時間的起始點計算得TA為180度，將軌道修改為正圓之後，遠近地點之間的差異雖縮小但仍未歸零。
近一步驗證發現是J2 term造成軌道高度非定值，將J2 term關閉後已可達到Hoemann的效果。
而先前發現的軌道高度超過指定高度為程式問題，已找出原因並加以修正完畢。
因此在最後，我們已經可以在MATLAB中模擬完美的Hohmann Transfer。

#### 時間對高度圖(with J2) target_altitude = 700


#### 時間對高度圖(with out J2) target_altitude = 700

由上面兩張模擬結果，我們可明確看出升軌推力點的施加位置是在近地點，而第二次推力(用於將軌道推回圓形)則是在遠地點，另外關閉J2則會讓造成軌道擾動的效果消失，符合預期。

為了近一步驗證軌道的六元素是否都未受到影響(應該只有半長軸抬升，ECC有瞬間的變化)，我們以下計算軌道六元素)

再次放上此圖以供參考
𝑎 = semimajor axis (半長軸)
𝑒 = eccentricity (離心率)
𝑖= inclination (軌道傾角)
Ω = right ascension of ascending node(升交點黃經)
𝜔 = argument of perigee(近日點輻角)
𝑡_𝑝 = time of perigee passage (通過近地點時間)

##### Semimajor Axis

* $\xi = \frac{v^2}{2} - \frac{\mu}{r}$
* $\xi = -\frac{\mu}{2a}$
* $a = -\frac{\mu}{2\xi}$

##### Eccentricity
* The eccentricity $e = |\vec{e}|$ is obtained from the eccentricity vector
* $\dot{\vec{r}} \times \vec{h} = \mu \frac{\vec{r}}{r} + \vec{B}$
* $\vec{e} = \frac{\vec{B}}{\mu}$
* $\vec{B} = \vec{v} \times \vec{h} - \mu \frac{\vec{r}}{r}$
* $\vec{e} = \frac{\vec{v} \times \vec{h}}{\mu} - \frac{\vec{r}}{r}$
* 離心率向量 (eccentricity vector) 是個大小等於軌道離心率 (eccentricity) 且方向指向近心點 (periapsis 或 pericenter) 的向量

##### Inclination
* $i = \cos^{-1} \left\{ \frac{\hat{K} \cdot \vec{h}}{|\hat{K}||\vec{h}|} \right\}$

##### Right Ascension of Ascending Node

* 昇交點的赤經，$\Omega$，RAAN，是在赤道平面上從$\hat{I}$單位向量到昇交點位置向量的角度，測量時為正向東。昇交點是衛星從南向北穿過赤道的點。所有傾斜軌道也都有降交點，這是衛星從北向南穿過赤道平面的點。連接這兩個交點的線定義為交點線。
* $\vec{n}$ is a vector pointing towards the ascending node
* $\vec{n} = \hat{K} \times \vec{h}$
* $\Omega = \cos^{-1} \left( \frac{\hat{I} \cdot \vec{n}}{|\hat{I}||\vec{n}|} \right) \quad \text{if} \ n_j < 0, \ \Omega = 360^\circ - \Omega$

##### Argument of Perigee

* 近心點角（$\omega$）也做近心點幅角。是描述在軌道上天體在近拱點 (periapsis) 時，相對於升交點 (RAAN) 的角度。
* $\omega = \cos^{-1} \left( \frac{\vec{e} \cdot \vec{n}}{|\vec{e}||\vec{n}|} \right), \quad \text{if} \ e_K < 0, \ \omega = 360^\circ - \omega$
* 對於圓形軌道 ($\dot{e} = 0$) 或赤道軌道 ($\vec{n} = 0$)，這個角度是未定義的。

##### True anomaly
* 真近點角 (v), 衛星與近地點之間的橢球焦點角距
* $v = \cos^{-1} \left( \frac{\vec{e} \cdot \vec{r}}{|\vec{e}||\vec{r}|} \right) \quad \text{if} \ \vec{r} \cdot \vec{v} < 0, \ v = 360^\circ - v$

#### 六元素圖(with J2)target_altitude = 700
#### 六元素圖(with out J2) target_altitude = 700

由上面兩張圖我們可以觀察到，沒有了J2影響後，ECC就是很完美的接近0，只有在推進瞬間有變化，符合霍曼轉移的理論以及我們的預期。
因此就結論而言，我們成功從數學公式推導出了霍曼轉移所需要的推力，並且在MATLAB模擬驗證。

# 多次、小推力軌道抬升
## 目標
有了先前在霍曼轉移的經驗以及知識，我們將他用在低推力連續推進上。
設計兩次低推力，連續推進，一次在近地點，一次在遠地點，抬升軌道同時嘗試維持軌道ECC。
## 推力點判斷重新設計
因為TA非線性的原因，改用直接計算之方式判斷近地點與遠地點，已確保每次經過都有正確開啟推力。
### 近地點、遠地點直接計算
### Matlab 模擬結果

Np:10 dt:500 num_steps = 5184;

% NMPC Orbit Controller with Orbital Element Stabilization (a, e, RA)
clc; 
clear; 
close all;

% Define Constants and Initial Conditions
mu = 398600.4415000000153668;  % Earth's gravitational parameter (km^3/s^2)
J2 = 0.001082629821312;  % Earth's J2 perturbation coefficient 0.001082629821312
RE = 6378.1369999999997162;  % Earth's radius (km)
Cd = 2.2;  % Drag coefficient
A_drag = 2.237487; % Drag area (m^2)
m = 500; % Satellite mass in kg 

% Initial Position and Velocity for FS-9 Orbit
r0 = [6889.4908134881943624, 190.2497292231479378, -16.5468474743937080];
v0 = [0.0453559761031717, -0.9866437758147498, 7.5404616924289787];
Y0 = [r0, v0]';  % Initial state vector [position; velocity] as a column vector

% Define NMPC Parameters
Np = 10; % Prediction Horizon
Nc = Np; % Set Control Horizon equal to Prediction Horizon
dt = 500; % Time step in seconds

% Maximum control input (thrust constraint)
F_max = 0.032; % Maximum thrust in Newtons
lb = [-pi/2 * ones(Np, 1); -pi/2 * ones(Np, 1)];
ub = [pi/2 * ones(Np, 1); pi/2 * ones(Np, 1)];

% Reference orbital elements (a, e, RA) for target orbit
target_elements = coe_from_sv(r0, v0, mu); % Initial elements as target
a_target = target_elements(7); % Semi-major axis
e_target = target_elements(2); % Eccentricity
Omega_target = target_elements(3); % Right ascension of ascending node
omega_target = target_elements(5);
target_elements = [a_target, e_target, Omega_target, omega_target];


% Initial weight matrices
Q_prev = diag([1e-3, 1e3, 0, 0]); % Initial weights for a, e, RA, ω
R_prev =  diag([1e-3, 1e-3]);  % Weight for control angles [alpha, beta]

% Simulation Parameters
num_steps = 173*7;
trajectory = zeros(num_steps+1, 6);
trajectory(1, :) = Y0';
control_angles = zeros(num_steps, 2);
total_force = zeros(num_steps, 1);


% Initialize state
Y = Y0; % Initial state

% Main Simulation Loop
for t = 2:num_steps + 1
   
    
    % Dynamically adjust weight matrices using previous values
    [Q_dynamic, R_dynamic] = adjust_weights(Q_prev, R_prev);

    % Update previous weights for next iteration
    Q_prev = Q_dynamic;
    R_prev = R_dynamic;

    % Initial guess for control input sequence
    U_initial = zeros(2 * Np, 1);
    options = optimoptions('fmincon', 'Display', 'none', 'Algorithm', 'sqp');
    U_opt = fmincon(@(U) nmpc_cost_angles(U, Y, Np, dt, mu, J2, RE, m, A_drag, Cd, Q_dynamic, R_dynamic, target_elements, F_max), ...
                U_initial, [], [], [], [], lb, ub, [], options);


    % Extract and apply first control angles
    alpha_beta = reshape(U_opt, Np, 2);
    control_angles(t-1, :) = alpha_beta(1, :);
    alpha = alpha_beta(1, 1);
    beta = alpha_beta(1, 2);

    % Compute thrust vector in inertial frame
    R_VNB_to_inertial = vnb_to_inertial(Y(1:3), Y(4:6));
    F_inertial = thrust_to_inertial(alpha, beta, F_max, R_VNB_to_inertial);

    % Store the magnitude of the total force
    total_force(t-1) = norm(F_inertial);

    % Propagate dynamics
    t_current = (t-1) * dt;
    t_end = t_current + dt;
    [~,Yout] = ode4(@(t, Y) satellite_dynamics(Y, F_inertial, mu, J2, RE, m, A_drag, Cd), ...
                     t_current, 60, t_end, Y);
    Y = Yout(end, :)';
   
    trajectory(t, :) = Y';
    
end

% Extract position and velocity vectors
rvec = trajectory(:, 1:3);  % Position (x, y, z)
vvec = trajectory(:, 4:6);  % Velocity (vx, vy, vz)
% Create a new figure for the 3D orbit
fprintf("start ploting \n");
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');


% Plot Control Angles (Alpha and Beta) Over Time
time_seconds = (1:num_steps) * dt; % Time in seconds for control inputs
time = (0:num_steps) * dt;
figure;
subplot(2, 1, 1);
plot(time_seconds, rad2deg(control_angles(:, 1)), 'LineWidth', 1.5);
ylabel('\alpha (deg)');
xlabel('Time (s)');
title('Control Angle \alpha (In-Plane Angle)');
grid on;

subplot(2, 1, 2);
plot(time_seconds, rad2deg(control_angles(:, 2)), 'LineWidth', 1.5);
ylabel('\beta (deg)');
xlabel('Time (s)');
title('Control Angle \beta (Out-of-Plane Angle)');
grid on;


% Plot Total Applied Force (F_inertial) Over Time
figure;
time_seconds = (1:num_steps) * dt; % Time vector for force plot

plot(time_seconds, total_force, 'b-', 'LineWidth', 1.5);
xlabel('Time (s)');
ylabel('Total Force (N)');
title('Total Applied Force Over Time');
grid on;




semi_major_axis = zeros(num_steps + 1, 1);
eccentricity = zeros(num_steps + 1, 1);
RAAN = zeros(num_steps + 1, 1);
argument_of_perigee = zeros(num_steps + 1, 1);

for t = 1:num_steps + 1
    
    position = trajectory(t, 1:3);
    velocity = trajectory(t, 4:6);
        
    coe = coe_from_sv(position, velocity, mu);

    semi_major_axis(t) = coe(7);
    eccentricity(t) = coe(2);
    RAAN(t) = coe(3);
    argument_of_perigee(t) = coe(5);
end

% Plot Semi-Major Axis, Eccentricity, and Right Ascension of Ascending Node over Time
figure;
subplot(4, 1, 1);
plot(time, semi_major_axis, '-r', 'DisplayName', 'Semi-Major Axis');
yline(a_target, '--b', 'DisplayName', 'Target a');
xlabel('Time (s)'); ylabel('Semi-Major Axis (km)');
legend; grid on;

subplot(4, 1, 2);
plot(time, eccentricity, '-g', 'DisplayName', 'Eccentricity');
yline(e_target, '--b', 'DisplayName', 'Target e');
xlabel('Time (s)'); ylabel('Eccentricity');
legend; grid on;

subplot(4, 1, 3);
plot(time, rad2deg(RAAN), '-k', 'DisplayName', 'Right Ascension');
yline(rad2deg(Omega_target), '--b', 'DisplayName', 'Target Omega');
xlabel('Time (s)'); ylabel('Right Ascension (deg)');
legend; grid on;

subplot(4, 1, 4);
plot(time, rad2deg(argument_of_perigee), '-m', 'DisplayName', 'Argument of Perigee');
yline(rad2deg(omega_target), '--b', 'DisplayName', 'Target ω');
xlabel('Time (s)'); ylabel('Argument of Perigee (deg)');
legend; grid on;



% Satellite Dynamics Function
function [dY, control_acc] = satellite_dynamics(Y, F_inertial, mu, J2, RE, m, A_drag, Cd)
    r = Y(1:3);  % Position vector
    v = Y(4:6);  % Velocity vector
    r_norm = norm(r);
    Z = r(3);
    Z2 = Z^2;
    r2 = r_norm^2;

    ax_J2 = (-mu * r(1) / r_norm^3) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    ay_J2 = (-mu * r(2) / r_norm^3) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 1));
    az_J2 = (-mu * r(3) / r_norm^3) * (1 - (3/2) * J2 * (RE^2 / r2) * (5 * Z2 / r2 - 3));
    a_J2 = [ax_J2; ay_J2; az_J2];

    altitude = r_norm - RE;
    density = atmosphere(altitude);
    omega_earth = [0; 0; 7.2921159e-5];
    v_Rlative = v - cross(omega_earth, r);
    V_r = norm(v_Rlative);
    drag_acc = -0.5 * Cd * (A_drag / m) * density * V_r^2 * (v_Rlative / V_r);
    
    control_acc =  (F_inertial / m)* 1e-3;
    total_acc = a_J2  + drag_acc + control_acc;
    dY = [v; total_acc];
end

% NMPC Cost Function with Orbital Elements (a, e, RA)
function J = nmpc_cost_angles(U, Y0, Np, dt, mu, J2, RE, m, A_drag, Cd, Q_elem, R_control, target_elements, F_max)
    U = reshape(U, Np, 2);
    Y = Y0(:);
    J = 0;
    dt_internal = 60;

    for k = 1:Np
        alpha = U(k, 1);
        beta = U(k, 2);
        R_VNB_to_inertial = vnb_to_inertial(Y(1:3), Y(4:6));
        F_inertial = thrust_to_inertial(alpha, beta, F_max, R_VNB_to_inertial);
        t_current = (k-1) * dt;
        t_end = t_current + dt;
        % Simulate the system for this time step using ODE4
        [~, Yout] = ode4(@(t, Y) satellite_dynamics(Y, F_inertial, mu, J2, RE, m, A_drag, Cd), ...
                         t_current, dt_internal, t_end, Y);
        Y = Yout(end, :)';
        
        % Calculate current orbital elements
        current_elements = coe_from_sv(Y(1:3), Y(4:6), mu);
           
        current_a = current_elements(7);
        current_e = current_elements(2);
        current_Omega = current_elements(3);
        current_omega = current_elements(5);
            
        % Compute element error
        element_error = [current_a - target_elements(1), current_e - target_elements(2), current_Omega - target_elements(3),  angular_error_degrees(current_omega, target_elements(4))]';
        
        % Dynamically adjust weight matrices using previous values
        
        % Accumulate cost with element tracking
        J = J + trace(element_error' * Q_elem * element_error) + [alpha; beta]' * R_control * [alpha; beta];
    end
end

% Function to dynamically adjust weight matrices based on orbital element errors
function [Q_dynamic, R_dynamic] = adjust_weights(Q_prev,R_prev)
    Q_dynamic = Q_prev;
    R_dynamic = R_prev;
end

function angular_error = angular_error_degrees(angle1, angle2)
    angle1 = rad2deg(angle1);
    angle2 = rad2deg(angle2);
    % Compute the raw difference
    raw_diff = angle1 - angle2;

    % Wrap the difference to the range [-180, 180)
    angular_error = mod(raw_diff + 180, 360) - 180;
    angular_error = deg2rad(angular_error);
end


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

function F_inertial = thrust_to_inertial(alpha, beta, F_max, R_VNB_to_inertial)
    % Thrust vector in VNB frame
    F_T = F_max * cos(beta) * cos(alpha);
    F_N = F_max * cos(beta) * sin(alpha);
    F_B = F_max * sin(beta);
    F_VNB = [F_T; F_N; F_B];
    F_inertial = R_VNB_to_inertial * F_VNB;
end

function R_VNB = vnb_to_inertial(r, v)
    % Compute VNB to inertial rotation matrix
    r_hat = r / norm(r);
    v_hat = v / norm(v);
    n_hat = cross(r_hat, v_hat);
    n_hat = n_hat / norm(n_hat);
    b_hat = cross(v_hat, n_hat);
    R_VNB = [v_hat, n_hat, b_hat];
end


# 角速度計算與Kalman Filter
https://hackmd.io/@averil0814/SJgvQYPDyg