あなたが暗い部屋に目隠しで放り込まれたとします。目隠しを外した瞬間、窓の外に北極星が見え、手元のコンパスが北を指しています。この2つの方向情報から、あなたは自分がどちらを向いて立っているかを正確に知ることができます。人工衛星の姿勢決定もこれと本質的に同じです — 太陽センサが示す太陽の方向、スタートラッカーが示す恒星の方向、地磁気センサが示す地球磁場の方向。これらの「既知の方向」を手がかりに、衛星が宇宙空間でどのような向きをとっているかを計算するのが姿勢決定アルゴリズムです。
しかし、実際のセンサ測定には必ずノイズが含まれます。2つのセンサが示す方向から姿勢行列を計算する方法は一つではなく、ノイズがあるときにどの計算方法が最も「良い」推定を与えるかは自明ではありません。この問題を厳密に定式化し、最適な姿勢推定を求める枠組みを与えたのがWahba問題です。1965年にGrace Wahbaが提起したこの問題は、半世紀以上にわたって宇宙工学の中心的課題であり続けています。
姿勢決定アルゴリズムを理解すると、以下の技術が開けます。
- 衛星姿勢制御系の設計: センサ選定から姿勢推定精度の予測まで、制御系設計の基盤となる
- ロボティクスの姿勢推定: IMUやカメラからの方向観測による姿勢推定は、ドローンや自律ロボットにも応用できる
- カルマンフィルタとの統合: 姿勢決定アルゴリズムの出力は、カルマンフィルタの「観測更新」として使われる
- コンピュータビジョン: PnP問題やポイントクラウド位置合わせなど、回転推定問題に広く応用される
本記事の内容
- Wahba問題の定式化と幾何学的意味
- TRIAD法の導出（2ベクトルから直交行列を構成する代数的手法）
- QUEST法の導出（最適クォータニオンを固有値問題として求める手法）
- 両手法の精度比較とトレードオフ
- Pythonでの実装とモンテカルロ誤差解析
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
Wahba問題: 最適姿勢推定の定式化
問題設定
衛星に搭載されたセンサが、$n$ 個の方向ベクトルを測定したとします。各ベクトルについて、2つの表現があります。
- $\bm{b}_i$: 機体座標系（body frame）で測定されたベクトル（センサの出力）
- $\bm{r}_i$: 慣性座標系（reference frame）で既知のベクトル（暦やカタログから計算）
理想的には、姿勢回転行列 $\bm{A}$ によって $\bm{b}_i = \bm{A}\bm{r}_i$ が全ての $i$ について成り立つはずです。しかし、測定ノイズのため、この関係は厳密には成り立ちません。
Wahbaは1965年に、次の損失関数を最小化する回転行列 $\bm{A}$ を求める問題を提起しました。
$$ \begin{equation} L(\bm{A}) = \frac{1}{2} \sum_{i=1}^{n} w_i \|\bm{b}_i – \bm{A}\bm{r}_i\|^2 \end{equation} $$
ここで $w_i > 0$ は各ベクトル観測の重みであり、センサの信頼度（精度の逆数の2乗）に基づいて設定されます。$\bm{A}$ は直交行列（$\bm{A}^T\bm{A} = \bm{I}$、$\det(\bm{A}) = 1$）である必要があります。
損失関数の幾何学的意味
損失関数(1)の物理的な意味を理解しましょう。$\|\bm{b}_i – \bm{A}\bm{r}_i\|^2$ は、センサが測定した方向 $\bm{b}_i$ と、姿勢 $\bm{A}$ で回転させた参照方向 $\bm{A}\bm{r}_i$ の間のユークリッド距離の2乗です。損失関数全体は、全てのベクトル観測について、「測定方向と予測方向のずれ」の重み付き2乗和を表しています。
これを最小化することは、全てのセンサの測定値を最もよく説明する回転行列を見つけることに他なりません。
損失関数の変形
損失関数を展開して、最適化に便利な形に変換します。ノルムの2乗を展開します。
$$ \|\bm{b}_i – \bm{A}\bm{r}_i\|^2 = \bm{b}_i^T\bm{b}_i – 2\bm{b}_i^T\bm{A}\bm{r}_i + \bm{r}_i^T\bm{A}^T\bm{A}\bm{r}_i $$
$\bm{A}$ が直交行列なので $\bm{A}^T\bm{A} = \bm{I}$ です。また $\bm{b}_i$ と $\bm{r}_i$ は単位ベクトルなので $\bm{b}_i^T\bm{b}_i = 1$、$\bm{r}_i^T\bm{r}_i = 1$ です。したがって、
$$ \|\bm{b}_i – \bm{A}\bm{r}_i\|^2 = 2 – 2\bm{b}_i^T\bm{A}\bm{r}_i $$
これを損失関数に代入すると、$\bm{A}$ に依存しない定数項を除くと、最小化は次のゲイン関数の最大化と等価になります。
$$ \begin{equation} g(\bm{A}) = \sum_{i=1}^{n} w_i \bm{b}_i^T \bm{A} \bm{r}_i \end{equation} $$
行列のトレースの性質 $\bm{b}_i^T \bm{A} \bm{r}_i = \text{tr}(\bm{A} \bm{r}_i \bm{b}_i^T)$ を使うと、さらに次のように書けます。
$$ g(\bm{A}) = \text{tr}(\bm{A} \bm{B}^T) $$
ここで $\bm{B}$ は次のように定義される $3 \times 3$ の行列です。
$$ \begin{equation} \bm{B} = \sum_{i=1}^{n} w_i \bm{b}_i \bm{r}_i^T \end{equation} $$
この行列 $\bm{B}$ は「姿勢プロファイル行列」（attitude profile matrix）と呼ばれ、Wahba問題の全ての解法で中心的な役割を果たします。
Wahba問題の定式化が整ったところで、まずは最も単純な2ベクトルからの姿勢決定法であるTRIAD法から見ていきましょう。
TRIAD法: 2ベクトルからの直交行列構成
基本アイデア
TRIAD法は1964年にHarold Blackによって提案された最も直感的な姿勢決定法です。そのアイデアは非常に明快です — 2つの非平行ベクトルから正規直交基底を構成し、機体座標系と慣性座標系のそれぞれで基底を作り、両者を結ぶ回転行列を求めます。
日常の例で言えば、東の方角と上の方向がわかれば、北がどちらかも自動的に決まり、自分の向きが完全にわかるのと同じです。2つの独立な方向から、3つの直交方向を作り出すわけです。
アルゴリズムの導出
2つの方向ベクトル対 $(\bm{b}_1, \bm{r}_1)$ と $(\bm{b}_2, \bm{r}_2)$ が与えられたとします。$\bm{b}_1, \bm{b}_2$ は機体座標系のベクトル、$\bm{r}_1, \bm{r}_2$ は対応する慣性座標系のベクトルです。
まず、機体座標系で正規直交基底 $\{\hat{\bm{t}}_1^b, \hat{\bm{t}}_2^b, \hat{\bm{t}}_3^b\}$ を構成します。
ステップ1: 第1基底ベクトルは $\bm{b}_1$ をそのまま使います。
$$ \hat{\bm{t}}_1^b = \bm{b}_1 $$
ステップ2: 第2基底ベクトルは $\bm{b}_1$ と $\bm{b}_2$ の外積方向にとります。外積は2つのベクトルに直交する方向を与えます。
$$ \hat{\bm{t}}_2^b = \frac{\bm{b}_1 \times \bm{b}_2}{|\bm{b}_1 \times \bm{b}_2|} $$
ステップ3: 第3基底ベクトルは第1と第2の外積で完成します。
$$ \hat{\bm{t}}_3^b = \hat{\bm{t}}_1^b \times \hat{\bm{t}}_2^b $$
全く同様に、慣性座標系でも正規直交基底 $\{\hat{\bm{t}}_1^r, \hat{\bm{t}}_2^r, \hat{\bm{t}}_3^r\}$ を構成します。
$$ \hat{\bm{t}}_1^r = \bm{r}_1, \quad \hat{\bm{t}}_2^r = \frac{\bm{r}_1 \times \bm{r}_2}{|\bm{r}_1 \times \bm{r}_2|}, \quad \hat{\bm{t}}_3^r = \hat{\bm{t}}_1^r \times \hat{\bm{t}}_2^r $$
2つの正規直交基底が得られたので、回転行列 $\bm{A}$ は次のように構成できます。$\bm{A}$ は慣性座標系の基底を機体座標系の基底に変換する行列なので、
$$ \begin{equation} \bm{A}_{\text{TRIAD}} = \begin{pmatrix} \hat{\bm{t}}_1^b & \hat{\bm{t}}_2^b & \hat{\bm{t}}_3^b \end{pmatrix} \begin{pmatrix} \hat{\bm{t}}_1^r & \hat{\bm{t}}_2^r & \hat{\bm{t}}_3^r \end{pmatrix}^T \end{equation} $$
各基底ベクトルを列ベクトルとして並べた行列の積として、回転行列が直接得られます。
TRIAD法の特性
TRIAD法には以下の特徴があります。
利点: – 計算が非常にシンプル（外積と行列の積のみ） – 特殊関数やイテレーションが不要 – 計算コストが $O(1)$（ベクトル数に依存しない）
制約と注意点: – 2つのベクトルしか使用しないため、3つ以上のベクトルが利用可能な場合でも追加情報を活用できない – 第1ベクトル $\bm{b}_1$ を特別扱いする: 第1ベクトルの方向は完全に保存されるが、第2ベクトルの情報は外積を通じてのみ使われるため、$\bm{b}_2$ の測定ノイズが不均等に反映される – 2つのベクトルが平行に近いと ($\bm{b}_1 \times \bm{b}_2 \approx \bm{0}$)、外積の正規化で大きな誤差が生じる
第1ベクトルに精度が高いセンサ（例: スタートラッカー）の出力を、第2ベクトルに精度が低いセンサ（例: 太陽センサや地磁気センサ）の出力を割り当てるのが一般的です。
TRIAD法は2つのベクトルしか活用できないという制約がありました。スタートラッカーのように多数のベクトル観測が利用可能な場合、全てのデータを最適に活用する方法が求められます。これがQUEST法の動機です。
QUEST法: 最適クォータニオン推定
Davenportのq法
QUEST法を理解するための前段階として、まずDavenportのq法を紹介します。Davenport（1968年）は、Wahba問題のゲイン関数(2)をクォータニオンで表現し、固有値問題に帰着させました。
回転行列 $\bm{A}$ をクォータニオン $\bm{q} = (q_1, q_2, q_3, q_4)^T$（$q_4$ がスカラー部）で表現すると、ゲイン関数は次のように2次形式で書けます。
$$ \begin{equation} g(\bm{q}) = \bm{q}^T \bm{K} \bm{q} \end{equation} $$
ここで $\bm{K}$ は $4 \times 4$ の対称行列であり、姿勢プロファイル行列 $\bm{B}$ から構成されます。
$$ \begin{equation} \bm{K} = \begin{pmatrix} \bm{S} – \sigma\bm{I}_3 & \bm{z} \\ \bm{z}^T & \sigma \end{pmatrix} \end{equation} $$
各要素は以下のとおりです。
$\sigma = \text{tr}(\bm{B})$ は $\bm{B}$ のトレース（対角成分の和）です。
$\bm{S} = \bm{B} + \bm{B}^T$ は $\bm{B}$ の対称部分です。
$\bm{z}$ は $\bm{B}$ の反対称部分から抽出されるベクトルで、次のように定義されます。
$$ \bm{z} = \begin{pmatrix} B_{23} – B_{32} \\ B_{31} – B_{13} \\ B_{12} – B_{21} \end{pmatrix} = \sum_{i=1}^{n} w_i (\bm{b}_i \times \bm{r}_i) $$
この $\bm{z}$ ベクトルは、各ベクトル対の外積の重み付き和です。物理的には、機体座標系のベクトルと慣性座標系のベクトルのずれの回転軸方向を表しています。
ゲイン関数(5)を単位クォータニオンの拘束 $\bm{q}^T\bm{q} = 1$ のもとで最大化する問題は、$\bm{K}$ の最大固有値に対応する固有ベクトルを求める問題になります。これはラグランジュの未定乗数法から直ちに導かれます。
$$ \bm{K}\bm{q}_{\text{opt}} = \lambda_{\max} \bm{q}_{\text{opt}} $$
最大固有値 $\lambda_{\max}$ に対応する単位固有ベクトル $\bm{q}_{\text{opt}}$ が、最適な姿勢クォータニオンです。
QUEST法の導出
$4 \times 4$ 行列の固有値問題を直接解くことは可能ですが、搭載コンピュータの限られた計算資源では効率的とは言えません。Shuster（1981年）は、固有値問題を効率的に解く手法としてQUEST（QUaternion ESTimator）法を提案しました。
QUEST法のアイデアは、最大固有値 $\lambda_{\max}$ の近似値がわかれば、固有ベクトルを直接計算できるという点にあります。
固有値方程式を展開します。
$$ \begin{pmatrix} \bm{S} – \sigma\bm{I} & \bm{z} \\ \bm{z}^T & \sigma \end{pmatrix} \begin{pmatrix} \bm{p} \\ q_4 \end{pmatrix} = \lambda \begin{pmatrix} \bm{p} \\ q_4 \end{pmatrix} $$
ここで $\bm{p} = (q_1, q_2, q_3)^T$ はクォータニオンのベクトル部です。上段のブロックから次の式を得ます。
$$ (\bm{S} – \sigma\bm{I})\bm{p} + q_4\bm{z} = \lambda\bm{p} $$
$\bm{p}$ について解くと、
$$ \bm{p} = q_4 [(\lambda + \sigma)\bm{I} – \bm{S}]^{-1}\bm{z} $$
下段のブロックから次の関係を得ます。
$$ \bm{z}^T\bm{p} + \sigma q_4 = \lambda q_4 $$
$\bm{p}$ を代入すると、$\lambda$ に関する方程式（特性方程式）が得られます。
$$ \begin{equation} \bm{z}^T [(\lambda + \sigma)\bm{I} – \bm{S}]^{-1} \bm{z} = \lambda – \sigma \end{equation} $$
QUEST法は、この方程式をニュートン法で反復的に解きます。初期値としては $\lambda_0 = \sum_{i} w_i$（全重みの和）を使います。これは最大固有値の良い初期近似であり、多くの場合1〜2回の反復で十分な精度に収束します。
$\lambda$ が求まれば、最適クォータニオンは次のように計算されます。
$$ \bm{p} = [(\lambda + \sigma)\bm{I} – \bm{S}]^{-1}\bm{z} $$
$$ \begin{equation} \bm{q}_{\text{opt}} = \frac{1}{\sqrt{|\bm{p}|^2 + 1}} \begin{pmatrix} \bm{p} \\ 1 \end{pmatrix} \end{equation} $$
（ここでは $q_4 = 1$ と仮置きしてから正規化しています。）
QUEST法の特性
QUEST法は以下の優れた特性を持ちます。
最適性: Wahba問題の厳密な最適解を与える。全てのベクトル観測を重み付きで統合するため、利用可能な情報を最大限に活用する。
効率性: $4 \times 4$ の固有値問題を $3 \times 3$ の逆行列計算と1次元の反復に帰着させるため、計算量が少ない。
頑健性: 多数のベクトル観測を統合することで、個々のセンサノイズの影響が軽減される。
任意のベクトル数に対応: 2つ以上の任意の数のベクトル観測を扱える。
一方、注意点として、$(\lambda + \sigma)\bm{I} – \bm{S}$ が特異（行列式がゼロ）になる場合があります。これは回転角が $180°$ に近い場合に起こりえますが、実用上はまれです。
ここまででTRIAD法とQUEST法の理論を詳しく見てきました。次に、両手法をPythonで実装し、モンテカルロシミュレーションで精度を比較します。
Pythonでの実装
TRIAD法の実装
まず、TRIAD法を実装します。2つのベクトル対から回転行列を求める関数です。
import numpy as np
def triad(b1, b2, r1, r2):
"""
TRIAD法による姿勢決定
b1, b2: 機体座標系のベクトル（単位ベクトル）
r1, r2: 慣性座標系のベクトル（単位ベクトル）
戻り値: 回転行列 A (body = A @ ref)
"""
# 機体座標系の正規直交基底
t1_b = b1 / np.linalg.norm(b1)
t2_b = np.cross(b1, b2)
t2_b = t2_b / np.linalg.norm(t2_b)
t3_b = np.cross(t1_b, t2_b)
# 慣性座標系の正規直交基底
t1_r = r1 / np.linalg.norm(r1)
t2_r = np.cross(r1, r2)
t2_r = t2_r / np.linalg.norm(t2_r)
t3_r = np.cross(t1_r, t2_r)
# 回転行列: M_body = A @ M_ref
M_b = np.column_stack([t1_b, t2_b, t3_b])
M_r = np.column_stack([t1_r, t2_r, t3_r])
A = M_b @ M_r.T
return A
# テスト
np.random.seed(42)
# 真の姿勢（オイラー角から回転行列を生成）
def euler_to_dcm(roll, pitch, yaw):
"""オイラー角 [rad] から回転行列を生成"""
cr, sr = np.cos(roll), np.sin(roll)
cp, sp = np.cos(pitch), np.sin(pitch)
cy, sy = np.cos(yaw), np.sin(yaw)
R = np.array([
[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
[sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
[-sp, cp*sr, cp*cr]
])
return R
A_true = euler_to_dcm(np.radians(15), np.radians(-30), np.radians(45))
# 慣性座標系のベクトル（太陽方向と磁場方向）
r1 = np.array([0.5, 0.7, 0.5])
r1 /= np.linalg.norm(r1) # 太陽方向
r2 = np.array([0.2, -0.3, 0.9])
r2 /= np.linalg.norm(r2) # 磁場方向
# 機体座標系のベクトル（ノイズなし）
b1_true = A_true @ r1
b2_true = A_true @ r2
# ノイズなしの場合
A_triad = triad(b1_true, b2_true, r1, r2)
err = np.linalg.norm(A_triad - A_true, 'fro')
print(f"TRIAD法（ノイズなし）のフロベニウスノルム誤差: {err:.2e}")
# ノイズありの場合
noise_sigma = 0.01 # rad（約0.57°）
b1_noisy = b1_true + np.random.normal(0, noise_sigma, 3)
b1_noisy /= np.linalg.norm(b1_noisy)
b2_noisy = b2_true + np.random.normal(0, noise_sigma, 3)
b2_noisy /= np.linalg.norm(b2_noisy)
A_triad_noisy = triad(b1_noisy, b2_noisy, r1, r2)
# 姿勢誤差角の計算
def attitude_error_angle(A_true, A_est):
"""回転行列間の誤差角度 [deg]"""
R_err = A_est @ A_true.T
cos_angle = np.clip((np.trace(R_err) - 1) / 2, -1, 1)
return np.degrees(np.arccos(cos_angle))
err_deg = attitude_error_angle(A_true, A_triad_noisy)
print(f"TRIAD法（ノイズσ={np.degrees(noise_sigma):.2f}°）の姿勢誤差: {err_deg:.4f}°")
ノイズがない場合の誤差が計算機イプシロン程度であることが確認でき、TRIAD法のアルゴリズムが正しく実装されていることがわかります。ノイズを加えた場合は、測定ノイズの大きさに応じた姿勢誤差が生じています。
QUEST法の実装
次に、QUEST法を実装します。特性方程式のニュートン法による求解と、最適クォータニオンの計算を行います。
import numpy as np
def quest(body_vectors, ref_vectors, weights=None, max_iter=50, tol=1e-12):
"""
QUEST法による姿勢決定
body_vectors: 機体座標系のベクトル (n x 3)
ref_vectors: 慣性座標系のベクトル (n x 3)
weights: 各観測の重み (n,)。Noneの場合は等重み
戻り値: クォータニオン [q1, q2, q3, q4] (q4がスカラー部)、回転行列 A
"""
n = len(body_vectors)
if weights is None:
weights = np.ones(n)
# 姿勢プロファイル行列 B を計算
B = np.zeros((3, 3))
for i in range(n):
B += weights[i] * np.outer(body_vectors[i], ref_vectors[i])
# K行列の要素を計算
sigma = np.trace(B)
S = B + B.T
z = np.array([
B[1, 2] - B[2, 1],
B[2, 0] - B[0, 2],
B[0, 1] - B[1, 0]
])
# 特性方程式をニュートン法で解く
# 初期値: lambda_0 = sum(weights)
lam = np.sum(weights)
for iteration in range(max_iter):
# M = (lambda + sigma) * I - S
M = (lam + sigma) * np.eye(3) - S
det_M = np.linalg.det(M)
if abs(det_M) < 1e-15:
# 特異の場合はSVD法にフォールバック
U, s, Vt = np.linalg.svd(B)
d = np.linalg.det(U) * np.linalg.det(Vt.T)
D = np.diag([1, 1, d])
A = U @ D @ Vt
q = dcm_to_quaternion(A)
return q, A
M_inv = np.linalg.inv(M)
# 特性方程式: z^T M^{-1} z = lambda - sigma
f = z @ M_inv @ z - (lam - sigma)
# ニュートン法の導関数
M_inv_z = M_inv @ z
df = -z @ M_inv @ M_inv @ z - 1
# ニュートン更新
delta = f / df
lam -= delta
if abs(delta) < tol:
break
# 最適クォータニオンを計算
M = (lam + sigma) * np.eye(3) - S
M_inv = np.linalg.inv(M)
p = M_inv @ z # ベクトル部（q4=1と仮置き）
# 正規化
norm = np.sqrt(np.dot(p, p) + 1.0)
q = np.array([p[0], p[1], p[2], 1.0]) / norm
# クォータニオンから回転行列に変換
A = quaternion_to_dcm(q)
return q, A
def quaternion_to_dcm(q):
"""クォータニオン [q1,q2,q3,q4] から回転行列への変換"""
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 dcm_to_quaternion(A):
"""回転行列からクォータニオンへの変換（Shepperd法）"""
tr = np.trace(A)
if tr > 0:
s = 2 * np.sqrt(tr + 1)
q4 = s / 4
q1 = (A[1, 2] - A[2, 1]) / s
q2 = (A[2, 0] - A[0, 2]) / s
q3 = (A[0, 1] - A[1, 0]) / s
else:
diag = np.diag(A)
idx = np.argmax(diag)
if idx == 0:
s = 2 * np.sqrt(1 + A[0,0] - A[1,1] - A[2,2])
q1 = s / 4
q2 = (A[0,1] + A[1,0]) / s
q3 = (A[0,2] + A[2,0]) / s
q4 = (A[1,2] - A[2,1]) / s
elif idx == 1:
s = 2 * np.sqrt(1 - A[0,0] + A[1,1] - A[2,2])
q1 = (A[0,1] + A[1,0]) / s
q2 = s / 4
q3 = (A[1,2] + A[2,1]) / s
q4 = (A[2,0] - A[0,2]) / s
else:
s = 2 * np.sqrt(1 - A[0,0] - A[1,1] + A[2,2])
q1 = (A[0,2] + A[2,0]) / s
q2 = (A[1,2] + A[2,1]) / s
q3 = s / 4
q4 = (A[0,1] - A[1,0]) / s
q = np.array([q1, q2, q3, q4])
return q / np.linalg.norm(q)
# テスト
body_vecs = np.array([b1_noisy, b2_noisy])
ref_vecs = np.array([r1, r2])
q_quest, A_quest = quest(body_vecs, ref_vecs)
err_quest = attitude_error_angle(A_true, A_quest)
print(f"QUEST法（2ベクトル、ノイズσ={np.degrees(noise_sigma):.2f}°）の姿勢誤差: {err_quest:.4f}°")
print(f"TRIAD法との比較: TRIAD={err_deg:.4f}°, QUEST={err_quest:.4f}°")
2ベクトルの場合、TRIAD法とQUEST法はほぼ同等の精度を示しますが、QUEST法は重みの最適化により若干の優位性を持つ場合があります。真価は3つ以上のベクトルが利用可能な場合に発揮されます。
モンテカルロ精度比較
TRIAD法とQUEST法の精度をモンテカルロシミュレーションで体系的に比較します。
import numpy as np
import matplotlib.pyplot as plt
def random_rotation(rng):
"""ランダムな回転行列を生成"""
q = rng.normal(0, 1, 4)
q /= np.linalg.norm(q)
return quaternion_to_dcm(q)
def add_noise_to_vector(v, sigma_rad, rng):
"""単位ベクトルにノイズを追加"""
noise = rng.normal(0, sigma_rad, 3)
v_noisy = v + noise
return v_noisy / np.linalg.norm(v_noisy)
# --- モンテカルロシミュレーション ---
n_trials = 1000
noise_sigma_deg = 0.5 # 測定ノイズ [deg]
noise_sigma_rad = np.radians(noise_sigma_deg)
# 慣性座標系のベクトル群（太陽、磁場、恒星3つ）
ref_vectors_all = np.array([
[0.5, 0.7, 0.5], # 太陽方向
[0.2, -0.3, 0.9], # 磁場方向
[-0.8, 0.1, 0.6], # 恒星1
[0.3, 0.9, -0.3], # 恒星2
[-0.1, 0.4, 0.9], # 恒星3
])
for i in range(len(ref_vectors_all)):
ref_vectors_all[i] /= np.linalg.norm(ref_vectors_all[i])
n_vec_cases = [2, 3, 5] # テストするベクトル数
rng = np.random.default_rng(42)
results_triad = {n: [] for n in n_vec_cases}
results_quest = {n: [] for n in n_vec_cases}
for trial in range(n_trials):
A_true = random_rotation(rng)
for n_vec in n_vec_cases:
ref_vecs = ref_vectors_all[:n_vec]
body_vecs_true = (A_true @ ref_vecs.T).T
# ノイズを追加
body_vecs_noisy = np.array([
add_noise_to_vector(b, noise_sigma_rad, rng)
for b in body_vecs_true
])
# TRIAD法（最初の2ベクトルのみ使用）
A_triad = triad(body_vecs_noisy[0], body_vecs_noisy[1],
ref_vecs[0], ref_vecs[1])
err_triad = attitude_error_angle(A_true, A_triad)
results_triad[n_vec].append(err_triad)
# QUEST法（全ベクトル使用）
try:
_, A_quest = quest(body_vecs_noisy, ref_vecs)
err_quest = attitude_error_angle(A_true, A_quest)
results_quest[n_vec].append(err_quest)
except Exception:
results_quest[n_vec].append(np.nan)
# --- 可視化 ---
fig, axes = plt.subplots(1, 3, figsize=(18, 5))
for idx, n_vec in enumerate(n_vec_cases):
ax = axes[idx]
errs_triad = np.array(results_triad[n_vec]) * 3600 # arcsec
errs_quest = np.array(results_quest[n_vec]) * 3600
errs_quest = errs_quest[~np.isnan(errs_quest)]
bins = np.linspace(0, max(np.percentile(errs_triad, 99),
np.percentile(errs_quest, 99)), 40)
ax.hist(errs_triad, bins=bins, alpha=0.6, color='#ff9800',
label=f'TRIAD (mean: {np.mean(errs_triad):.0f}")')
ax.hist(errs_quest, bins=bins, alpha=0.6, color='#00bcd4',
label=f'QUEST (mean: {np.mean(errs_quest):.0f}")')
ax.set_xlabel('Attitude error [arcsec]', fontsize=12)
ax.set_ylabel('Count', fontsize=12)
ax.set_title(f'{n_vec} vectors ($\\sigma$ = {noise_sigma_deg}°)', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
ax.set_facecolor('#1a1a2e')
fig.suptitle('TRIAD vs QUEST: Monte Carlo Comparison',
fontsize=15, color='white', y=1.02)
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('triad_vs_quest.png', dpi=150, bbox_inches='tight',
facecolor='#0f0f23')
plt.show()
# 数値結果のサマリー
print("\n=== 精度比較サマリー ===")
print(f"{'ベクトル数':>10} {'TRIAD平均 [arcsec]':>20} {'QUEST平均 [arcsec]':>20} {'改善率':>10}")
for n_vec in n_vec_cases:
mean_t = np.mean(results_triad[n_vec]) * 3600
mean_q = np.nanmean(results_quest[n_vec]) * 3600
improvement = (1 - mean_q / mean_t) * 100
print(f"{n_vec:>10} {mean_t:>20.1f} {mean_q:>20.1f} {improvement:>9.1f}%")
3つのヒストグラムから、TRIAD法とQUEST法の精度の違いが明確に見て取れます。
2ベクトルの場合: 両手法の精度はほぼ同等です。これは情報量が同じ（2つのベクトル）であるため当然の結果です。
3ベクトルの場合: QUEST法が明確に優位になります。QUEST法は3つ目のベクトルの情報を損失関数の最適化に取り込めるのに対し、TRIAD法は依然として最初の2つのベクトルしか使用しないためです。
5ベクトルの場合: QUEST法の優位性がさらに顕著になります。$\sqrt{n}$ のスケーリングに近い精度向上が見られ、QUEST法が利用可能な全情報を効率的に統合していることが確認できます。
ベクトル間角度と精度の関係
姿勢決定精度は、観測ベクトル間の角度（幾何学的配置）にも依存します。これを可視化しましょう。
import numpy as np
import matplotlib.pyplot as plt
# --- ベクトル間角度と姿勢誤差の関係 ---
angles_between = np.linspace(5, 175, 35) # 2ベクトル間の角度 [deg]
n_trials = 500
noise_sigma_rad = np.radians(0.5)
rng = np.random.default_rng(42)
mean_errors_triad = []
mean_errors_quest = []
std_errors_triad = []
std_errors_quest = []
for angle_deg in angles_between:
angle_rad = np.radians(angle_deg)
# 固定の参照ベクトル対（指定角度で配置）
r1 = np.array([0, 0, 1])
r2 = np.array([np.sin(angle_rad), 0, np.cos(angle_rad)])
errs_t = []
errs_q = []
for trial in range(n_trials):
A_true = random_rotation(rng)
b1_true = A_true @ r1
b2_true = A_true @ r2
b1_noisy = add_noise_to_vector(b1_true, noise_sigma_rad, rng)
b2_noisy = add_noise_to_vector(b2_true, noise_sigma_rad, rng)
# TRIAD
A_t = triad(b1_noisy, b2_noisy, r1, r2)
errs_t.append(attitude_error_angle(A_true, A_t))
# QUEST
try:
_, A_q = quest(np.array([b1_noisy, b2_noisy]),
np.array([r1, r2]))
errs_q.append(attitude_error_angle(A_true, A_q))
except Exception:
errs_q.append(np.nan)
mean_errors_triad.append(np.mean(errs_t) * 3600)
mean_errors_quest.append(np.nanmean(errs_q) * 3600)
std_errors_triad.append(np.std(errs_t) * 3600)
std_errors_quest.append(np.nanstd(errs_q) * 3600)
# --- 可視化 ---
fig, ax = plt.subplots(figsize=(10, 6))
ax.fill_between(angles_between,
np.array(mean_errors_triad) - np.array(std_errors_triad),
np.array(mean_errors_triad) + np.array(std_errors_triad),
color='#ff9800', alpha=0.2)
ax.fill_between(angles_between,
np.array(mean_errors_quest) - np.array(std_errors_quest),
np.array(mean_errors_quest) + np.array(std_errors_quest),
color='#00bcd4', alpha=0.2)
ax.plot(angles_between, mean_errors_triad, 'o-', color='#ff9800',
linewidth=2, markersize=4, label='TRIAD')
ax.plot(angles_between, mean_errors_quest, 's-', color='#00bcd4',
linewidth=2, markersize=4, label='QUEST')
ax.set_xlabel('Angle between two vectors [deg]', fontsize=12)
ax.set_ylabel('Mean attitude error [arcsec]', fontsize=12)
ax.set_title('Attitude Error vs Vector Separation Angle\n'
f'($\\sigma$ = {np.degrees(noise_sigma_rad):.1f}°, 2 vectors)',
fontsize=13)
ax.legend(fontsize=12)
ax.grid(True, alpha=0.3)
ax.set_facecolor('#1a1a2e')
ax.set_xlim(0, 180)
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('angle_vs_error.png', dpi=150, bbox_inches='tight',
facecolor='#0f0f23')
plt.show()
print(f"\n最小誤差の角度（TRIAD）: {angles_between[np.argmin(mean_errors_triad)]:.0f}°")
print(f"最小誤差の角度（QUEST）: {angles_between[np.argmin(mean_errors_quest)]:.0f}°")
このグラフから、2つの重要な知見が得られます。
第一に、ベクトル間角度が90°付近で精度が最良になります。 これは直感的にも理解できます。2つのベクトルが直交に近いほど、3次元の姿勢に対する情報が均等になり、推定精度が向上します。逆に、2つのベクトルが平行に近い（角度が0°や180°に近い）場合、外積のノルムが小さくなり、直交基底の構成が不安定になるため、精度が著しく劣化します。
第二に、平行に近い領域（$< 20°$ や $> 160°$）で誤差が急増しています。 これはTRIAD法の外積 $\bm{b}_1 \times \bm{b}_2$ の正規化における数値不安定性に起因します。実際の衛星設計では、太陽方向と磁場方向の角度が小さくなる軌道位置では、別のセンサの組み合わせに切り替えるなどの対策が行われます。
TRIAD法とQUEST法の比較
ここまでの解析を踏まえて、両手法の特性を表にまとめます。
| 特性 | TRIAD | QUEST |
|---|---|---|
| 使用ベクトル数 | 2（固定） | 任意（2以上） |
| 最適性 | 最適ではない（第1ベクトルを特別扱い） | Wahba問題の最適解 |
| 計算量 | $O(1)$（外積と行列積） | $O(n)$ + 反復（通常2〜3回） |
| 頑健性 | ベクトルが平行に近いと不安定 | 左同（ただし多数ベクトルで軽減） |
| 出力形式 | 回転行列 | クォータニオン（→回転行列） |
| 実装の簡易さ | 非常にシンプル | やや複雑（ニュートン法） |
| 搭載計算機向け | 極めて軽量 | 軽量（3×3逆行列＋反復） |
| 多数ベクトルの活用 | 不可 | 可能（$\sqrt{n}$ の精度向上） |
実際の衛星では、両手法を状況に応じて使い分けることが一般的です。安全モードなど計算資源が限られる状況ではTRIAD法を、通常運用でスタートラッカーの多数の星ベクトルが利用可能な場合はQUEST法を使用します。
まとめ
本記事では、衛星の姿勢決定アルゴリズムの2つの代表的手法をWahba問題の枠組みから体系的に解説しました。
- Wahba問題: 複数のベクトル観測から最適な回転行列を推定する問題として定式化される。損失関数は姿勢プロファイル行列 $\bm{B}$ のトレース最大化に帰着する
- TRIAD法: 2つのベクトルから外積を用いて正規直交基底を構成し、回転行列を直接計算する。実装が非常にシンプルだが、2ベクトルしか活用できない
- QUEST法: $\bm{K}$ 行列の最大固有値問題をニュートン法で効率的に解き、最適クォータニオンを得る。任意の数のベクトル観測を最適に統合可能
- 幾何学的配置の影響: 観測ベクトル間の角度が90°に近いほど精度が良く、平行に近いと精度が劣化する
これらの姿勢決定アルゴリズムは、各時刻のスナップショット（瞬時の観測データ）から姿勢を推定するものです。しかし実際の衛星では、時間方向の連続性（前の時刻の推定値）やジャイロスコープのデータも活用したい場合がほとんどです。次の記事では、カルマンフィルタを用いてこれらの情報を時間的に統合し、リアルタイムの高精度姿勢推定を実現する手法を解説します。
次のステップとして、以下の記事も参考にしてください。