Week 8 — 姿態確定與控制

基礎知識導論

Week 8 基礎知識導論 — 姿態動力學與 B-dot 消旋

衛星從部署器彈出來時是在翻滾的。在停止翻滾之前,
太陽能板發電不穩、天線指不到地面、相機拍不了照。
消旋 (detumbling) 是入軌後的第一件工作,也是 ADCS 最基本的功能。

1. 這一週在解什麼問題

給定部署時的初始角速度(典型 5–20 °/s,dispenser 規格會給上限),
只用磁力矩器(最便宜、最可靠、無活動件的致動器),
需要多久才能把角速度降到可以開始正常運作的程度?

本週寫一個閉迴路模擬,把姿態輸出成 .a 檔餵回 STK 看 3D 動畫。

2. 核心觀念

2.1 姿態的三種表示法

飛行軟體幾乎一律用四元數。 歐拉角只用來顯示給人看。

2.2 剛體轉動方程(Euler 方程)

I * dω/dt  +  ω × ( I * ω )  =  τ_control + τ_disturb

• I 慣性張量 (kg·m²);ω 機體角速度 (rad/s)
• ω × (Iω) 是陀螺耦合項 —— 這是非線性的來源,也是「翻滾會換軸」的原因
• 對 6U CubeSat,I 的對角元素量級約 0.02–0.1 kg·m²

2.3 干擾力矩:誰在推你

這些量級聽起來很小,但它們持續作用,一天累積下來足以讓衛星轉起來。
ADCS 的一半工作是對抗它們。

2.4 磁力矩器:唯一無活動件的致動器

線圈通電產生磁矩 m (A·m²),與地磁 B 作用:

τ = m × B

關鍵限制:力矩必定垂直於 B。
所以沿著 B 方向的角動量分量,這一瞬間完全無法控制。
磁力矩器是欠致動 (under-actuated) 的 —— 它靠的是 B 隨軌道變化,
在不同時刻控制不同的軸,長期下來才把三軸都收掉。

這就是為什麼消旋需要好幾圈,而不是幾分鐘。

2.5 B-dot 控制律

最經典的消旋律,只需要磁力計,不需要知道姿態:

m = -k * dB_body/dt              (B-dot)

或用角速度形式(需要陀螺儀):

m = -k * ( ω × B )

為什麼有效:機體轉動時,機體座標下量到的地磁向量 B_body 會變化。
這個變化率正比於角速度在垂直 B 平面上的分量。
把磁矩反向施加,產生的力矩總是耗散轉動動能:

dE/dt = ω · τ  <  0

它是一個能量耗散律,不是一個追蹤律。它不會把你指到某個方向,
只會把動能一路吃到接近零。這正是消旋需要的。

2.6 地磁模型:IGRF vs 傾斜偶極

本週最重要的實測結論:

偶極模型高估了 B-dot 的效能。 原因是偶極場太「乾淨」,
沿軌道的變化規律,B-dot 幾乎每一刻都在有效方向上;
真實地磁的高階項讓局部方向變化更雜亂,有效耗散降低。

用簡化模型做設計、用真實模型做驗證,結論可能差一個數量級。
這是本週的核心教學點。

2.7 一個數值方法的陷阱

B-dot 律會快速切換(磁矩方向頻繁反轉,實務上還常用 bang-bang 控制)。
自適應步長積分器(如 RK45、LSODA)遇到這種不連續會不斷縮小步長,
最後停滯不前 —— 表現是程式跑很久卻沒進展。

正確作法:固定步長 RK4,步長取控制週期的分數。
這是控制模擬與純物理模擬的重要差別:控制器本身是離散的,
積分器不該假裝系統是連續光滑的。

2.8 把姿態送回 STK

STK 的 .a(Attitude)檔可以吃外部姿態時間序列,
再用 3D 視窗把消旋過程視覺化。這一步的意義不只是好看:

• 可以檢查四元數的慣例對不對(標量在前 vs 在後、機體對慣性 vs 反過來)
• 可以檢查太陽能板在消旋過程中的照射狀況(接回 Week 7)
• 可以檢查天線指向(接回 Week 11)

慣例弄反是姿態工作最常見的 bug,而 3D 視窗是最快的檢查工具。

3. 名詞與符號

4. 觀念 → 本週實驗對照

效能註記:geomag.py 把 B(t) 在 5 秒網格上用一次向量化呼叫算完並快取,
讓本週的執行時間從 33.7 分鐘降到約 2.8 秒,最大相對誤差 0.0036%。
這是「先想清楚資料流,再寫迴圈」的實例。

5. 常見誤解

1. 「磁力矩器可以三軸同時控制。」 — 不行。力矩必定垂直 B,
   沿 B 的分量該瞬間完全不可控。要靠 B 隨軌道變化來補。
2. 「B-dot 可以把衛星指到某個方向。」 — 不行。B-dot 只耗散動能,
   不做姿態追蹤。指向要靠反作用輪或磁力矩器 + 姿態決定演算法。
3. 「用偶極模型估一下就好。」 — 見 2.6,實測差距很大,而且是樂觀方向的錯。
4. 「積分器用預設的就好。」 — 見 2.7,不連續控制律需要固定步長。
5. 「四元數只是數學技巧。」 — 慣例弄反會讓姿態反向旋轉,而且看起來很合理,
   是最難抓的 bug 之一。永遠用 3D 視覺化檢查一次。

6. 延伸閱讀

姿態動力學與控制

• Basilisk 天體動力學模擬框架(Schaub 教授團隊)
  — 開源飛行動力學模擬器,姿態控制的參考實作與豐富文件。
• MIT OCW 16.851 Satellite Engineering
  — ADCS 章節的完整講義。
• NASA Technical Reports Server (NTRS)
  — 搜尋 "B-dot detumbling CubeSat",有大量實際任務的參數與結果可對照。
• 教科書:Schaub & Junkins, Analytical Mechanics of Space Systems;
  Wertz, Spacecraft Attitude Determination and Control(經典)。

地磁模型

• NOAA NCEI:International Geomagnetic Reference Field
  — IGRF 的官方發布頁,含係數檔與線上計算器。
• ppigrf(本週使用的 Python IGRF 實作)
  — 注意:它要吃 datetime.datetime,給 datetime.date 會出錯。
• NOAA 太空天氣預報中心 — 地磁擾動與太陽活動。

工具

• Ansys STK 線上說明 — Attitude 檔案格式與外部姿態匯入。
• SciPy integrate — 積分器選擇與剛性問題。

7. 課前自我檢核

1. 為什麼磁力矩器無法在單一瞬間控制三個軸?這對消旋所需時間有什麼影響?
2. B-dot 律為什麼一定會耗散動能?寫出 dE/dt 並說明它的符號。
3. 偶極模型與 IGRF-13 的消旋結果差了多少?哪一個比較樂觀?為什麼這個方向很危險?
4. 自適應步長積分器遇到 bang-bang 控制會發生什麼事?怎麼判斷你正在踩這個坑?
5. 你怎麼確認送進 STK 的四元數慣例是對的?

Week8_Bdot_Lab_Handout.md

Week 8 實驗單 — B-dot Detumbling(磁力矩器消旋)

課程:CubeSat 設計與實作 · Week 8 姿態確定與控制(ADCS)
工具:Python(numpy / scipy / matplotlib)+ 選配 pyIGRF/ppigrf;STK 提供軌道輸入
檔案:week8_bdot_detumbling.py
預計時數:2–3 小時

1. 為什麼這個 Lab 用 Python 不用 STK

CubeSat 從發射器(P-POD)彈出後會翻滾(tumbling),每軸可能有數度/秒的角速度。入軌第一件事就是 detumbling——把翻滾消掉,姿態才穩得下來、太陽能板才對得到太陽、天線才對得到地面站。

STK 能算「衛星在哪、該指向哪」,但姿態控制的閉迴路模擬(控制律如何把翻滾轉速降下來)在教學版 STK 做不到(那是 SOLIS 付費模組的範疇)。所以這一塊我們用開源 Python 補上:自己寫剛體姿態動力學 + B-dot 控制律。分工是:STK 給軌道幾何,Python 做物理響應。

2. 學習目標

完成後你應該能夠:

1. 寫出剛體姿態的運動學(四元數)與動力學(尤拉方程)。
2. 說明 B-dot 控制律 m = -k·(dB/dt) 為何能消旋,以及為何只需磁力計與陀螺就能實作。
3. 解釋為什麼磁力矩器只能產生垂直於當地磁場的力矩(欠致動),以及這對收斂的影響。
4. 判讀消旋曲線:角速度衰減、磁矩飽和、磁場沿軌道的振盪。
5. 把 STK 匯出的軌道星曆接進模擬,取代內建的解析圓軌道。

3. 背景理論(簡版)

姿態運動學(body 相對慣性系的四元數 q):

q̇ = ½ Ω(ω) q

姿態動力學(尤拉方程,I 為慣量張量,τ 為外力矩):

I ω̇ = τ − ω × (I ω)

B-dot 控制律。磁力矩器產生磁矩 m,在地磁場 B 中受力矩 τ = m × B。取

m = −k · Ḃ_body

其中 Ḃ_body 是衛星本體座標下量到的磁場變化率。當衛星在轉,B 在本體座標看起來就在變;B-dot 律讓磁矩去「反抗」這個變化,結果是把轉動動能持續抽走。經典的量測式實作用 Ḃ ≈ −ω × b̂(b̂ 為磁場單位向量),所以只要磁力計 + 陀螺就能算,不需要真的做數值微分——這是 B-dot 在真實 CubeSat 上如此普及的原因。

關鍵限制(欠致動):τ = m × B 永遠垂直於 B,所以沿著 B 方向的角動量分量當下無法直接消。靠的是衛星沿軌道飛行時 B 方向一直在變(尤其極軌,一軌內 B 轉很多),各方向輪流被消掉,最終整體收斂。

4. 操作步驟

Part A — 跑基準案例(30 min)

1. 直接執行 python week8_bdot_detumbling.py。
2. 觀察主控台輸出:初始 |ω|、最終 |ω|、消旋百分比、降到初始 10% 所需時間(以軌道週期計)。
3. 打開 week8_bdot_detumbling.png,三張圖由上而下:
   - |ω| vs 時間:應看到明顯衰減。基準(USE_IGRF = True,IGRF-13)8 軌降到約 20% 初始值;
   把 USE_IGRF 改成 False 換成傾斜偶極,同樣 8 軌會降到約 1%。
   兩個都跑,並在報告裡解釋為什麼解析偶極會高估 B-dot 效能。
   - 磁矩 |m|:初期大多貼在飽和線(硬體上限),隨轉速下降才鬆開。
   - |B| 本體:隨軌道週期性振盪(極軌一軌約兩個峰),這是欠致動能收斂的物理基礎。

紀錄:初始/最終角速度、消旋百分比、收斂軌道數 —— IGRF 與偶極各記一組。

Part B — 參數研究(60 min)

改檔案上方參數,各跑一次,記錄收斂時間並解釋趨勢:

思考題:為什麼在 M_MAX 固定時,把 K_BDOT 加大到某程度後收斂時間幾乎不再改善?(提示:看磁矩圖是否整段飽和。)

Part C — 接 STK 軌道(45 min)

1. 在 STK 建一顆 SSO ~600 km 衛星(可用你先前的 FIREYE/STKSat 場景改傾角)。
2. 匯出 ECEF/ECI 星曆為 CSV(time, x, y, z)。可用:
   - Connect:ExportDataFile */Satellite/<name> Ephemeris "...\ephem.e" Type STK ...,或
   - 資料提供者輸出 Cartesian Position(time,x,y,z)另存 CSV。
3. 在 week8_bdot_detumbling.py 最下方取消註解:
   python load_stk_ephemeris("stk_ephem_ecef.csv", pos_unit="km")
4. 重跑,確認主控台顯示 Orbit source: STK ephemeris。比較用 STK 真實軌道 vs 內建圓軌道,收斂曲線是否改變。

Part D(選作)— 用真實 IGRF 磁場

pip install ppigrf(或 pyIGRF)。程式會自動偵測並改用 IGRF-13,主控台會顯示 field: IGRF (ppigrf)。比較 IGRF 與傾斜偶極兩種磁場模型下的收斂差異,討論保底解析模型的誤差來源。

5. 繳交與評分

繳交:修改後的 .py、產生的 .png、一頁討論(含參數研究數據表)。

6. 延伸與銜接

• 接 Week 6 熱:detumbling 完成後姿態穩定,才能定義各面朝向 → 熱網路的日照面才確定。
• 接下一階段 ADCS:detumbling 只是「消旋」,穩定後才進入指向控制(reaction wheel + nadir/target 追蹤),那需要 STK 給參考指向、Python 做 PD/PID 閉迴路。可作為 Week 8 的第二個 Lab。
• 業界級工具:想更完整(含 flight software-in-the-loop),可進階到 Basilisk(開源太空載具模擬框架)。

7. 常見問題

• 沒裝 pyIGRF/ppigrf 會怎樣? 程式自動退回傾斜偶極解析磁場,Lab 照常完整執行,只是磁場精度較低。
• 收斂看起來很慢/不收斂? 檢查 M_MAX 是否太小(整段飽和)、T_END 是否太短(軌道數不夠)、初始 W0 是否過大。
• 四元數為何要每步正規化? 數值積分會讓 |q| 漂離 1,不正規化會累積誤差使 DCM 失真。程式每步都做了 quat_norm。

圖

week08_adcs/week8_bdot_detumbling.png

week08_adcs/week8_detumble_integrated.png

程式碼

# -*- coding: utf-8 -*-
# ================================================================
# geomag.py — 地磁場模型(IGRF-13 或傾斜偶極),附時間網格快取
#
# 為什麼要這個模組:
#   B-dot 模擬用固定步長 RK4,每步呼叫 4 次磁場。8 個軌道週期 / dt=1s
#   = 46,080 步 → 184,320 次磁場呼叫。ppigrf 逐點呼叫實測 11.0 ms,
#   光磁場就要 33.7 分鐘,整個 Lab 沒辦法在課堂上跑完。
#
#   但 B 只是「時間的函數」(軌道與姿態無關),而且沿軌道變化平滑。
#   所以改成:先在粗時間網格上「一次向量化」算好 B(t),積分時線性內插。
#   實測 9,216 點的向量呼叫只要 0.19 s —— 逐點要 1.7 分鐘。
#
# 另外修掉原本兩支腳本共同的兩個問題:
#   1. ppigrf.igrf() 傳 datetime.date 會拋
#      TypeError: Cannot compare Timestamp with datetime.date(pandas 2.x),
#      要傳 datetime.datetime。
#   2. 原本寫 try/except Exception: pass,所以上面那個錯誤被靜默吞掉,
#      每一步都白算一次再退回偶極模型 —— 學生以為在用 IGRF-13,其實不是。
#      這裡改成只警告一次並明講退回原因。
# ================================================================

import datetime as _dt
import numpy as np

R_EARTH = 6378137.0        # m
B0      = 3.12e-5          # T,赤道地表偶極場強


def import_igrf():
    """回傳 (kind, module);都沒裝時回傳 (None, None)。"""
    try:
        import ppigrf
        return "ppigrf", ppigrf
    except ImportError:
        pass
    try:
        import pyIGRF
        return "pyIGRF", pyIGRF
    except ImportError:
        pass
    return None, None


def _lla_from_eci(xyz):
    """把 ECI 位置當 ECEF 用(忽略地球自轉相位,對本 Lab 足夠)。
    xyz: (N,3) m  ->  lat(deg), lon(deg), alt(km)"""
    x, y, z = xyz[:, 0], xyz[:, 1], xyz[:, 2]
    r = np.linalg.norm(xyz, axis=1)
    lat = np.degrees(np.arcsin(z / r))
    lon = np.degrees(np.arctan2(y, x))
    alt_km = (r - R_EARTH) / 1e3
    return lat, lon, alt_km


def _ned_to_eci(Bn, Be, Bu, lat, lon):
    """地理 NEU 分量 -> ECI 向量(向量化)。回傳 (N,3),單位 T。"""
    latr, lonr = np.radians(lat), np.radians(lon)
    sla, cla = np.sin(latr), np.cos(latr)
    slo, clo = np.sin(lonr), np.cos(lonr)
    e_hat = np.stack([-slo,          clo,        np.zeros_like(slo)], axis=1)
    n_hat = np.stack([-sla * clo,   -sla * slo,  cla],                axis=1)
    u_hat = np.stack([ cla * clo,    cla * slo,  sla],                axis=1)
    B = (Bn[:, None] * n_hat + Be[:, None] * e_hat + Bu[:, None] * u_hat)
    return B * 1e-9        # nT -> T


def dipole_eci(xyz):
    """傾斜偶極解析場(向量化)。xyz: (N,3) m -> (N,3) T。"""
    r = np.linalg.norm(xyz, axis=1)
    r_hat = xyz / r[:, None]
    m_hat = np.array([0.0, 0.0, -1.0])          # 偶極軸約反平行地理北
    coeff = B0 * (R_EARTH / r) ** 3
    return coeff[:, None] * (3.0 * (r_hat @ m_hat)[:, None] * r_hat - m_hat)


class GeoField:
    """B(t) 的時間網格快取。

        field = GeoField(r_eci, T_END, grid_dt=5.0)
        B = field(t)          # (3,) ECI, Tesla

    r_of_t(t) 必須是「只吃時間」的純函數(回傳 (3,) m)。
    載入新星曆或改軌道後呼叫 .reset() 讓快取重建。
    """

    def __init__(self, r_of_t, t_end, grid_dt=5.0,
                 epoch=None, prefer_igrf=True):
        self.r_of_t = r_of_t
        self.t_end = float(t_end)
        self.grid_dt = float(grid_dt)
        self.epoch = epoch or _dt.datetime(2024, 1, 1)
        self.prefer_igrf = prefer_igrf
        self.kind, self._mod = import_igrf() if prefer_igrf else (None, None)
        self.note = ""
        self._ts = None
        self._B = None

    def reset(self):
        self._ts = self._B = None

    # -- 快取建立 -------------------------------------------------
    def _build(self):
        n = int(np.ceil(self.t_end / self.grid_dt)) + 2
        ts = np.arange(n) * self.grid_dt
        xyz = np.array([self.r_of_t(float(t)) for t in ts])

        B = None
        if self.kind is not None:
            try:
                B = self._igrf_grid(xyz)
            except Exception as e:
                # 不要靜默:講清楚為什麼退回偶極
                print(f"[geomag] {self.kind} 呼叫失敗，退回傾斜偶極模型："
                      f"{type(e).__name__}: {e}")
                self.note = f"{self.kind} 失敗 -> dipole"
                self.kind = None
        if B is None:
            B = dipole_eci(xyz)

        self._ts, self._B = ts, B

    def _igrf_grid(self, xyz):
        lat, lon, alt_km = _lla_from_eci(xyz)
        if self.kind == "ppigrf":
            # 一次向量呼叫;注意必須傳 datetime.datetime(不是 date)
            Be, Bn, Bu = self._mod.igrf(lon, lat, alt_km, self.epoch)
            Be, Bn, Bu = np.ravel(Be), np.ravel(Bn), np.ravel(Bu)
        else:
            out = np.array([self._mod.igrf_value(la, lo, al, self.epoch.year)
                            for la, lo, al in zip(lat, lon, alt_km)])
            Bn, Be, Bu = out[:, 3], out[:, 4], -out[:, 5]
        return _ned_to_eci(Bn, Be, Bu, lat, lon)

    # -- 取值 -----------------------------------------------------
    def __call__(self, t):
        if self._ts is None:
            self._build()
        return np.array([np.interp(t, self._ts, self._B[:, 0]),
                         np.interp(t, self._ts, self._B[:, 1]),
                         np.interp(t, self._ts, self._B[:, 2])])

    def label(self):
        """給圖表標題用的 ASCII 短標籤(避免中文字型缺字警告)。"""
        if self._ts is None:
            self._build()
        return f"IGRF-13 ({self.kind})" if self.kind else "tilted-dipole"

    def describe(self):
        if self._ts is None:
            self._build()
        base = f"IGRF-13 ({self.kind})" if self.kind else "tilted-dipole (analytic)"
        return (f"{base}, 網格 {self.grid_dt:g} s × {len(self._ts)} 點"
                + (f" [{self.note}]" if self.note else ""))

# -*- coding: utf-8 -*-
# ================================================================
# Week 8 驗證段 — 消旋結果與磁場模型的一致性
#
# 本週最容易出事的不是數值,是「你以為在用 IGRF-13,其實在用偶極 fallback」。
# 舊版把 ppigrf 的呼叫失敗用 except: pass 吞掉,結果 184,320 次呼叫全部白算,
# 學生看到的 98.7% 其實是偶極模型的結果。這一段就是要讓那種情況無所遁形。
#
# 執行:python week08_verify.py
#   離開碼 0 = 全部通過,1 = 有項目對不上
# ================================================================

import os
import sys

# notebook 的 kernel 沒有 __file__。而且啟動 STK Engine 會改變 process 的
# 工作目錄,所以不能事後才靠 os.getcwd() —— notebook 開頭的 bootstrap cell
# 會先把週次目錄釘在 WEEK_DIR。
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
        else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))
REPO = os.path.abspath(os.path.join(HERE, os.pardir))
sys.path.insert(0, os.path.join(REPO, "tools"))

from stk_verify import Verifier, load_lab      # noqa: E402


def main():
    import numpy as np
    sys.path.insert(0, HERE)                 # 讓實驗腳本找得到 geomag
    m = load_lab(os.path.join(HERE, "week8_bdot_detumbling.py"))
    v = Verifier(None)

    # (1) 現在到底在用哪個磁場模型?必須和 USE_IGRF 的設定一致
    label = m._FIELD.label()
    v.expect_true(f"磁場模型與 USE_IGRF={m.USE_IGRF} 一致",
                  ("IGRF" in label) == bool(m.USE_IGRF), label)

    # (2) 網格快取的精度:抽樣比對內插值與逐點精確值
    ts = np.linspace(0, m.T_END, 25)
    exact = m._FIELD._igrf_grid(np.array([m.r_eci(float(t)) for t in ts]))         if m._FIELD.kind else None
    if exact is not None:
        approx = np.array([m._FIELD(float(t)) for t in ts])
        err = np.max(np.linalg.norm(approx - exact, axis=1)
                     / np.linalg.norm(exact, axis=1))
        v.expect_true("5 s 網格內插誤差 < 0.01%", err < 1e-4, f"最大相對誤差 {err*100:.4f}%")
    else:
        v.expect_true("偶極模型無須內插精度檢查", True, label)

    # (3) 消旋結果(IGRF 基準:8 圈降到約 20% 初始值)
    t, W, M, BB = m.simulate()
    Wd = np.degrees(W)
    reduction = 100.0 * (1 - Wd[-1] / Wd[0])
    v.expect("初始角速度", Wd[0], 4.39, tol=0.05, unit=" deg/s")
    if m.USE_IGRF and m._FIELD.kind:
        v.expect("IGRF-13 八圈消旋率", reduction, 80.3, tol=2.0, unit=" %")
    else:
        v.expect("傾斜偶極八圈消旋率", reduction, 99.1, tol=1.0, unit=" %")
    v.expect_true("角速度單調下降(整體趨勢)", Wd[-1] < Wd[0] * 0.5,
                  f"{Wd[0]:.2f} -> {Wd[-1]:.3f} deg/s")

    # (4) 磁矩應該有一段時間貼在飽和線 —— 這是欠致動的物理特徵
    sat_frac = float(np.mean(M >= m.M_MAX * 0.999))
    v.expect_true("初期磁矩曾達飽和", sat_frac > 0.05,
                  f"{sat_frac*100:.1f}% 的時間貼在 {m.M_MAX} A m^2")

    ok = v.report("Week 8 驗證段")
    return 0 if ok else 1


if __name__ == "__main__":
    _rc = main()
    if "ipykernel" not in sys.modules:
        raise SystemExit(_rc)

# -*- coding: utf-8 -*-
# ================================================================
# Week 8 Lab - B-dot Detumbling 磁力矩器消旋模擬
# CubeSat 設計與實作 | STK 做不到(教學版無 SOLIS)的 ADCS 閉迴路,用開源補上
#
# 物理:
#   姿態運動學  q_dot = 0.5 * Omega(w) * q            (body<-inertial 四元數)
#   姿態動力學  I w_dot = tau - w x (I w)              (Euler 方程)
#   B-dot 控制  m = -k * b_dot_body                    (磁力矩器磁矩)
#               tau = m x B_body                       (磁力矩)
#   b_dot 的實作用「量測差分」版本(不需要真微分,較貼近真實硬體):
#               b_dot ~= -w x b_hat   (由陀螺與磁力計即可得,經典 B-dot)
#
# 地磁場 B(沿軌道):
#   有 ppigrf 或 pyIGRF -> 用真實 IGRF-13
#   都沒有            -> 退回「傾斜偶極」解析模型(Lab 仍可完整執行)
#
# 軌道輸入:
#   預設用內建圓軌道解析傳播(不需外部檔)
#   也可吃 STK 匯出的 ECEF 星曆 CSV(見 load_stk_ephemeris)
#
# 輸出:week8_bdot_detumbling.png  + 主控台摘要
# ================================================================

import numpy as np
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt

# ----------------------------------------------------------------
# 0. 常數與任務參數(1U CubeSat 典型值,學生可調)
# ----------------------------------------------------------------
MU_EARTH = 3.986004418e14          # m^3/s^2
R_EARTH  = 6378.137e3              # m
B0       = 3.12e-5                 # T, 地表赤道磁場強度(偶極模型用)

# --- 衛星慣量(1U ~ 1.3 kg,對角化)---
I_BODY = np.diag([2.0e-3, 2.1e-3, 1.8e-3])   # kg m^2
I_INV  = np.linalg.inv(I_BODY)

# --- 磁力矩器 ---
K_BDOT   = 5.0e4                    # B-dot 增益(調這個看收斂快慢)
M_MAX    = 0.2                      # 單軸最大磁矩 A m^2(飽和限制,真實硬體有上限)

# --- 軌道(SSO ~600 km)---
ALT      = 600e3                   # m
INC      = np.radians(97.8)        # SSO 傾角
RAAN     = np.radians(0.0)
SMA      = R_EARTH + ALT
N_ORBIT  = np.sqrt(MU_EARTH / SMA**3)          # 平均角速度 rad/s
PERIOD   = 2*np.pi/N_ORBIT

# --- 初始翻滾(部署後典型:每軸 2-3 度/秒)---
W0 = np.radians(np.array([3.0, -2.0, 2.5]))    # rad/s

# --- 模擬設定 ---
T_END    = 8*PERIOD                # 模擬 8 個軌道週期(足夠看到收斂)
DT       = 1.0                     # s,積分步長

# --- 磁場模型開關(教學重點:兩種模型結論差很多)---
#   True  = IGRF-13 真實地磁場 -> 8 圈消旋 80.3%,末 0.87 deg/s
#   False = 傾斜偶極解析模型   -> 8 圈消旋 99.1%,末 0.04 deg/s
# 偶極模型把 B-dot 的效能高估了。舊版因為 ppigrf 呼叫失敗被靜默吞掉,
# 一直在跑偶極,所以實驗單上寫的 98.7% 其實是偶極的結果。
# 建議兩個都跑一次,討論差異來自哪裡(場強分布、南大西洋異常、極區幾何)。
USE_IGRF = True

# ----------------------------------------------------------------
# 1. 地磁場模型 —— 見 geomag.py
#    B 只是時間的函數(與姿態無關)且沿軌道平滑,所以先在 5 s 網格上
#    「一次向量化」算好再內插:磁場成本從 33.7 分鐘降到約 2.8 秒,
#    相對誤差 < 0.004%。舊版是逐點呼叫 ppigrf,而且把呼叫失敗
#    靜默吞掉(except Exception: pass),實際上一直在用偶極 fallback。
# ----------------------------------------------------------------
from geomag import GeoField
import os

# 產出要落在**週次目錄**,不是 process 的工作目錄。每一支 weekNN_verify.py
# 開頭都有這個慣用法並註明「啟動 STK Engine 會改變 process 的工作目錄」,
# 但 lab 這一側先前沒有套用 —— 結果庫根目錄散落著九個產出,
# 其中一份 week15_launch.png 還是過期的。notebook 的 kernel 沒有 __file__,
# 所以留 WEEK_DIR(notebook 開頭的 bootstrap cell 會設)當後備。
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
        else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))

# ----------------------------------------------------------------
# 2. 軌道:內建圓軌道傳播 或 STK 匯出星曆
# ----------------------------------------------------------------
def r_eci_circular(t):
    """圓軌道 ECI 位置(m)。含傾角與 RAAN。"""
    theta = N_ORBIT * t
    # 軌道平面內
    x_p = SMA*np.cos(theta); y_p = SMA*np.sin(theta); z_p = 0.0
    # 傾角旋轉(繞 x)後 RAAN 旋轉(繞 z)
    ci, si = np.cos(INC), np.sin(INC)
    cO, sO = np.cos(RAAN), np.sin(RAAN)
    x1, y1, z1 = x_p, y_p*ci - z_p*si, y_p*si + z_p*ci
    x  = x1*cO - y1*sO; y = x1*sO + y1*cO; z = z1
    return np.array([x, y, z])

_STK_EPHEM = None
def load_stk_ephemeris(csv_path, tcol=0, xcol=1, ycol=2, zcol=3, pos_unit="km"):
    """吃 STK 匯出的星曆 CSV(時間 + ECEF/ECI 位置)。
    在 STK:ReportCreate 或 ExportDataFile Ephemeris,或用 Astrogator 的
    'Cartesian Position' 資料提供者輸出 time,x,y,z。載入後 r_eci() 會改用它。"""
    global _STK_EPHEM
    data = np.genfromtxt(csv_path, delimiter=",", skip_header=1)
    scale = 1e3 if pos_unit == "km" else 1.0
    t = data[:, tcol]
    xyz = data[:, [xcol, ycol, zcol]] * scale
    _STK_EPHEM = (t, xyz)
    print(f"[STK] 已載入星曆 {csv_path}: {len(t)} 點")
    _FIELD.reset()          # 換了軌道,磁場快取必須重建

def r_eci(t):
    if _STK_EPHEM is not None:
        tt, xyz = _STK_EPHEM
        return np.array([np.interp(t, tt, xyz[:, 0]),
                         np.interp(t, tt, xyz[:, 1]),
                         np.interp(t, tt, xyz[:, 2])])
    return r_eci_circular(t)

# 建在 r_eci / T_END 都備妥之後;第一次取值時才真正建表
_FIELD = GeoField(r_eci, T_END, grid_dt=5.0, prefer_igrf=USE_IGRF)

def B_eci(r_unused, t):
    """保留原簽章(deriv 照舊呼叫),實際只取決於時間。"""
    return _FIELD(t)

# ----------------------------------------------------------------
# 3. 四元數工具(scalar-last: [x,y,z,w])
# ----------------------------------------------------------------
def quat_norm(q): return q/np.linalg.norm(q)

def quat_to_dcm(q):
    """body<-inertial DCM。q: [x,y,z,w]。"""
    x, y, z, w = q
    return np.array([
        [1-2*(y*y+z*z), 2*(x*y+z*w),   2*(x*z-y*w)],
        [2*(x*y-z*w),   1-2*(x*x+z*z), 2*(y*z+x*w)],
        [2*(x*z+y*w),   2*(y*z-x*w),   1-2*(x*x+y*y)]])

def qdot(q, w):
    """四元數運動學。w: body angular velocity (rad/s)。"""
    x, y, z, s = q
    wx, wy, wz = w
    return 0.5*np.array([
        s*wx + y*wz - z*wy,
        s*wy + z*wx - x*wz,
        s*wz + x*wy - y*wx,
       -x*wx - y*wy - z*wz])

# ----------------------------------------------------------------
# 4. B-dot 控制律
# ----------------------------------------------------------------
def bdot_control(w_body, B_body):
    """經典 B-dot(量測式):b_dot ~= -w x b_hat,m = -k b_dot,再飽和。"""
    Bnorm = np.linalg.norm(B_body)
    if Bnorm < 1e-12:
        return np.zeros(3)
    b_hat = B_body / Bnorm
    b_dot = -np.cross(w_body, b_hat)          # 單位向量的變化率(量測可得)
    m = -K_BDOT * b_dot
    m = np.clip(m, -M_MAX, M_MAX)             # 磁力矩器飽和
    return m

# ----------------------------------------------------------------
# 5. 動力學導數(狀態 = [q(4), w(3)])
# ----------------------------------------------------------------
def deriv(t, state):
    q = quat_norm(state[:4])
    w = state[4:]
    r = r_eci(t)
    B_in = B_eci(r, t)                        # inertial
    Rbi = quat_to_dcm(q)                      # body<-inertial
    B_body = Rbi @ B_in
    m = bdot_control(w, B_body)
    tau = np.cross(m, B_body)                 # 磁力矩 = m x B
    wdot = I_INV @ (tau - np.cross(w, I_BODY @ w))
    return np.concatenate([qdot(q, w), wdot]), np.linalg.norm(m), np.linalg.norm(B_body)

# ----------------------------------------------------------------
# 6. 積分(RK4;同時記錄 |w|, 磁矩, |B|)
# ----------------------------------------------------------------
def simulate():
    n = int(T_END/DT)+1
    t_arr = np.linspace(0, T_END, n)
    q = quat_norm(np.array([0.0, 0.0, 0.0, 1.0]))
    w = W0.copy()
    state = np.concatenate([q, w])

    W = np.zeros(n); M = np.zeros(n); BB = np.zeros(n)
    for i, t in enumerate(t_arr):
        W[i] = np.linalg.norm(state[4:])
        d1, mmag, bmag = deriv(t, state)
        M[i] = mmag; BB[i] = bmag
        # RK4(導數函式回傳 tuple,取第 0 項)
        k1 = d1
        k2,_,_ = deriv(t+DT/2, state+DT/2*k1)
        k3,_,_ = deriv(t+DT/2, state+DT/2*k2)
        k4,_,_ = deriv(t+DT,   state+DT*k3)
        state = state + DT/6*(k1+2*k2+2*k3+k4)
        state[:4] = quat_norm(state[:4])
    return t_arr, W, M, BB

# ----------------------------------------------------------------
# 7. 執行 + 出圖
# ----------------------------------------------------------------
def main():
    field = _FIELD.describe()
    orbit = "STK ephemeris" if _STK_EPHEM is not None else "built-in circular"
    print(f"Field model : {field}")
    print(f"Orbit source: {orbit}")
    print(f"Initial |w| : {np.degrees(np.linalg.norm(W0)):.2f} deg/s")

    t, W, M, BB = simulate()
    Wdeg = np.degrees(W)
    w0, wf = Wdeg[0], Wdeg[-1]
    # 收斂:降到初始 10% 的時間
    thresh = 0.1*w0
    idx = np.where(Wdeg <= thresh)[0]
    t_conv = t[idx[0]] if len(idx) else None

    print(f"Final   |w| : {wf:.3f} deg/s  (initial {w0:.2f})")
    print(f"Reduction   : {100*(1-wf/w0):.1f} %")
    if t_conv: print(f"Time to 10% : {t_conv/60:.1f} min ({t_conv/PERIOD:.2f} orbits)")
    else:      print("Time to 10% : not reached in sim window (raise T_END or K_BDOT)")

    fig, ax = plt.subplots(3, 1, figsize=(10, 9), sharex=True)
    tm = t/60.0

    ax[0].plot(tm, Wdeg, color="tab:blue", lw=1.8)
    if t_conv: ax[0].axvline(t_conv/60, color="red", ls="--", lw=1,
                             label=f"10% @ {t_conv/60:.0f} min")
    ax[0].axhline(thresh, color="gray", ls=":", lw=1, label="10% of initial")
    ax[0].set_ylabel("|omega|  (deg/s)")
    ax[0].set_title(f"B-dot Detumbling  |  field: {_FIELD.label()}  |  orbit: {orbit}")
    ax[0].legend(fontsize=8); ax[0].grid(alpha=0.3)

    ax[1].plot(tm, M*1e3, color="tab:green", lw=1.2)
    ax[1].axhline(M_MAX*1e3, color="red", ls=":", lw=1, label="saturation")
    ax[1].set_ylabel("magnetic moment |m| (mA m^2)")
    ax[1].legend(fontsize=8); ax[1].grid(alpha=0.3)

    ax[2].plot(tm, BB*1e6, color="tab:purple", lw=1.0)
    ax[2].set_ylabel("|B| in body (uT)")
    ax[2].set_xlabel("time (min)")
    ax[2].grid(alpha=0.3)

    fig.tight_layout()
    fig.savefig(os.path.join(HERE, "week8_bdot_detumbling.png"), dpi=130)
    print("Wrote week8_bdot_detumbling.png")

if __name__ == "__main__":
    # 要用 STK 星曆時,取消下一行註解並指向你的 CSV:
    # load_stk_ephemeris("stk_ephem_ecef.csv", pos_unit="km")
    main()

# -*- coding: utf-8 -*-
# ================================================================
# Week 8 整合版 - B-dot Detumbling(Python 算 + 出圖 + STK 3D 動畫)
#
# 整合三件事:
#   (A) 已驗證的 B-dot 物理(承 week8_bdot_detumbling.py)
#   (B) scipy solve_ivp 自適應積分(取代手刻 RK4)
#   (C) 輸出「格式正確」的 STK .a 姿態檔 + comtypes 載入程式
#       (修正原方案的 4 個 bug:補 ScenarioEpoch / CoordinateAxes、
#        移除誤用的 Sequence、建立輸出資料夾、處理四元數慣例)
#
# 執行(純算 + 出圖 + 產 .a 檔,不需 STK):
#   python week8_detumble_stk_integrated.py
# 之後把 .a 檔用 STK 端程式載入(見檔尾 load_into_stk),即可在
# STK 3D 視窗看衛星從翻滾收斂到穩定。
# ================================================================

import os
import numpy as np
from scipy.integrate import solve_ivp   # 見 integrate() 說明:B-dot 快速切換用固定步長較合適
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
# 產出要落在**週次目錄**,不是 process 的工作目錄。每一支 weekNN_verify.py
# 開頭都有這個慣用法並註明「啟動 STK Engine 會改變 process 的工作目錄」,
# 但 lab 這一側先前沒有套用 —— 結果庫根目錄散落著九個產出,
# 其中一份 week15_launch.png 還是過期的。notebook 的 kernel 沒有 __file__,
# 所以留 WEEK_DIR(notebook 開頭的 bootstrap cell 會設)當後備。
HERE = (os.path.dirname(os.path.abspath(__file__)) if "__file__" in globals()
        else globals().get("WEEK_DIR") or os.path.abspath(os.getcwd()))

# ---------------- 參數(與 Week8 基準一致)----------------
MU = 3.986004418e14; RE = 6378.137e3; B0 = 3.12e-5
I_BODY = np.diag([2.0e-3, 2.1e-3, 1.8e-3]); I_INV = np.linalg.inv(I_BODY)
K_BDOT = 5.0e4; M_MAX = 0.2
ALT = 600e3; INC = np.radians(97.8); RAAN = 0.0
SMA = RE + ALT; N_ORB = np.sqrt(MU/SMA**3); PERIOD = 2*np.pi/N_ORB
W0 = np.radians(np.array([3.0, -2.0, 2.5]))
T_END = 8*PERIOD

SCENARIO_EPOCH = "1 Jan 2024 00:00:00.000"   # 必與 STK 場景 StartTime 一致
COORD_AXES     = "J2000"                       # 必與 Python 動力學參考系一致
OUT_A = os.path.join(HERE, "cubesat_detumble.a")   # 落在週次目錄

# ---------------- 磁場 —— 見 geomag.py ----------------
# B 只吃時間且沿軌道平滑,先在 5 s 網格向量化算好再內插:
# 磁場成本從 33.7 分鐘降到約 2.8 秒,相對誤差 < 0.004%。
#   USE_IGRF=True  -> IGRF-13,8 圈消旋 80.3%
#   USE_IGRF=False -> 傾斜偶極,8 圈消旋 99.1%(舊版實際跑的就是這個)
USE_IGRF = True
from geomag import GeoField


def r_eci(t):
    th = N_ORB*t
    xp, yp = SMA*np.cos(th), SMA*np.sin(th)
    ci, si = np.cos(INC), np.sin(INC); cO, sO = np.cos(RAAN), np.sin(RAAN)
    x1, y1, z1 = xp, yp*ci, yp*si
    return np.array([x1*cO - y1*sO, x1*sO + y1*cO, z1])

# ---------------- 四元數工具(scalar-last [x,y,z,w])----------------
_FIELD = GeoField(r_eci, T_END, grid_dt=5.0, prefer_igrf=USE_IGRF)

def B_eci(r_unused, t):
    """保留原簽章(rhs 照舊呼叫),實際只取決於時間。"""
    return _FIELD(t)

def qnorm(q): return q/np.linalg.norm(q)
def q_to_dcm(q):
    x,y,z,w = q
    return np.array([[1-2*(y*y+z*z),2*(x*y+z*w),2*(x*z-y*w)],
                     [2*(x*y-z*w),1-2*(x*x+z*z),2*(y*z+x*w)],
                     [2*(x*z+y*w),2*(y*z-x*w),1-2*(x*x+y*y)]])
def qdot(q, w):
    x,y,z,s = q; wx,wy,wz = w
    return 0.5*np.array([s*wx+y*wz-z*wy, s*wy+z*wx-x*wz, s*wz+x*wy-y*wx, -x*wx-y*wy-z*wz])

# ---------------- B-dot 控制律 ----------------
def bdot(w, Bb):
    n = np.linalg.norm(Bb)
    if n < 1e-12: return np.zeros(3)
    m = -K_BDOT*(-np.cross(w, Bb/n))
    return np.clip(m, -M_MAX, M_MAX)

# ---------------- solve_ivp 右手邊(狀態 = [q(4), w(3)])----------------
def rhs(t, s):
    q = qnorm(s[:4]); w = s[4:]
    Bb = q_to_dcm(q) @ B_eci(r_eci(t), t)
    m = bdot(w, Bb)
    tau = np.cross(m, Bb)
    wd = I_INV @ (tau - np.cross(w, I_BODY @ w))
    return np.concatenate([qdot(q, w), wd])

# ---------------- 積分器 ----------------
# 教學重點:B-dot 磁力矩隨姿態快速切換方向,會逼 solve_ivp(自適應步長)
# 把步長縮到極小而幾乎停住。對這種快速切換問題,「固定步長 RK4」才是正確工具:
# 快、穩、可重現。把 USE_SOLVE_IVP 改 True 可親自體驗它變慢。
USE_SOLVE_IVP = False
DT = 1.0

def integrate(s0):
    if USE_SOLVE_IVP:
        sol = solve_ivp(rhs, [0, T_END], s0, t_eval=np.arange(0, T_END, 2.0),
                        method="LSODA", rtol=1e-5, atol=1e-7)
        return sol.t, sol.y
    n = int(T_END/DT)+1
    T = np.linspace(0, T_END, n)
    Y = np.zeros((7, n)); y = s0.copy()
    for i, t in enumerate(T):
        Y[:, i] = y
        k1 = rhs(t, y); k2 = rhs(t+DT/2, y+DT/2*k1)
        k3 = rhs(t+DT/2, y+DT/2*k2); k4 = rhs(t+DT, y+DT*k3)
        y = y + DT/6*(k1+2*k2+2*k3+k4)
        y[:4] = qnorm(y[:4])
    return T, Y

# ---------------- 主流程 ----------------
def main():
    print(f"Field: {_FIELD.describe()}")
    q0 = np.array([0,0,0,1.0])
    s0 = np.concatenate([q0, W0])
    t_full, y_full = integrate(s0)
    # 下採樣到每 4 秒一點,縮小 .a 檔與繪圖量
    step = max(1, int(round(4.0/(t_full[1]-t_full[0]))))
    class _S: pass
    sol = _S(); sol.t = t_full[::step]
    yy = y_full[:, ::step]
    Q = np.array([qnorm(yy[:4, i]) for i in range(yy.shape[1])])
    W = yy[4:, :]
    Wdeg = np.degrees(np.linalg.norm(W, axis=0))
    w0, wf = Wdeg[0], Wdeg[-1]
    thr = 0.1*w0; idx = np.where(Wdeg <= thr)[0]
    tconv = sol.t[idx[0]] if len(idx) else None

    print(f"Initial |w|: {w0:.2f} deg/s   Final |w|: {wf:.3f} deg/s   Reduction: {100*(1-wf/w0):.1f}%")
    print(f"Time to 10%: {tconv/60:.1f} min ({tconv/PERIOD:.2f} orbits)" if tconv else "10% not reached")

    # ---- 出圖 ----
    fig, ax = plt.subplots(2, 1, figsize=(10, 7), sharex=True)
    tm = sol.t/60
    ax[0].plot(tm, Wdeg, color="tab:blue", lw=1.8)
    if tconv: ax[0].axvline(tconv/60, color="red", ls="--", lw=1, label=f"10% @ {tconv/60:.0f} min")
    ax[0].axhline(thr, color="gray", ls=":", lw=1)
    ax[0].set_ylabel("|omega| (deg/s)"); ax[0].legend(fontsize=8); ax[0].grid(alpha=0.3)
    ax[0].set_title(f"B-dot Detumbling (fixed-step RK4)  |  field: {_FIELD.label()}")
    ax[1].plot(tm, Q[:,0], label="qx", lw=0.9); ax[1].plot(tm, Q[:,1], label="qy", lw=0.9)
    ax[1].plot(tm, Q[:,2], label="qz", lw=0.9); ax[1].plot(tm, Q[:,3], label="qw", lw=1.2, color="k")
    ax[1].set_ylabel("quaternion"); ax[1].set_xlabel("time (min)")
    ax[1].legend(fontsize=8, ncol=4); ax[1].grid(alpha=0.3)
    fig.tight_layout(); fig.savefig(os.path.join(HERE, "week8_detumble_integrated.png"), dpi=130)
    print("Wrote week8_detumble_integrated.png")

    # ---- 輸出格式正確的 STK .a 姿態檔 ----
    write_stk_attitude(sol.t, Q, OUT_A)
    print(f"Wrote {OUT_A}  ({len(sol.t)} points)")

def write_stk_attitude(t, Q, path, conjugate=False):
    """輸出 STK External Attitude (.a)。修正原方案 4 個 bug:
       補 ScenarioEpoch / CoordinateAxes、不用 Sequence(那是尤拉角的)、
       建資料夾、可選四元數共軛(動畫轉反時設 conjugate=True)。
       STK 四元數欄序:time qx qy qz qs(scalar-last)。"""
    d = os.path.dirname(path)
    if d: os.makedirs(d, exist_ok=True)
    with open(path, "w") as f:
        f.write("stk.v.11.0\n")
        f.write("BEGIN Attitude\n")
        f.write(f"NumberOfAttitudePoints {len(t)}\n")
        f.write("BlockingFactor 20\n")
        f.write("InterpolationOrder 1\n")
        f.write("CentralBody Earth\n")
        f.write(f"ScenarioEpoch {SCENARIO_EPOCH}\n")
        f.write(f"CoordinateAxes {COORD_AXES}\n")
        f.write("AttitudeTimeQuaternions\n")
        for i in range(len(t)):
            qx, qy, qz, qw = Q[i]
            if conjugate: qx, qy, qz = -qx, -qy, -qz
            f.write(f"{t[i]:.4f} {qx:.9f} {qy:.9f} {qz:.9f} {qw:.9f}\n")
        f.write("END Attitude\n")

# ================================================================
# STK 端載入程式(comtypes)- 在有 STK 的 Windows 機器執行
# ================================================================
def load_into_stk(a_path=None, sat_name="CubeSat_Detumble"):
    """把姿態檔載進 STK。a_path 給 None 就用本檔剛產生的那一份。

    預設值原本寫死 `C:\\Temp\\cubesat_detumble.a`,而這支腳本其實把
    檔案寫在**週次目錄**(OUT_A)—— 兩者對不上,學生照預設值跑會
    載到一個不存在的檔。
    """
    a_path = a_path or OUT_A
    import comtypes, comtypes.client
    # 關鍵:先產生型別庫,否則 comtypes.gen.STKObjects 不存在
    comtypes.client.GetModule("STKObjects.tlb")   # 或用 (GUID,1,0)
    import comtypes.gen.STKObjects as stk
    try:
        app = comtypes.client.GetActiveObject("STK12.Application")
    except Exception:
        app = comtypes.client.CreateObject("STK12.Application")
    app.Visible = True
    root = app.Personality2
    scen = root.CurrentScenario
    scen2 = scen.QueryInterface(stk.IAgScenario)
    # 讓 .a 檔的 ScenarioEpoch 與場景一致(這裡以場景 StartTime 為準)
    print("Scenario StartTime:", scen2.StartTime, "(確認與 .a 檔 ScenarioEpoch 一致)")
    if scen.Children.Contains(stk.eSatellite, sat_name):
        sat = scen.Children.Item(sat_name)
    else:
        sat = scen.Children.New(stk.eSatellite, sat_name)
    sat_o = sat.QueryInterface(stk.IAgSatellite)
    sat_o.SetAttitudeType(stk.eAttitudeExternal)
    ext = sat_o.Attitude.QueryInterface(stk.IAgScenarioAttitudeExternal)
    ext.Filename = a_path
    root.Rewind()
    print("已載入姿態檔。STK 中按 Play 觀看消旋動畫。若轉向相反,重存 .a 時設 conjugate=True。")

if __name__ == "__main__":
    main()