自動車の運転を想像してください。目をつぶっていても、体が感じる加速や回転の感覚で、しばらくは車がどちらに向かっているか大まかにわかります。しかし、時間が経つにつれてその感覚は不正確になっていきます。目を開ければ、道路の標識やランドマークを見て自分の位置と向きを正確に修正できます。実は人工衛星の姿勢推定もこれと全く同じ構造をしています。ジャイロスコープ（角速度センサ）が「体の感覚」に相当し、太陽センサやスタートラッカーが「目で見る」に相当するのです。
しかし、ジャイロには「ドリフト」と呼ばれる系統的な誤差があり、時間とともに姿勢推定値が真値からずれていきます。一方、太陽センサやスタートラッカーは離散的な（飛び飛びの）タイミングでしか測定値を提供しません。しかもエクリプス中は太陽センサが使えず、スタートラッカーの更新レートは数Hz程度にとどまります。これらの異なる特性を持つセンサの情報を時間的に最適に統合し、連続的かつ高精度な姿勢推定を実現するのがカルマンフィルタです。
ただし、姿勢推定にカルマンフィルタを適用するには特別な注意が必要です。姿勢はクォータニオンや回転行列で表されますが、これらには「単位ノルム」や「直交行列」といった拘束条件があります。標準的なカルマンフィルタを素朴に適用すると、この拘束を破壊してしまいます。この問題を解決するのが乗法拡張カルマンフィルタ（MEKF: Multiplicative Extended Kalman Filter）であり、これが宇宙工学における姿勢推定のゴールドスタンダードとなっています。
MEKFを理解すると、以下の技術が開けます。
- 衛星の姿勢制御系: ほぼ全ての実運用衛星の姿勢推定にMEKFまたはその派生型が使用されている
- ドローン・ロボットのIMU融合: IMU（ジャイロ+加速度計）と外部センサの統合は、MEKFと本質的に同じ構造
- ジャイロバイアスの自動校正: MEKFは姿勢と同時にジャイロのバイアスも推定でき、校正の手間を軽減する
- エクリプス中の姿勢維持: センサが利用できない期間もジャイロ伝搬で姿勢を維持し、センサ復帰時に精密に補正する
本記事の内容
- なぜ姿勢推定に標準カルマンフィルタが使えないのか
- 乗法誤差クォータニオンの導入とMEKFの定式化
- 状態ベクトル（姿勢誤差角 + ジャイロバイアス）の構成
- 予測ステップと更新ステップの詳細な導出
- 共分散行列の設計指針
- Pythonでのフルシミュレーション実装
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
なぜ姿勢に特殊なカルマンフィルタが必要なのか
標準カルマンフィルタの問題
カルマンフィルタの基本形は、状態ベクトル $\bm{x}$ に対して「予測 → 観測による補正」を繰り返す線形のアルゴリズムです。状態の更新は加法的に行われます。
$$ \hat{\bm{x}}^+ = \hat{\bm{x}}^- + \bm{K}\bm{y} $$
ここで $\bm{K}$ はカルマンゲイン、$\bm{y}$ は観測残差（イノベーション）です。
姿勢をクォータニオン $\bm{q} = (q_1, q_2, q_3, q_4)^T$ で表す場合、標準カルマンフィルタを素朴に適用すると深刻な問題が生じます。
問題1: 単位拘束の破壊 クォータニオンは $|\bm{q}| = 1$ でなければ回転を表現できません。しかし、$\hat{\bm{q}}^+ = \hat{\bm{q}}^- + \bm{K}\bm{y}$ の加法的更新は、一般に $|\hat{\bm{q}}^+| \neq 1$ となります。更新のたびに正規化すれば対処できますが、これは最適性を損ないます。
問題2: 共分散行列の次元の不整合 クォータニオンは4成分ですが、3次元回転の自由度は3です（$|\bm{q}| = 1$ の拘束により1つ減る）。$4 \times 4$ の共分散行列は拘束方向（クォータニオンのノルム方向）に余分な自由度を持ち、フィルタの挙動が不安定になります。
問題3: 共分散の物理的意味の曖昧さ クォータニオンの各成分 $q_1, q_2, q_3, q_4$ の分散は、直接的に角度の不確実性に対応しないため、共分散行列の設計と解釈が困難です。
これらの問題を一挙に解決するのが、「乗法」アプローチです。
乗法誤差クォータニオンの導入
MEKFの核心的なアイデアは、クォータニオンそのものではなく、「推定値からの小さなずれ」を状態変数とすることです。
真のクォータニオンを $\bm{q}$、推定値を $\hat{\bm{q}}$ とするとき、乗法的に誤差を定義します。
$$ \begin{equation} \bm{q} = \delta\bm{q} \otimes \hat{\bm{q}} \end{equation} $$
ここで $\otimes$ はクォータニオンの乗算、$\delta\bm{q}$ は誤差クォータニオンです。これは「真の姿勢 = 小さな回転誤差 $\times$ 推定姿勢」という意味です。
加法的な誤差定義 $\bm{q} = \hat{\bm{q}} + \delta\bm{q}$ と対比してください。加法的定義では $\delta\bm{q}$ が回転を表す保証がありませんが、乗法的定義では $\delta\bm{q}$ 自体がクォータニオン（回転を表す）であり、$|\delta\bm{q}| = 1$ が自動的に保持されます。
誤差が小さいとき、誤差クォータニオンは誤差角度ベクトル $\delta\bm{\theta} = (\delta\theta_1, \delta\theta_2, \delta\theta_3)^T$ で近似できます。
$$ \begin{equation} \delta\bm{q} \approx \begin{pmatrix} \frac{1}{2}\delta\bm{\theta} \\ 1 \end{pmatrix} \end{equation} $$
この近似は $|\delta\bm{\theta}| \ll 1$ のとき有効で、$\delta\bm{\theta}$ の各成分はそれぞれの軸まわりの微小回転角（ラジアン）に対応します。
これがMEKFの鍵です: カルマンフィルタの状態変数を3次元ベクトル $\delta\bm{\theta}$ とすることで、
- 拘束条件の問題が解消（$\delta\bm{\theta}$ は無拘束の3次元ベクトル）
- 共分散行列は $3 \times 3$（自由度と整合）
- 共分散の各成分が角度の不確実性に直接対応
この考え方を理解したところで、次にMEKFの完全な定式化を導出していきます。
MEKFの定式化
状態ベクトル
MEKFの状態ベクトルは、姿勢誤差に加えてジャイロバイアスを含みます。ジャイロバイアスとは、ジャイロスコープが示す角速度に含まれる系統的な定常誤差のことです。たとえば、衛星が静止しているのにジャイロが $0.01$ °/s の角速度を出力する場合、これがバイアスです。
状態ベクトルを次のように定義します。
$$ \begin{equation} \bm{x} = \begin{pmatrix} \delta\bm{\theta} \\ \bm{\beta} \end{pmatrix} \in \mathbb{R}^6 $$
ここで、
- $\delta\bm{\theta} \in \mathbb{R}^3$: 姿勢誤差角（推定姿勢からの微小回転角）
- $\bm{\beta} \in \mathbb{R}^3$: ジャイロバイアスベクトル [rad/s]
6次元の状態ベクトルにより、姿勢（3自由度）とジャイロバイアス（3自由度）を同時に推定します。ジャイロバイアスを状態変数に含めることで、フィルタが自動的にバイアスを学習し、ジャイロの出力を補正できるようになります。これは実運用上非常に重要な機能です。
ジャイロモデル
ジャイロスコープの測定モデルを定式化します。ジャイロの測定値 $\bm{\omega}_m$ は、真の角速度 $\bm{\omega}$、バイアス $\bm{\beta}$、白色ノイズ $\bm{\eta}_v$ の和です。
$$ \begin{equation} \bm{\omega}_m = \bm{\omega} + \bm{\beta} + \bm{\eta}_v $$
ここで $\bm{\eta}_v \sim \mathcal{N}(\bm{0}, \sigma_v^2 \bm{I}_3)$ はジャイロの角速度白色ノイズ（Angle Random Walk: ARW に対応）です。
バイアスは時間とともにゆっくり変動します。このドリフトをランダムウォーク（白色ノイズの積分）でモデル化します。
$$ \begin{equation} \dot{\bm{\beta}} = \bm{\eta}_u $$
ここで $\bm{\eta}_u \sim \mathcal{N}(\bm{0}, \sigma_u^2 \bm{I}_3)$ はバイアスのドリフトノイズ（Rate Random Walk に対応）です。
$\sigma_v$ と $\sigma_u$ はジャイロの仕様書から得られるパラメータです。典型的な宇宙用ジャイロの値は以下のとおりです。
| パラメータ | 典型値 | 意味 |
|---|---|---|
| $\sigma_v$ (ARW) | $0.001$ 〜 $0.1$ °/$\sqrt{\text{hr}}$ | 短期ノイズ |
| $\sigma_u$ (RRW) | $0.0001$ 〜 $0.01$ °/hr$^{3/2}$ | バイアスの安定性 |
クォータニオンの運動方程式
衛星のクォータニオン $\bm{q}$ の時間発展は、角速度 $\bm{\omega}$ を用いて次のように記述されます。
$$ \begin{equation} \dot{\bm{q}} = \frac{1}{2} \bm{\Omega}(\bm{\omega}) \bm{q} $$
ここで $\bm{\Omega}(\bm{\omega})$ は角速度から構成される $4 \times 4$ の行列です。
$$ \bm{\Omega}(\bm{\omega}) = \begin{pmatrix} -[\bm{\omega}\times] & \bm{\omega} \\ -\bm{\omega}^T & 0 \end{pmatrix} $$
$[\bm{\omega}\times]$ は角速度ベクトルの外積行列（スキュー対称行列）です。
$$ [\bm{\omega}\times] = \begin{pmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{pmatrix} $$
この運動方程式は、「角速度が既知であれば、クォータニオンを時間積分して姿勢を伝搬できる」ことを表しています。
状態変数とモデルが整ったところで、次にカルマンフィルタの予測ステップと更新ステップを導出します。
予測ステップ
クォータニオンの伝搬
予測ステップでは、ジャイロの測定値 $\bm{\omega}_m$ を用いて推定クォータニオンを次の時刻に伝搬します。推定されたバイアスを差し引いた補正角速度を使います。
$$ \hat{\bm{\omega}} = \bm{\omega}_m – \hat{\bm{\beta}} $$
離散時間のクォータニオン伝搬は次のように行います。サンプリング間隔 $\Delta t$ が十分小さい場合、1次のオイラー積分は次のとおりです。
$$ \hat{\bm{q}}_{k+1}^- = \hat{\bm{q}}_k^+ + \frac{\Delta t}{2} \bm{\Omega}(\hat{\bm{\omega}}_k) \hat{\bm{q}}_k^+ $$
その後、正規化 $\hat{\bm{q}}_{k+1}^- \leftarrow \hat{\bm{q}}_{k+1}^- / |\hat{\bm{q}}_{k+1}^-|$ を行います。
より高精度な方法として、4次のルンゲ・クッタ法も使われます。本記事では簡潔のため1次オイラー法を用いますが、$\Delta t$ が小さければ十分な精度が得られます。
バイアスの予測は、ランダムウォークモデルから単純にそのまま保持されます。
$$ \hat{\bm{\beta}}_{k+1}^- = \hat{\bm{\beta}}_k^+ $$
誤差状態の伝搬（線形化）
カルマンフィルタの共分散伝搬には、誤差状態の線形ダイナミクスが必要です。誤差角 $\delta\bm{\theta}$ とバイアス誤差 $\delta\bm{\beta} = \bm{\beta} – \hat{\bm{\beta}}$ の微小変動を線形化すると、次の連続時間の誤差状態方程式を得ます。
$$ \begin{pmatrix} \delta\dot{\bm{\theta}} \\ \delta\dot{\bm{\beta}} \end{pmatrix} = \bm{F} \begin{pmatrix} \delta\bm{\theta} \\ \delta\bm{\beta} \end{pmatrix} + \bm{G} \begin{pmatrix} \bm{\eta}_v \\ \bm{\eta}_u \end{pmatrix} $$
ここでシステム行列 $\bm{F}$ とノイズ入力行列 $\bm{G}$ は次のとおりです。
$$ \begin{equation} \bm{F} = \begin{pmatrix} -[\hat{\bm{\omega}}\times] & -\bm{I}_3 \\ \bm{0}_{3\times3} & \bm{0}_{3\times3} \end{pmatrix}, \quad \bm{G} = \begin{pmatrix} -\bm{I}_3 & \bm{0}_3 \\ \bm{0}_3 & \bm{I}_3 \end{pmatrix} $$
$\bm{F}$ の左上ブロック $-[\hat{\bm{\omega}}\times]$ は、姿勢誤差がジャイロで補正された角速度によって回転することを表しています。右上ブロック $-\bm{I}_3$ は、バイアス誤差が姿勢誤差に直接寄与することを表しています — バイアスが推定値からずれていれば、ジャイロ伝搬によって姿勢誤差が蓄積するのです。
離散時間の共分散伝搬
連続時間のシステム行列 $\bm{F}$ を離散時間に変換します。$\Delta t$ が十分小さい場合、状態遷移行列は1次近似で次のとおりです。
$$ \bm{\Phi} \approx \bm{I}_6 + \bm{F}\Delta t $$
プロセスノイズの共分散行列 $\bm{Q}_d$ は、連続時間のスペクトル密度を時間積分して得られます。
$$ \begin{equation} \bm{Q}_d = \begin{pmatrix} (\sigma_v^2 \Delta t + \frac{1}{3}\sigma_u^2 \Delta t^3)\bm{I}_3 & -\frac{1}{2}\sigma_u^2 \Delta t^2 \bm{I}_3 \\ -\frac{1}{2}\sigma_u^2 \Delta t^2 \bm{I}_3 & \sigma_u^2 \Delta t \bm{I}_3 \end{pmatrix} \end{equation} $$
この行列の対角ブロックの物理的意味を確認しましょう。左上ブロックの $\sigma_v^2 \Delta t$ の項はジャイロのARWによる姿勢不確実性の増大、$\frac{1}{3}\sigma_u^2 \Delta t^3$ の項はバイアスドリフトが姿勢に及ぼす影響（時間の3乗で増大）を表しています。右下ブロックの $\sigma_u^2 \Delta t$ はバイアスのランダムウォークによる不確実性です。非対角ブロックは姿勢誤差とバイアス誤差の相関を表しています。
予測ステップの共分散伝搬は次のとおりです。
$$ \bm{P}_{k+1}^- = \bm{\Phi}_k \bm{P}_k^+ \bm{\Phi}_k^T + \bm{Q}_d $$
予測ステップの導出が完了しました。次に、センサの観測データを用いて推定値を補正する更新ステップを見ていきましょう。
更新ステップ
観測モデル
センサ（太陽センサやスタートラッカー）が提供するのは、機体座標系での方向ベクトル $\bm{b}_i$ です。対応する慣性座標系のベクトルを $\bm{r}_i$ とすると、推定姿勢 $\hat{\bm{q}}$ を用いた予測観測は次のとおりです。
$$ \hat{\bm{b}}_i = \bm{A}(\hat{\bm{q}}) \bm{r}_i $$
観測残差（イノベーション）は、測定ベクトルと予測ベクトルの差です。
$$ \Delta\bm{b}_i = \bm{b}_i^{\text{meas}} – \hat{\bm{b}}_i $$
この観測残差を誤差状態 $\delta\bm{\theta}$ と結びつけましょう。真の姿勢行列は推定姿勢からの誤差回転として次のように書けます。
$$ \bm{A}_{\text{true}} = [\bm{I} – [\delta\bm{\theta}\times]]\bm{A}(\hat{\bm{q}}) $$
$[\delta\bm{\theta}\times]$ は微小角のスキュー対称行列です。したがって、
$$ \bm{b}_i^{\text{true}} = [\bm{I} – [\delta\bm{\theta}\times]]\bm{A}(\hat{\bm{q}})\bm{r}_i = \hat{\bm{b}}_i – [\delta\bm{\theta}\times]\hat{\bm{b}}_i $$
外積行列の性質 $[\bm{a}\times]\bm{b} = -[\bm{b}\times]\bm{a}$ を使うと、
$$ \bm{b}_i^{\text{true}} \approx \hat{\bm{b}}_i + [\hat{\bm{b}}_i\times]\delta\bm{\theta} $$
したがって、観測残差と誤差状態の関係は次のとおりです。
$$ \Delta\bm{b}_i = [\hat{\bm{b}}_i\times]\delta\bm{\theta} + \bm{v}_i $$
ここで $\bm{v}_i$ は測定ノイズです。$n$ 個のベクトル観測を積み重ねた観測方程式は次のようになります。
$$ \begin{equation} \Delta\bm{b} = \bm{H}\bm{x} + \bm{v} \end{equation} $$
観測行列 $\bm{H}$ は $3n \times 6$ の行列で、次のように構成されます。
$$ \begin{equation} \bm{H} = \begin{pmatrix} [\hat{\bm{b}}_1\times] & \bm{0}_{3\times3} \\ [\hat{\bm{b}}_2\times] & \bm{0}_{3\times3} \\ \vdots & \vdots \\ [\hat{\bm{b}}_n\times] & \bm{0}_{3\times3} \end{pmatrix} $$
右側の $\bm{0}$ ブロックは、方向ベクトルの観測がバイアスに直接依存しないことを意味しています。バイアスは予測ステップを通じて間接的に姿勢に影響を与え、更新ステップではその影響が共分散の相関を通じて反映されます。
カルマン更新
標準的なカルマン更新の手順を適用します。
カルマンゲインの計算:
$$ \bm{K} = \bm{P}^- \bm{H}^T (\bm{H}\bm{P}^-\bm{H}^T + \bm{R})^{-1} $$
ここで $\bm{R}$ は観測ノイズの共分散行列です。各ベクトル観測の精度が $\sigma_{\text{sensor}}$ [rad] であれば、$\bm{R} = \sigma_{\text{sensor}}^2 \bm{I}_{3n}$ です。
状態の更新:
$$ \Delta\hat{\bm{x}} = \bm{K}\Delta\bm{b} $$
ここで $\Delta\hat{\bm{x}} = (\delta\hat{\bm{\theta}}, \delta\hat{\bm{\beta}})^T$ です。
クォータニオンの乗法的更新:
ここがMEKFの核心です。推定された誤差角 $\delta\hat{\bm{\theta}}$ を用いて、誤差クォータニオンを構成します。
$$ \delta\hat{\bm{q}} = \frac{1}{\sqrt{1 + \frac{1}{4}|\delta\hat{\bm{\theta}}|^2}} \begin{pmatrix} \frac{1}{2}\delta\hat{\bm{\theta}} \\ 1 \end{pmatrix} $$
推定クォータニオンを乗法的に更新します。
$$ \begin{equation} \hat{\bm{q}}^+ = \delta\hat{\bm{q}} \otimes \hat{\bm{q}}^- \end{equation} $$
この乗法的更新により、更新後のクォータニオンは自動的に単位ノルムを保持します（2つの単位クォータニオンの積は単位クォータニオン）。
バイアスの更新:
バイアスは通常の加法的更新です。
$$ \hat{\bm{\beta}}^+ = \hat{\bm{\beta}}^- + \delta\hat{\bm{\beta}} $$
共分散の更新:
$$ \bm{P}^+ = (\bm{I}_6 – \bm{K}\bm{H})\bm{P}^- $$
リセット: 更新後、誤差状態 $\delta\bm{\theta}$ はゼロにリセットされます。これは、誤差が推定クォータニオンに「吸収」されたためです。
以上でMEKFの全ステップが揃いました。次に、これをPythonで完全に実装し、シミュレーションで動作を検証しましょう。
Pythonでの実装: MEKFシミュレーション
基本関数の定義
まず、クォータニオン演算の基本関数とMEKFクラスを実装します。
import numpy as np
def quat_mult(q, p):
"""クォータニオン乗算 q ⊗ p（Hamilton積）
q, p: [q1, q2, q3, q4] (q4がスカラー部)"""
q1, q2, q3, q4 = q
p1, p2, p3, p4 = p
return np.array([
q4*p1 + q1*p4 + q2*p3 - q3*p2,
q4*p2 - q1*p3 + q2*p4 + q3*p1,
q4*p3 + q1*p2 - q2*p1 + q3*p4,
q4*p4 - q1*p1 - q2*p2 - q3*p3
])
def quat_to_dcm(q):
"""クォータニオンから回転行列（DCM）"""
q1, q2, q3, q4 = q
return np.array([
[1-2*(q2**2+q3**2), 2*(q1*q2+q3*q4), 2*(q1*q3-q2*q4)],
[2*(q1*q2-q3*q4), 1-2*(q1**2+q3**2), 2*(q2*q3+q1*q4)],
[2*(q1*q3+q2*q4), 2*(q2*q3-q1*q4), 1-2*(q1**2+q2**2)]
])
def skew(v):
"""ベクトルの外積行列（スキュー対称行列）"""
return np.array([
[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]
])
def quat_normalize(q):
"""クォータニオンの正規化"""
return q / np.linalg.norm(q)
class MEKF:
"""乗法拡張カルマンフィルタ（MEKF）"""
def __init__(self, q0, beta0, P0, sigma_v, sigma_u):
"""
q0: 初期クォータニオン [q1, q2, q3, q4]
beta0: 初期バイアス推定 [rad/s]
P0: 初期共分散 (6x6)
sigma_v: ジャイロARWノイズ [rad/s/sqrt(Hz)]
sigma_u: ジャイロバイアスドリフトノイズ [rad/s^2/sqrt(Hz)]
"""
self.q = quat_normalize(q0.copy())
self.beta = beta0.copy()
self.P = P0.copy()
self.sigma_v = sigma_v
self.sigma_u = sigma_u
def predict(self, omega_meas, dt):
"""予測ステップ"""
# バイアス補正された角速度
omega_hat = omega_meas - self.beta
# クォータニオンの伝搬（1次オイラー）
omega_norm = np.linalg.norm(omega_hat)
if omega_norm > 1e-10:
# 指数写像による正確な離散化
angle = omega_norm * dt
axis = omega_hat / omega_norm
dq = np.array([
axis[0] * np.sin(angle/2),
axis[1] * np.sin(angle/2),
axis[2] * np.sin(angle/2),
np.cos(angle/2)
])
self.q = quat_mult(dq, self.q)
self.q = quat_normalize(self.q)
# 状態遷移行列 Φ
F = np.zeros((6, 6))
F[0:3, 0:3] = -skew(omega_hat)
F[0:3, 3:6] = -np.eye(3)
Phi = np.eye(6) + F * dt
# プロセスノイズ共分散
sv2 = self.sigma_v**2
su2 = self.sigma_u**2
Qd = np.zeros((6, 6))
Qd[0:3, 0:3] = (sv2 * dt + su2 * dt**3 / 3) * np.eye(3)
Qd[0:3, 3:6] = (-su2 * dt**2 / 2) * np.eye(3)
Qd[3:6, 0:3] = (-su2 * dt**2 / 2) * np.eye(3)
Qd[3:6, 3:6] = (su2 * dt) * np.eye(3)
# 共分散伝搬
self.P = Phi @ self.P @ Phi.T + Qd
def update(self, body_meas_list, ref_list, sigma_sensor):
"""更新ステップ
body_meas_list: 測定された機体座標系ベクトルのリスト
ref_list: 慣性座標系ベクトルのリスト
sigma_sensor: センサノイズ [rad]
"""
n = len(body_meas_list)
if n == 0:
return
A_hat = quat_to_dcm(self.q)
# 観測残差と観測行列を構築
dz = np.zeros(3 * n)
H = np.zeros((3 * n, 6))
for i in range(n):
b_pred = A_hat @ ref_list[i]
dz[3*i:3*i+3] = body_meas_list[i] - b_pred
H[3*i:3*i+3, 0:3] = skew(b_pred)
# H の バイアスブロック (3:6) はゼロ
# 観測ノイズ共分散
R = sigma_sensor**2 * np.eye(3 * n)
# カルマンゲイン
S = H @ self.P @ H.T + R
K = self.P @ H.T @ np.linalg.inv(S)
# 状態更新
dx = K @ dz
d_theta = dx[0:3]
d_beta = dx[3:6]
# 乗法的クォータニオン更新
dq_norm = np.sqrt(1 + 0.25 * np.dot(d_theta, d_theta))
dq = np.array([
0.5 * d_theta[0],
0.5 * d_theta[1],
0.5 * d_theta[2],
1.0
]) / dq_norm
self.q = quat_mult(dq, self.q)
self.q = quat_normalize(self.q)
# バイアス更新（加法的）
self.beta += d_beta
# 共分散更新（Joseph形式で数値安定性を確保）
IKH = np.eye(6) - K @ H
self.P = IKH @ self.P @ IKH.T + K @ R @ K.T
print("MEKFクラスの定義完了")
このコードでは、クォータニオンの伝搬に指数写像を用いた正確な離散化を採用しています。また、共分散の更新にはJoseph形式（$\bm{P}^+ = (\bm{I}-\bm{K}\bm{H})\bm{P}^-(\bm{I}-\bm{K}\bm{H})^T + \bm{K}\bm{R}\bm{K}^T$）を使用しており、数値的な正定値性が保持されます。
シミュレーション環境の構築
次に、衛星の姿勢運動、ジャイロ測定、センサ観測をシミュレートする環境を構築します。
import numpy as np
def simulate_attitude_dynamics(q0, omega_body, dt, n_steps):
"""衛星の姿勢運動をシミュレート（一定角速度）"""
q_history = np.zeros((n_steps + 1, 4))
q_history[0] = q0
q = q0.copy()
for k in range(n_steps):
omega = omega_body # 一定角速度
omega_norm = np.linalg.norm(omega)
if omega_norm > 1e-10:
angle = omega_norm * dt
axis = omega / omega_norm
dq = np.array([
axis[0] * np.sin(angle/2),
axis[1] * np.sin(angle/2),
axis[2] * np.sin(angle/2),
np.cos(angle/2)
])
q = quat_mult(dq, q)
q = quat_normalize(q)
q_history[k + 1] = q
return q_history
def simulate_gyro(omega_true, beta_true, sigma_v, dt, rng):
"""ジャイロ測定値を生成"""
noise = rng.normal(0, sigma_v / np.sqrt(dt), 3)
return omega_true + beta_true + noise
def simulate_sensor(q_true, ref_vectors, sigma_sensor, rng):
"""センサ測定（方向ベクトル）を生成"""
A_true = quat_to_dcm(q_true)
measurements = []
for r in ref_vectors:
b_true = A_true @ r
noise = rng.normal(0, sigma_sensor, 3)
b_meas = b_true + noise
b_meas /= np.linalg.norm(b_meas)
measurements.append(b_meas)
return measurements
# --- シミュレーション設定 ---
dt = 0.1 # サンプリング間隔 [s]
T_total = 600.0 # シミュレーション時間 [s] = 10分
n_steps = int(T_total / dt)
# 真の姿勢運動
omega_true = np.array([0.01, -0.005, 0.008]) # 真の角速度 [rad/s]
q0_true = np.array([0.0, 0.0, 0.0, 1.0]) # 初期クォータニオン
q_history = simulate_attitude_dynamics(q0_true, omega_true, dt, n_steps)
# 真のジャイロバイアス（時間変動あり）
beta_true_initial = np.array([0.001, -0.0005, 0.0008]) # [rad/s]
# 慣性座標系の参照ベクトル（太陽方向とスタートラッカー星方向）
ref_sun = np.array([0.5, 0.7, 0.5])
ref_sun /= np.linalg.norm(ref_sun)
ref_star1 = np.array([-0.3, 0.8, 0.5])
ref_star1 /= np.linalg.norm(ref_star1)
ref_vectors = [ref_sun, ref_star1]
# センサパラメータ
sigma_v = 1e-4 # ジャイロARW [rad/s/sqrt(Hz)]
sigma_u = 1e-6 # ジャイロバイアスドリフト [rad/s^2/sqrt(Hz)]
sigma_sensor = np.radians(0.1) # センサノイズ [rad] (0.1°)
sensor_interval = 1.0 # センサ更新間隔 [s]
sensor_steps = int(sensor_interval / dt)
print(f"シミュレーション時間: {T_total} s ({T_total/60:.1f} 分)")
print(f"サンプリング間隔: {dt} s")
print(f"総ステップ数: {n_steps}")
print(f"センサ更新間隔: {sensor_interval} s")
print(f"真の角速度: {omega_true} rad/s")
print(f"真のジャイロバイアス: {beta_true_initial} rad/s")
シミュレーション環境の設定が完了しました。衛星は3軸周りにゆっくり回転しており、ジャイロにはバイアスと白色ノイズが含まれ、太陽センサとスタートラッカーが1秒ごとに方向ベクトルを提供する設定です。
MEKFの実行と結果解析
いよいよMEKFを実行し、姿勢推定精度とジャイロバイアスの推定収束を確認します。
import numpy as np
import matplotlib.pyplot as plt
# --- MEKF初期化 ---
# 初期姿勢の誤差（5°程度の初期誤差を与える）
rng = np.random.default_rng(42)
d_theta_init = np.radians(5) * rng.standard_normal(3)
dq_init_norm = np.sqrt(1 + 0.25 * np.dot(d_theta_init, d_theta_init))
dq_init = np.array([
0.5 * d_theta_init[0],
0.5 * d_theta_init[1],
0.5 * d_theta_init[2],
1.0
]) / dq_init_norm
q0_est = quat_mult(dq_init, q0_true)
q0_est = quat_normalize(q0_est)
beta0_est = np.zeros(3) # バイアスの初期推定はゼロ
P0 = np.zeros((6, 6))
P0[0:3, 0:3] = np.radians(10)**2 * np.eye(3) # 姿勢: 10°の不確実性
P0[3:6, 3:6] = (1e-3)**2 * np.eye(3) # バイアス: 1e-3 rad/sの不確実性
mekf = MEKF(q0_est, beta0_est, P0, sigma_v, sigma_u)
# --- シミュレーションループ ---
time = np.arange(n_steps + 1) * dt
attitude_errors = np.zeros(n_steps + 1)
bias_errors = np.zeros((n_steps + 1, 3))
sigma_att = np.zeros(n_steps + 1)
sigma_bias = np.zeros(n_steps + 1)
q_est_history = np.zeros((n_steps + 1, 4))
beta_est_history = np.zeros((n_steps + 1, 3))
# 真のバイアス（ゆっくりドリフト）
beta_true = beta_true_initial.copy()
# 初期状態の記録
q_est_history[0] = mekf.q
beta_est_history[0] = mekf.beta
# 初期誤差の計算
R_err = quat_to_dcm(mekf.q) @ quat_to_dcm(q_history[0]).T
cos_ang = np.clip((np.trace(R_err) - 1) / 2, -1, 1)
attitude_errors[0] = np.degrees(np.arccos(cos_ang))
bias_errors[0] = mekf.beta - beta_true
sigma_att[0] = np.degrees(np.sqrt(np.trace(mekf.P[0:3, 0:3]) / 3))
sigma_bias[0] = np.sqrt(np.trace(mekf.P[3:6, 3:6]) / 3)
for k in range(n_steps):
# バイアスのドリフト（真値）
beta_true += rng.normal(0, sigma_u * np.sqrt(dt), 3)
# ジャイロ測定値の生成
omega_meas = simulate_gyro(omega_true, beta_true, sigma_v, dt, rng)
# 予測ステップ
mekf.predict(omega_meas, dt)
# センサ更新（一定間隔）
if (k + 1) % sensor_steps == 0:
body_meas = simulate_sensor(q_history[k + 1], ref_vectors,
sigma_sensor, rng)
mekf.update(body_meas, ref_vectors, sigma_sensor)
# 記録
q_est_history[k + 1] = mekf.q
beta_est_history[k + 1] = mekf.beta
# 姿勢誤差の計算
R_err = quat_to_dcm(mekf.q) @ quat_to_dcm(q_history[k + 1]).T
cos_ang = np.clip((np.trace(R_err) - 1) / 2, -1, 1)
attitude_errors[k + 1] = np.degrees(np.arccos(cos_ang))
# バイアス誤差
bias_errors[k + 1] = mekf.beta - beta_true
# 1σ不確実性
sigma_att[k + 1] = np.degrees(np.sqrt(np.trace(mekf.P[0:3, 0:3]) / 3))
sigma_bias[k + 1] = np.sqrt(np.trace(mekf.P[3:6, 3:6]) / 3)
# --- 可視化 ---
fig, axes = plt.subplots(3, 1, figsize=(14, 12))
# 1: 姿勢誤差の収束
axes[0].semilogy(time, attitude_errors, color='#00bcd4', linewidth=1.5,
label='Attitude error')
axes[0].semilogy(time, sigma_att, '--', color='#ff9800', linewidth=1.5,
label='$1\\sigma$ uncertainty')
axes[0].set_ylabel('Attitude error [deg]', fontsize=12)
axes[0].set_title('MEKF: Attitude Estimation Error Convergence', fontsize=13)
axes[0].legend(fontsize=11)
axes[0].grid(True, alpha=0.3, which='both')
axes[0].set_facecolor('#1a1a2e')
axes[0].set_ylim(1e-3, 20)
# 2: ジャイロバイアスの推定
colors_bias = ['#00bcd4', '#ff9800', '#e91e63']
labels_bias = ['$\\beta_x$', '$\\beta_y$', '$\\beta_z$']
for i in range(3):
axes[1].plot(time, beta_est_history[:, i] * 1e3, color=colors_bias[i],
linewidth=1.5, label=f'Est {labels_bias[i]}')
axes[1].axhline(beta_true_initial[i] * 1e3, color=colors_bias[i],
linestyle='--', alpha=0.5, linewidth=1)
axes[1].set_ylabel('Gyro bias [mrad/s]', fontsize=12)
axes[1].set_title('Gyro Bias Estimation', fontsize=13)
axes[1].legend(fontsize=10, ncol=3)
axes[1].grid(True, alpha=0.3)
axes[1].set_facecolor('#1a1a2e')
# 3: バイアス推定誤差
bias_err_norm = np.linalg.norm(bias_errors, axis=1)
axes[2].semilogy(time, bias_err_norm * 1e3, color='#00bcd4', linewidth=1.5,
label='Bias error norm')
axes[2].semilogy(time, sigma_bias * 1e3 * 3, '--', color='#ff9800',
linewidth=1.5, label='$3\\sigma$ uncertainty')
axes[2].set_xlabel('Time [s]', fontsize=12)
axes[2].set_ylabel('Bias error [mrad/s]', fontsize=12)
axes[2].set_title('Gyro Bias Estimation Error', fontsize=13)
axes[2].legend(fontsize=11)
axes[2].grid(True, alpha=0.3, which='both')
axes[2].set_facecolor('#1a1a2e')
fig.patch.set_facecolor('#0f0f23')
for ax in axes:
ax.tick_params(colors='white')
ax.xaxis.label.set_color('white')
ax.yaxis.label.set_color('white')
ax.title.set_color('white')
for spine in ax.spines.values():
spine.set_color('white')
plt.tight_layout()
plt.savefig('mekf_convergence.png', dpi=150, bbox_inches='tight',
facecolor='#0f0f23')
plt.show()
# 最終的な精度の表示
print(f"\n=== MEKF 最終性能 ===")
print(f"初期姿勢誤差: {attitude_errors[0]:.3f}°")
print(f"最終姿勢誤差: {attitude_errors[-1]:.4f}°")
print(f"最終姿勢誤差: {attitude_errors[-1]*3600:.1f} arcsec")
print(f"定常姿勢精度 (1σ): {sigma_att[-1]*3600:.1f} arcsec")
print(f"初期バイアス誤差: {np.linalg.norm(bias_errors[0])*1e3:.3f} mrad/s")
print(f"最終バイアス誤差: {np.linalg.norm(bias_errors[-1])*1e3:.4f} mrad/s")
3つのグラフから、MEKFの性能が明確に読み取れます。
第1グラフ（姿勢誤差の収束）: 初期の約5°の姿勢誤差が、数十秒のうちに急速に収束し、100秒程度で定常状態に達しています。対数スケールで見ると、初期の急激な収束（センサ更新によるクォータニオンの修正）と、その後の緩やかな改善（バイアス推定の収束に伴う精度向上）の2つのフェーズが観察されます。1$\sigma$ の不確実性（オレンジ破線）が実際の誤差をおおむね包絡しており、フィルタの共分散推定が適切であることが確認できます。
第2グラフ（ジャイロバイアスの推定）: 3軸のバイアス推定値が、初期値ゼロからじわじわと真値（破線）に収束していく様子が見られます。バイアスの収束は姿勢の収束よりも遅く、数百秒のオーダーを要しています。これはバイアスが姿勢を通じて間接的にしか観測されないためで、「可観測性」の観点から当然の結果です。
第3グラフ（バイアス推定誤差）: バイアス推定誤差のノルムが時間とともに単調に減少し、3$\sigma$ の不確実性境界内に収まっていることが確認できます。最終的なバイアス推定誤差は初期の10分の1以下に改善されています。
エクリプス期間のシミュレーション
実運用で重要な、エクリプス（太陽センサ不可）期間の影響をシミュレートします。エクリプス中はセンサ更新が停止し、ジャイロ伝搬のみで姿勢を維持する必要があります。
import numpy as np
import matplotlib.pyplot as plt
# --- エクリプス付きシミュレーション ---
T_total_ecl = 1200.0 # 20分
n_steps_ecl = int(T_total_ecl / dt)
# エクリプス期間: 300s〜600s（5分間）
eclipse_start = 300.0
eclipse_end = 600.0
# MEKF再初期化
mekf_ecl = MEKF(q0_est, beta0_est, P0.copy(), sigma_v, sigma_u)
# 姿勢運動の再シミュレーション
q_history_ecl = simulate_attitude_dynamics(q0_true, omega_true, dt, n_steps_ecl)
# 真のバイアス
beta_true_ecl = beta_true_initial.copy()
# 記録用配列
time_ecl = np.arange(n_steps_ecl + 1) * dt
att_err_ecl = np.zeros(n_steps_ecl + 1)
sigma_att_ecl = np.zeros(n_steps_ecl + 1)
in_eclipse = np.zeros(n_steps_ecl + 1, dtype=bool)
rng_ecl = np.random.default_rng(123)
# 初期状態の記録
R_err = quat_to_dcm(mekf_ecl.q) @ quat_to_dcm(q_history_ecl[0]).T
cos_ang = np.clip((np.trace(R_err) - 1) / 2, -1, 1)
att_err_ecl[0] = np.degrees(np.arccos(cos_ang))
sigma_att_ecl[0] = np.degrees(np.sqrt(np.trace(mekf_ecl.P[0:3, 0:3]) / 3))
for k in range(n_steps_ecl):
t = (k + 1) * dt
beta_true_ecl += rng_ecl.normal(0, sigma_u * np.sqrt(dt), 3)
omega_meas = simulate_gyro(omega_true, beta_true_ecl, sigma_v, dt, rng_ecl)
mekf_ecl.predict(omega_meas, dt)
# エクリプス判定
is_eclipse = eclipse_start <= t <= eclipse_end
in_eclipse[k + 1] = is_eclipse
# センサ更新（エクリプス中は停止）
if (k + 1) % sensor_steps == 0 and not is_eclipse:
body_meas = simulate_sensor(q_history_ecl[k + 1], ref_vectors,
sigma_sensor, rng_ecl)
mekf_ecl.update(body_meas, ref_vectors, sigma_sensor)
# 姿勢誤差
R_err = quat_to_dcm(mekf_ecl.q) @ quat_to_dcm(q_history_ecl[k + 1]).T
cos_ang = np.clip((np.trace(R_err) - 1) / 2, -1, 1)
att_err_ecl[k + 1] = np.degrees(np.arccos(cos_ang))
sigma_att_ecl[k + 1] = np.degrees(np.sqrt(np.trace(mekf_ecl.P[0:3, 0:3]) / 3))
# --- 可視化 ---
fig, ax = plt.subplots(figsize=(14, 6))
ax.semilogy(time_ecl, att_err_ecl, color='#00bcd4', linewidth=1.5,
label='Attitude error')
ax.semilogy(time_ecl, sigma_att_ecl, '--', color='#ff9800', linewidth=1.5,
label='$1\\sigma$ uncertainty')
# エクリプス領域を強調
ax.axvspan(eclipse_start, eclipse_end, color='#444444', alpha=0.4,
label='Eclipse (no sensor)')
ax.axvline(eclipse_start, color='white', linestyle=':', alpha=0.5)
ax.axvline(eclipse_end, color='white', linestyle=':', alpha=0.5)
# アノテーション
ax.annotate('Eclipse start\n(sensor off)',
xy=(eclipse_start, att_err_ecl[int(eclipse_start/dt)]),
xytext=(eclipse_start - 100, 1),
fontsize=10, color='white',
arrowprops=dict(arrowstyle='->', color='white'))
ax.annotate('Eclipse end\n(sensor resumes)',
xy=(eclipse_end, att_err_ecl[int(eclipse_end/dt)]),
xytext=(eclipse_end + 50, 1),
fontsize=10, color='white',
arrowprops=dict(arrowstyle='->', color='white'))
ax.set_xlabel('Time [s]', fontsize=12)
ax.set_ylabel('Attitude error [deg]', fontsize=12)
ax.set_title('MEKF with Eclipse Period: Attitude Error', fontsize=13)
ax.legend(fontsize=11, loc='upper right')
ax.grid(True, alpha=0.3, which='both')
ax.set_facecolor('#1a1a2e')
ax.set_ylim(1e-3, 20)
fig.patch.set_facecolor('#0f0f23')
ax.tick_params(colors='white')
ax.xaxis.label.set_color('white')
ax.yaxis.label.set_color('white')
ax.title.set_color('white')
for spine in ax.spines.values():
spine.set_color('white')
plt.tight_layout()
plt.savefig('mekf_eclipse.png', dpi=150, bbox_inches='tight',
facecolor='#0f0f23')
plt.show()
# 数値結果
idx_ecl_start = int(eclipse_start / dt)
idx_ecl_end = int(eclipse_end / dt)
print(f"\n=== エクリプスの影響 ===")
print(f"エクリプス突入時の姿勢誤差: {att_err_ecl[idx_ecl_start]:.4f}°")
print(f"エクリプス終了時の姿勢誤差: {att_err_ecl[idx_ecl_end]:.4f}°")
print(f"エクリプス中の誤差増大: {att_err_ecl[idx_ecl_end]/max(att_err_ecl[idx_ecl_start], 1e-6):.1f}倍")
print(f"エクリプス後の収束時間（概算）: {eclipse_end + 50:.0f}s 付近で回復")
print(f"最終姿勢誤差: {att_err_ecl[-1]:.4f}°")
このグラフはMEKFの動作を最も雄弁に物語っています。
フェーズ1（0〜300秒: 通常運用）: センサ更新が1秒ごとに行われ、姿勢誤差は急速に収束して定常精度に達します。
フェーズ2（300〜600秒: エクリプス）: 灰色で示されたエクリプス期間に入ると、センサ更新が停止します。ジャイロ伝搬のみで姿勢を維持するため、ジャイロのノイズとバイアスの推定残差により、姿勢誤差がゆっくりと増大していきます。同時に、1$\sigma$ の不確実性（オレンジ破線）も増大し、フィルタが「自分の推定値の信頼度が下がっている」ことを正しく認識していることがわかります。
フェーズ3（600秒〜: エクリプス後）: センサ更新が再開されると、蓄積した誤差が急速に補正されます。不確実性も速やかに減少し、数十秒で通常運用時の精度に復帰します。これが「カルマンフィルタの予測と更新のサイクル」の真骨頂です — 観測がない期間はモデルベースの予測で凌ぎ、観測が得られたらその情報を最適に取り込んで推定を修正するのです。
共分散行列の設計指針
MEKFの性能を左右する重要な設計パラメータは、プロセスノイズとセンサノイズの設定です。ここでは実用的な設計指針をまとめます。
プロセスノイズ $\bm{Q}$ の設計
プロセスノイズは $\sigma_v$（ARW）と $\sigma_u$（バイアスドリフト）の2つのパラメータで決まります。
ジャイロの仕様書からの設定: 理想的には、ジャイロの仕様書に記載されたAllan分散（またはAllan偏差）のプロットから $\sigma_v$ と $\sigma_u$ を読み取ります。Allan偏差プロットにおいて、$\tau^{-1/2}$ の傾きを持つ領域がARW（$\sigma_v$）に、$\tau^{1/2}$ の傾きを持つ領域がRRW（$\sigma_u$）に対応します。
チューニングの経験則: 仕様書が不正確な場合や、モデルの不確実性を吸収する必要がある場合は、仕様書の値に安全係数（1.5〜3倍）を掛けることが一般的です。$\sigma_v$ を大きくするとフィルタはセンサ観測をより信頼し、ジャイロの寄与を下げます。$\sigma_u$ を大きくするとバイアスの変動を許容し、バイアス推定がより追従的になります。
観測ノイズ $\bm{R}$ の設計
センサごとに異なるノイズ特性を反映します。
| センサ | 典型的な精度 $\sigma_{\text{sensor}}$ |
|---|---|
| 粗太陽センサ | $2°$ 〜 $5°$ |
| 精太陽センサ | $0.01°$ 〜 $0.1°$ |
| 地磁気センサ | $1°$ 〜 $3°$ |
| スタートラッカー | $0.001°$ 〜 $0.01°$ |
精度が大きく異なるセンサを併用する場合、$\bm{R}$ の対角成分にそれぞれのノイズ分散を設定することで、カルマンゲインが自動的に高精度センサの情報をより重視するように調整されます。
初期共分散 $\bm{P}_0$ の設計
$\bm{P}_0$ は初期の不確実性を反映します。姿勢の初期不確実性は、事前情報がない場合 $\sigma_{\theta,0} \sim 10°$ 〜 $30°$ 程度に設定します。バイアスの初期不確実性は、仕様書の最大バイアス値（例: $1$ °/h $\approx 5 \times 10^{-6}$ rad/s）に基づいて設定します。
$\bm{P}_0$ を大きく設定しすぎると初期の収束に時間がかかり、小さく設定しすぎるとフィルタがセンサ観測を無視する可能性があります。不確実な場合は大きめに設定する方が安全です。
まとめ
本記事では、乗法拡張カルマンフィルタ（MEKF）による衛星の姿勢推定を基礎理論から実装まで体系的に解説しました。
- なぜ特殊なカルマンフィルタが必要か: クォータニオンの単位拘束と自由度の不整合により、標準カルマンフィルタが直接適用できない。乗法的誤差表現が解決策
- 状態ベクトル: 姿勢誤差角（3次元）+ ジャイロバイアス（3次元）の6次元。バイアスの同時推定により、ジャイロの自動校正を実現
- 予測ステップ: ジャイロ出力からバイアスを差し引いた角速度でクォータニオンを伝搬。共分散はARWとRRWの2つのノイズパラメータで駆動
- 更新ステップ: 方向ベクトルの観測残差から誤差角を推定し、クォータニオンを乗法的に更新。自動的に単位拘束が保持される
- エクリプスへの頑健性: センサが利用不可の期間もジャイロ伝搬で姿勢を維持し、センサ復帰後に速やかに精度を回復
MEKFは、衛星の姿勢推定における事実上の標準手法であり、その基本構造はドローンや自律ロボットのIMU融合にも広く応用されています。本記事で扱ったのはセンサ統合の「推定」部分ですが、この推定値を用いて衛星の姿勢を制御する方法（PD制御、リアクションホイール駆動など）については、姿勢制御の後続記事で解説します。
次のステップとして、以下の記事も参考にしてください。