宇宙探査機の軌道設計ーMATLAB fminconを用いたMultiple Shootingの最適化（低推力推進と化学推進）

 # 目次
[1.はじめに](#1-はじめに)
[2.MultipleShootingとは](#2-MultipleShootingとは)
[3.軌道設計の仮定](#3-軌道設計の仮定)
[4.最適化の定式化](#4-最適化の定式化)
[5.コードの解説](#5-コードの解説)
[6.順伝播/逆伝播](#6-順伝播/逆伝播)

# 1. はじめに
最適化は大きく、直接法、間接法に分けられます。本稿では直接法の一般的な解法の一つである逐次二次最適化を用いたMultiple Shootingによる軌道最適化/軌道設計に関する説明とコードの共有を行います。コードは全て、軌道最適化/アストロダイナミクス分野で一般的に用いられているMATLAB、最適化はOptimiation Toolbox内のfminconを使用します。 [^1]

コードができたので一旦公開しますが、解説の方、もう少しブラッシュアップしていきます。[^2]
[^2]: 実は解説はgptでやってみました。細かい解説はあるのですが、概観が無いので足していきます。

[^1]: 実際のミッションでは、fminconと同様に、逐次二次最適化ソルバーである[snopt](https://www.google.com/url?sa=t&source=web&rct=j&opi=89978449&url=https://ccom.ucsd.edu/~optimizers/solvers/snopt/&ved=2ahUKEwiQ79WD4bKJAxUW5DQHHVy8DHoQFnoECAgQAQ&usg=AOvVaw0xrtHR3bv1LN7k0uL3VJFE)がよく用いられています。snoptがfminconよりも収束性が良いとのことです。一方、並列化ができないなどの課題があるので（多分...）、fminconで解けるならfminconで良いと思います。なお、こちらはライセンスがややこしいので注意してください。

# 2. MultipleShootingとは
Shooting法とは、t=ti から、 t=tfの境界条件を満たすように入力の値を狙い撃ちする方法です。このとき、関数の非線形性が強いと、f(tf)の予測の精度が低くなります。そこで、tfまで1発で狙い撃ちするのではなく、その間をいくつかの区間に分けてそれぞれshootする方法が、multiple shooting法です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3901421/5817e803-0949-c99f-f73c-27eb0f691db1.png)

軌道最適化では、shooting法といくつかの仮定を用いることで簡単に軌道設計ができます。

:bangbang:Multiple Shootingのポイントは、中間の区間の初期点が前の区間のshootに関係なく与えられることです。これによって、各ノードの初期点から同時に積分する並列化も可能になります。

# 3. 軌道設計の仮定
Multiple Shootingの軌道設計では、推進機ごとに異なる仮定を用いて設計を行うことが一般的です。

### 3.1 インパルス推進の軌道設計の仮定
インパルス推進とは、化学推進のことを指します。インパルスという名の通り、スラスターの駆動時間は微小時間＝0に近似できます。

例えば上の図で、t = t_i のときにインパルス推進ΔVを駆動させるとすると、
```math
\begin{bmatrix} X(t_i) \\ V(t_i) \end{bmatrix} \Rightarrow \begin{bmatrix} X(t_i) \\ V(t_i) + \Delta V \end{bmatrix}
```
にし、それを初期値にして軌道伝播（運動方程式の積分）をすることで、化学推進の駆動を再現できます。

### 3.2 低推力推進の軌道設計の仮定
低推力推進はインパルス推進と異なり、微小なΔVを連続的に加え続けるような推進を指します。Multiple Shooingで低推力推進を実装するときは、Shootingの1区間（またはそこをさらに細分化した区間）で推進が一定であるという仮定を置きます。

つまり、運動方程式内部で、
```math
\dot{y} = \begin{bmatrix} A \\ V \end{bmatrix} \Rightarrow \dot{y} = \begin{bmatrix} A \\ V + \Delta V \end{bmatrix}
```
にして軌道伝播することで再現できます。

### ☕Coffee Break
高度な最適化ソルバーを用いる場合は、Shootingの区間を非常に小さく設定することで、3.2の方法で3.1のようなインパルスの解を得ることができます。これは、L1最適な解と呼ばれ、理論的に最適（燃料最小や遷移時間最小）な入力となります。逆に入れば、L1最適な解はインパルスの構造を持つから、3.1のような定式化が妥当といえます。

🍦このようなスパース最適は機械学習などの分野でもよく出てきます。色々な最新の分野が数理最適化の知識でつながると面白いですね

# 4. 最適化の定式化
最適化問題では、消費燃料などの目的関数を最小化することと、軌道の始点と終点（GEOの1点からLLOの1点など）を結ぶことの2つがあります。これを、推進剤や宇宙機の状態の制約の元で解くのが軌道最適化です。

まとめると、
* minimize f(u)=消費燃料
* subject to 境界条件などの等式制約、入力（推進機）の上限などの不等式制約

が最適化問題の定式化になります。

### ☕Coffee Break2
宇宙探査機のミッション設計では、燃料の最小化というよりも、そもそも境界条件を達成するような解が存在するのかどうかを探すことが重要です。

これは、探査機の性能で行くことができる空間が狭いため、実現できるかどうか？をまず第一に知りたいからです。

また、消費燃料が非常に少ない軌道を見つけたとしても、それが安全でない軌道（少しの推進機の駆動ミスで終着点が大幅に変わってしまうような軌道）の可能性があるからです。このような安全な軌道を探す分野は、ロバスト最適化と呼ばれています。



# 5. コードの解説
まずは、コードは[こちらのgithubレポジトリ](https://github.com/koheioue/MATLAB_Shooting_fmincon)からcloneしてください。
cloneができない場合、以下のファイルのコード全部コピーしてMatlabのファイルに貼り付けることで、以下のようなファイル構成にしてもらえるとOKです。（LICENSEとREADMEは無くても動きます）
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3901421/b09a820d-36b8-ac1d-f2f4-e0bd63c16eb4.png)

## 
とりあえずmainを実行すれば、地球の公転軌道を初期軌道にし、地球ー火星遷移軌道を求めるコードが走ります。長くても10秒位で終わるはずです。
##

### 5.1 main

まずはコード全文
```matlab
clear
close all

flag_opt = 0;% 1:最適化後, 0:最適化前

%% multiple shooting の条件
coeff_obj = 1e3;% 目的関数の係数
lsf = 1e6;
tsf = 1e6;
msf = 1;

%% 初期値の設定
y_node_int_all = [0, -1.496e8, 0, 29.78, 0, 0; 
                  1.496e8, 0, 0, 0, 29.78, 0; 
                  0, 1.496e8, 0, -29.78, 0, 0];

u_node_int_all = [0, 0, 0, 0; 
                  0, 0, 0, 3.154e+7/4; 
                  0, 0, 0, 3.154e+7/4];

%% スケーリングファクターを適用
y_node_int_all(:, 1:3) = y_node_int_all(:, 1:3) / lsf; % x, y, z のスケーリング
y_node_int_all(:, 4:6) = y_node_int_all(:, 4:6) * tsf / lsf; % vx, vy, vz のスケーリング
u_node_int_all(:, 1:3) = u_node_int_all(:, 1:3) * tsf / lsf; % vx, vy, vz のスケーリング
u_node_int_all(:, 4) = u_node_int_all(:, 4) /tsf; % T のスケーリング

N = length(u_node_int_all(:,1));
u_node_int_all(:, 4) = u_node_int_all(:, 4) * 1.015;
auxdata_set
auxdata.x_target = [0, 227936640, 0, -24.07, 0, 0]; % 目標点の設定
auxdata.x_target(1:3) = auxdata.x_target(1:3) / lsf; % x, y, z のスケーリング
auxdata.x_target(4:6) = auxdata.x_target(4:6) * tsf / lsf; % vx, vy, vz のスケーリング

%% 最適化に使う用にreshape
x_node_int_vector = reshape(y_node_int_all', [], 1);
u_node_int_vector = reshape(u_node_int_all', [], 1);
y0 = [x_node_int_vector;u_node_int_vector];
plot_traj_from_y

%% 最適化
yL = -[Inf*ones(length(x_node_int_vector),1); Inf*ones(length(u_node_int_vector),1)];
yU =  [Inf*ones(length(x_node_int_vector),1); Inf*ones(length(u_node_int_vector),1)];

%% 初期軌道の図を消す
close all

%% ちゃんと制約条件が適切か確認
confun(y0, auxdata);
objfun(y0, auxdata);

%% 最適化オプションの設定

options = optimoptions('fmincon', ...
    'Algorithm', 'sqp', ...
    'Display', 'iter-detailed', ...
    'UseParallel', false, ... % 並列化を有効にするなら true
    'StepTolerance', 1e-6, ...
    'OptimalityTolerance', 1e-4, ...
    'ConstraintTolerance', 1e-12, ...
    'MaxFunctionEvaluations', 5e5, ...
    'MaxIterations', 100);

% 最適化の実行
[yopt, f, output] = fmincon(@(y) objfun(y, auxdata), y0, [], [], [], [], yL, yU, @(y) confun(y, auxdata), options);
% 2回目の最適化の実行（必要なら）
% y0 = yopt;
% auxdata.coeff_obj = 100*coeff_obj;
% [yopt, f, output] = fmincon(@(y) objfun(y, auxdata), y0, [], [], [], [], yL, yU, @(y) confun(y, auxdata), options);

flag_opt = 1;% 1:最適化後, 0:最適化前
plot_traj_from_y
```

### 初期設定と最適化の準備

#### 1. 変数の初期化

- `flag_opt`: 最適化の実行状況を管理するフラグです。`1`は最適化済み、`0`は最適化前を意味します。
- `coeff_obj`: 目的関数に掛ける係数（スカラー）です。
- `lsf`, `tsf`, `msf`: それぞれ、長さ・時間・質量のスケーリングファクターとして設定しています。

#### 2. 複数のノードの初期値

- `y_node_int_all`：初期軌道の各shooting点における状態量（x,y,z,vx,vy,vz）をノードごとに設定しています。
- `u_node_int_all`：初期制御入力をノードごとに設定しています。ux, uy, uz, t になっています（tは各区間の時間で全体の遷移時間とは異なります。

初期軌道は、以下のように地球の公転軌道を用いています。下からスタートし、上で終わり、その中間にインパルスΔVを吹く中間ノードを設定しています。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3901421/333af1e9-78bc-44e6-c1be-21869a067b93.png)



#### 3. スケーリングの適用

- `y_node_int_all`、`u_node_int_all`に対して、位置・速度・時間のスケーリングを行っています。
❗️　このスケーリングは最適化において非常に重要な作業です。最適化を行う際に、yが最適化の変数として用いられますが、位置と速度をそのまま用いると、大きな差が生まれます。これにより、最適化を行う際に、それぞれの勾配が大きく異なることとなり、収束性が非常に悪くなってしまいます。よってこれらをそれぞれ、位置・速度・時間でスケーリングすることで同じくらいのオーダーにします。

#### 4. 目標点の設定

- `auxdata.x_target`：到達目標地点（位置と速度）を設定しています。スケーリングも適用します。
- 数値例では、火星の位置と速度を入れています。

### 最適化準備

#### 1. 変数のリシェイプ

- `x_node_int_vector`, `u_node_int_vector`: 初期状態と制御入力を一列に並べ替え、最適化計算に使用します。
- `y0`: 初期値ベクトルとして、状態量と制御入力を結合します。このベクトルを、最適化で用います。

#### 2. 最適化変数の上限と下限を設定

```matlab
yL = -[Inf*ones(length(x_node_int_vector),1); Inf*ones(length(u_node_int_vector),1)];
yU =  [Inf*ones(length(x_node_int_vector),1); Inf*ones(length(u_node_int_vector),1)];
```

上記のコードでは、最適化変数の上限と下限を設定しています。`yL`は最適化変数の下限を無限小に、`yU`は上限を無限大に設定しています。この範囲設定により、特定の制約を適用しない状態での最適化が行われます。

### 制約条件と目的関数の確認

- `confun(y0, auxdata)`: 制約条件が正しく設定されているかを確認します。
- `objfun(y0, auxdata)`: 目的関数が正しく計算されているかを確認します。

### 最適化オプションの設定と実行

#### `optimoptions`による最適化オプションの設定

```matlab
options = optimoptions('fmincon', ...
    'Algorithm', 'sqp', ...
    'Display', 'iter-detailed', ...
    'UseParallel', false, ... % 並列化を有効にするなら true
    'StepTolerance', 1e-6, ...
    'OptimalityTolerance', 1e-4, ...
    'ConstraintTolerance', 1e-12, ...
    'MaxFunctionEvaluations', 5e5, ...
    'MaxIterations', 100);
```

- `Algorithm`：逐次2次計画法（SQP）アルゴリズムを選択。
- `Display`：各イテレーションの詳細を表示。
- `UseParallel`：最適化の並列化。trueにするとonになり、速くなります。ParallelComputingToolboxが必要です。
- `StepTolerance`：最適化のステップ長さの最小値。これより小さいと更新しても意味がないので打ち切りになります。
- `OptimalityTolerance`：最適性の条件。勾配が1e-4になれば勾配=0（局所最適）とみなす。
- `ConstraintTolerance`：境界条件の制約条件。これ以下になれば、境界条件を満たす。
- `MaxFunctionEvaluations`と`MaxIterations`：最大関数評価数と最大イテレーション数の設定。

#### `fmincon`による最適化の実行

```matlab
[yopt, f, output] = fmincon(@(y) objfun(y, auxdata), y0, [], [], [], [], yL, yU, @(y) confun(y, auxdata), options);
```

- `fmincon`：目的関数`objfun`と制約条件`confun`を利用し、最適化を実行します。
- `yopt`：最適化後の変数ベクトル。
- `f`：最適化の最終目的関数値。
- `output`：最適化の詳細な実行情報。

最適化の実行状況は以下のように表示されます
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3901421/a91d263d-fb35-27d0-08d8-72e7404411ea.png)
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3901421/03c934bd-b8dd-eb18-ba4b-0030de0c0e03.png)

- Iter：反復回数
- Func-count：最適化内部での回数（詳細不明）
- Fval：目的関数の値（これが減っていくと消費燃料が減る）
　　　　　※最初増えているのは、初期軌道を弾道飛行にしているため）
- feasibility：境界条件をどれだけ満たすかを示す
- Step length, Norm of step：最適化のステップの長さ。小さいとほとんど進まない。これが小さいのに、feasibilityが大きいと、（境界条件を満たすような解が）解けていないので、パラメータ調整を見直そう。
- First-order optimality：目的関数の1次微分。これが0に近いと、局所解に近づいていることを表す。

※Delta V sum は、最適解のΔVの合計、その下には解析解を出力します。

#### 追加の最適化実行（必要に応じて）
fminconを用いた最適化のテクニックです。1回の最適化で終わらない場合、最適解を初期解にして2回目の最適化を行うを収束することがあります。それでも収束しない場合、必要であれば、係数`coeff_obj`を増やして再度最適化を行います。デフォルトの数値例では不要です。

### 結果のプロット

- `flag_opt`を`1`に設定し、最適化後のフラグとした後、`plot_traj_from_y`を実行して軌道の結果をプロットします。

### 5.2 confun
最適化におけるconstraint（等式制約、不等式制約）を記述するコード。
fminconを用いた軌道最適化において、軌道伝播はこの制約に持ってきます。

```matlab

function [c, ceq] = confun(y, auxdata)

N = auxdata.N;
options = auxdata.options;
x_node_int = reshape(y(1:6*(N)),6,N)';% (N)×6の行列
u_node     = reshape(y(6*(N)+1:6*(N)+4*N),4,N)';% N×4の行列
T_sum = [cumsum(u_node(1:N, 4))];% 0, T1, T1+T2, ...のベクトル

ceq_node = [auxdata.y_node_int_all(1,1:6)-x_node_int(1,:)];
x_all_plot = [];

for rev = 1 : N-1
    tspan   = [T_sum(rev) T_sum(rev+1)];
    x0 = [x_node_int(rev,1:3), x_node_int(rev,4:6)+u_node(rev,1:3)];
    [t, x] = ode45(@(t, x) x_dot_2bp(t, x, auxdata), tspan, x0, options);
    x_all_plot = [x_all_plot;x];
    ceq_node = [ceq_node;x(end,:)-x_node_int(rev+1,:)];
end
ceq_node = [ceq_node;x(end,1:3)-auxdata.x_target(1:3), x(end,4:6)+u_node(N, 1:3)-auxdata.x_target(4:6)];
ceq = reshape(ceq_node', [], 1);% 等式制約
c = [];% 不等式制約は今回無し

figure(1)
% close
axis equal
% 太陽をプロットする（大きさは、半径R_sunを使用）（2次元平面で）
theta = linspace(0, 2*pi, 20);
x_sun = auxdata.R_sun * cos(theta);
y_sun = auxdata.R_sun * sin(theta);
fill(x_sun, y_sun, 'r', 'FaceAlpha', 0.5, 'EdgeColor', 'k', 'LineWidth', 2)
hold on
plot(x_all_plot(:,1), x_all_plot(:,2), Color=[1,0,0], LineWidth=2)% 軌道を赤線で描画
hold on
plot(x_node_int(:,1), x_node_int(:,2), 'b*', MarkerSize=10)

end
```
### `confun`関数の説明

#### 機能概要
この関数は、最適化の制約条件を定義し、軌道シミュレーションを行います。また、状態量が時間の経過とともに目標に到達するかを等式制約として確認します。

---

### 1. 定数と変数の準備

- `N`：ノード数を設定します。
- `options`：数値解法オプションを読み込みます。
- `x_node_int`：状態変数を6×Nの行列に整形し、各ノードでの位置と速度を抽出します。
- `u_node`：制御変数を4×Nの行列に整形します。
- `T_sum`：累積時間のベクトルを生成し、各ノードでの終了時間を計算します。

---

### 2. 制約条件の準備

- `ceq_node`：最初のノードと初期状態との差分を等式制約の初期値として設定します。
- `x_all_plot`：軌道プロット用の変数で、ODEのシミュレーション結果を保存するために使用します。

---

### 3. 軌道シミュレーションと制約条件の更新

- `for`ループにより、ノードごとに軌道を伝播します。これが、multiple shootingになります。
    - `tspan`：各ノードの時間範囲を設定します。
    - `x0`：各ノードの初期位置と速度（制御入力で調整）を設定します。
    - `ode45`：2体問題の微分方程式`x_dot_2bp`を数値解法で解き、軌道を計算します。
    - `ceq_node`：各ノードの終了状態と次ノードの初期状態との差分を等式制約に追加します。

※ここを、forではなくparforに書き換えることで、並列化ができるようになります。Parallel Computing Toolboxが必要です。

#### 目標点との比較

- 最後のノードで、最終位置と速度が目標地点`auxdata.x_target`に一致するように等式制約を追加します。

#### 等式制約と不等式制約の定義

- `ceq`：すべての等式制約を列ベクトルに整形します。
- `c`：今回は不等式制約がないため、空に設定します。

---

### 4. 図のプロット

- `figure(1)`：軌道図を描画します。
- `fill`：太陽を円で描画し、色を赤に設定します。
- `plot`：軌道を赤線、ノード位置を青い星型マーカーで描画します。

---

この関数により、軌道が目標地点に到達するかを制約条件として定義し、プロットで確認できるようになっています。


### 5.3 objfun
```matlab
% objfun
function [L] = objfun(y, auxdata)
    N = auxdata.N;
    u_node     = reshape(y(6*(N)+1:6*(N)+4*N),4,N)';% N×4の行列
    thrust_node = u_node(:,1:3);
    coeff_obj = auxdata.coeff_obj;
%     L = 1;% 燃料最小化ではなく、境界条件を達成するだけの問題のときはこちら。 
    L = coeff_obj*sum(sum(thrust_node.^2)); % 燃料最適化したいときに付ける
end
```

### `objfun`関数の説明

#### 機能概要
この関数は、燃料最小化のための目的関数を計算します。具体的には、各ノードでの推力の二乗和を合計し、燃料消費量を最小化するための値を出力します。

---

### 1. 初期化と変数の準備

- `N`：ノード数を読み込みます。
- `u_node`：制御変数を4×Nの行列に整形し、各ノードでの推力と終了時間に対応する値を抽出します。
- `thrust_node`：`u_node`から推力部分（x, y, z方向の成分）を取り出します。

---

### 2. 目的関数の計算

- `coeff_obj`：燃料消費に対する重み係数を読み込みます。
- `L`：目的関数の計算式で、燃料最小化を行うために推力の二乗和を合計します。
    - `sum(sum(thrust_node.^2))`：各ノードでの推力二乗を合計し、燃料消費量を評価します。
    - `coeff_obj`：重み係数を掛けることで、目的関数のスケールを調整します。

---

### 備考

- 燃料最小化が不要な場合、`L = 1`の行を使用して単に境界条件を満たす問題として解くことができます。

### 5.4 plot_traj_from_y
```matlab
% yを受け取って、それから軌道を描画します。
close all
if flag_opt == 0
    y_plot = y0;
elseif flag_opt == 1
    y_plot = yopt;
end

%% ここはまだスケーリングされている、
x_node     = reshape(y_plot(1:6*(N)),6,N)';% (N)×6の行列
u_node     = reshape(y_plot(6*(N)+1:6*(N)+4*N),4,N)';% N×4の行列
x_node_real(:, 1:3) = x_node(:, 1:3) * lsf; % x, y, z のスケーリングを元に戻す
x_node_real(:, 4:6) = x_node(:, 4:6) * lsf / tsf; % vx, vy, vz のスケーリングを元に戻す
% u_node(:, 1:3) = u_node(:, 1:3) * lsf / tsf; % vx, vy, vz のスケーリングを元に戻す

T_sum = [cumsum(u_node(1:N, 4))];% 0, T1, T1+T2, ...のベクトル
x_node_end = [];
options = auxdata.options;

figure(1)
axis equal
grid on
% 太陽をプロットする（大きさは、半径R_sunを使用）（2次元平面で）
theta = linspace(0, 2*pi, 100);
x_sun = auxdata.R_sun_km * cos(theta);
y_sun = auxdata.R_sun_km * sin(theta);
fill(x_sun, y_sun, 'r', 'FaceAlpha', 0.5, 'EdgeColor', 'k', 'LineWidth', 2)
hold on
% 火星をプロット
x_mars = auxdata.R_mars_km * cos(theta);
y_mars = 227936640 + auxdata.R_mars_km * sin(theta);
fill(x_mars, y_mars, 'r', 'FaceAlpha', 0.5, 'EdgeColor', 'k', 'LineWidth', 2)
hold on
xx_mars = 227936640 * cos(theta);
yy_mars = 227936640 * sin(theta);
plot(xx_mars, yy_mars, '--', Color=[0.5 0.5 0.5])
hold on
% 地球の公転軌道をプロット
xx_earth = 1.496e8 * cos(theta);
yy_earth = 1.496e8 * sin(theta);
plot(xx_earth, yy_earth, '--', Color=[0.5 0.5 0.5])
hold on
plot(x_node_real(1,1), x_node_real(1,2), 'r*', MarkerSize=10, LineWidth=2)
hold on

for node_num = 1 : N-1
    tspan   = [T_sum(node_num) T_sum(node_num+1)];
    x0 = [x_node(node_num,1:3), x_node(node_num,4:6)+u_node(node_num,1:3)];
    [t, x] = ode45(@(t, x) x_dot_2bp(t, x, auxdata), tspan, x0, options);
    x(:, 1:3) = x(:, 1:3) * lsf; % x, y, z のスケーリングを元に戻す
    x(:, 4:6) = x(:, 4:6) * lsf / tsf; % vx, vy, vz のスケーリングを元に戻す
    x_node_end = [x_node_end;x(end,:)];
    figure(1)
    hold on
    plot(x(:,1), x(:,2), Color=[1,0,0], LineWidth=2)% 軌道を赤線で描画
    hold on
    plot(x0(1), x0(2), 'r*', MarkerSize=5, LineWidth=2)% 軌道の始点は赤*
    hold on
    plot(x(end), x(end), 'b*', MarkerSize=5, LineWidth=2)% 軌道の終点は青*
    hold on
    quiver(x_node_real(node_num,1), x_node_real(node_num,2), 1e7*u_node(node_num,1), 1e7*u_node(node_num,2), 'LineWidth', 2, 'MaxHeadSize',20, Color=[0 0 0])
    hold on
end
quiver(x_node_real(N,1), x_node_real(N,2), 1e7*u_node(N,1), 1e7*u_node(N,2), 'LineWidth', 2, 'MaxHeadSize',20, Color=[0 0 0])


%% ΔVの合計  km/s
delta_v = u_node(:, 1:3);
delta_v_sum_km_s = sum(vecnorm(delta_v, 2, 2)); 
disp(['Delta V sum = ', num2str(delta_v_sum_km_s), ' km/s'])
```

### 軌道描画コードの解説

#### 機能概要
このコードは、最適化前または最適化後の軌道を描画します。軌道の各ノードをプロットし、推力方向のベクトルと目標地点である火星の位置も描画されます。また、軌道全体のΔV（デルタV）を計算し、推力総量を評価します。

---

### 1. 軌道データの設定

- `y_plot`：最適化の実行前(`flag_opt = 0`)は初期データ`y0`、実行後(`flag_opt = 1`)は最適化後のデータ`yopt`が設定されます。
- `x_node`および`u_node`：スケーリングした後の位置および速度データと、各ノードにおける推力データを変数に変換します。※軌道伝播の際にスケーリング後の状態が必要になります。図はスケーリングする前（実際の物理量）です。
- `x_node_real`：実際の物理量にスケーリング（距離`lsf`と時間`tsf`）を使って戻して、km, km/s単位での軌道を計算します。

---

### 2. 軌道の準備

- `T_sum`：各ノードの経過時間を累積和で計算し、時間区間`[0, T1, T1+T2, ...]`のベクトルを生成します。
- `x_node_end`：最終ノードの位置データを格納するためのリストです。

---

### 3. 描画の設定

#### 太陽と火星の描画

- 太陽：半径`R_sun_km`で描画し、赤で塗りつぶした円として表示。
- 火星：半径`R_mars_km`の円と、目標軌道円を表示し、火星の位置を赤で塗りつぶして描画。
- 地球の公転軌道：半径1.496e8 kmの円軌道として表示します。

#### 軌道の描画

- 各ノード間での推力を計算し、そのノードでの開始位置からODEを用いて次ノードの位置へ軌道を描きます。
- 各ノードにおける位置の始点と終点をそれぞれ赤色と青色のマーカーで表示します。

#### 推力ベクトルの描画

- 各ノードにおける推力ベクトルを黒色矢印で描画し、力の向きと大きさを視覚化します。`quiver`関数を使用して、各ノード位置での推力方向を描画します。

---

### 4. ΔV（デルタV）合計の計算

- `delta_v_sum_km_s`：すべてのノードでの推力量（ΔV）の合計を計算し、km/s単位で表示します。
  - `vecnorm`関数を用いて各ノードでの推力ベクトルのノルムを算出し、合計することで全体のΔVを評価します。


最適化が終わると、図のように地球火星遷移軌道と、インパルスΔVを矢印で表示します。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3901421/c725f3fc-31b2-6908-3592-eff1d4703b7c.png)


## 5.5 auxdata_set
定数を構造体に保存します。confun, objfunでは構造体として呼び出すことで、各定数を使用できます。
```matlab
%% 定数の定義（kmやkgなどの単位で定義）
g0_km_s2         = 9.8e-3;       % 重力定数 (km/s^2)
M_earth_kg       = 5.972e24;     % 地球の質量 (kg)
R_sun_earth_km   = 1.496e8;      % 太陽と地球の平均距離 (km)
R_sun_mars_km    = 227936640; % 火星の公転軌道半径 (km)
R_earth_km       = 6371;         % 地球の半径（km）
R_sun_km         = 6.960e5;      % 太陽の半径（km）
R_mars_km        = 6734;         % 火星の半径（km）
day_to_sec       = 60*60*24;     % 1日の秒数
earth_rev_days   = 3.652E+02;    % 地球の公転周期（日）

%% スケーリングファクターを適用
g0               = g0_km_s2 * tsf^2 / lsf; % 重力定数（スケーリング後）
R_sun_earth      = R_sun_earth_km / lsf;   % 太陽と地球の距離（スケーリング後）
R_sun_mars       = R_sun_mars_km  / lsf;   % 太陽と火星の距離（スケーリング後）
R_earth          = R_earth_km / lsf;       % 地球の半径（スケーリング後）
R_sun            = R_sun_km / lsf;         % 太陽の半径（スケーリング後）
R_mars           = R_mars_km / lsf;        % 火星の半径（スケーリング後）

%% 定数の計算（lsf, tsfでスケーリング）
mu_earth         = 398600.4418 / (lsf^3 / tsf^2); % 地球の重力定数（スケーリング後）
mu_sun           = 1.327124e+11 / (lsf^3 / tsf^2); % 太陽の重力定数（スケーリング後）
mu_se            = mu_earth / (mu_earth + mu_sun); % 地球と太陽の重力定数の比
omega            = sqrt((mu_sun + mu_earth) / R_sun_earth^3); % 角速度（rad/s）

%% auxdataに定数を格納
auxdata.lsf             = lsf;
auxdata.tsf             = tsf;
auxdata.g0              = g0;
auxdata.M_earth         = M_earth_kg;
auxdata.R_sun_earth     = R_sun_earth;
auxdata.R_sun_mars      = R_sun_mars;
auxdata.R_sun           = R_sun;
auxdata.R_earth         = R_earth;
auxdata.R_mars          = R_mars;
auxdata.R_earth_km      = R_earth_km;
auxdata.R_mars_km       = R_mars_km;
auxdata.R_sun_km        = R_sun_km;
auxdata.R_sun_earth_km  = R_sun_earth_km;
auxdata.R_sun_mars_km   = R_sun_mars_km;
auxdata.mu_earth        = mu_earth;
auxdata.mu_sun          = mu_sun;
auxdata.mu_se           = mu_se;
auxdata.omega           = omega;
auxdata.day_to_sec      = day_to_sec;
auxdata.earth_rev_days  = earth_rev_days;

% その他のパラメータをauxdataに格納
auxdata.N               = N;
auxdata.coeff_obj       = coeff_obj;
auxdata.y_node_int_all  = y_node_int_all;
auxdata.u_node_int_all  = u_node_int_all;

% ODE solver options
options = odeset('RelTol', 1e-8, 'AbsTol', 1e-8);
auxdata.options = options;
```

# 6. 順伝播/逆伝播
今回のコードでは、宇宙機の軌道が進む方向と、積分の方向を同じ方向にしました。しかし、例えば到着惑星の重力を考慮して最適化を行うとき、惑星の周回軌道を終点にして最適化を解こうとすると、惑星付近の非線形性が強い領域を目標にしてshoootすることになります。これでは、最適化が非常に解きにくくなるので、終点から逆方向に軌道伝播（shoot）することで収束性が良くなります。