🛰️ SpaceLesson

測距・追跡#36

カルマンフィルタによる軌道決定 — 逐次的に更新する最小分散推定

前回のバッチ最小二乗は観測データを蓄積してから一括処理する方式だった。今回は新しい観測が届くたびに推定値をその場で更新していくカルマンフィルタを、状態空間モデルと最小分散推定の立場から数式で導出し、なぜそれが最適推定量なのか、非線形軌道力学へのEKF拡張、JPLのODP/MONTEでの実務的位置づけまでを扱う。

前提知識: radiometric-navigation

カルマンフィルタ軌道決定状態空間モデル逐次推定拡張カルマンフィルタ

この回で学ぶこと

前回、測距(レンジング)とドップラー計測から得られる観測データを使って探査機の軌道(位置・速度)を推定する最小二乗法による軌道決定を学びました。あの方式には1つの大きな特徴がありました。ある時間窓のあいだに集めた観測データ {z1,z2,…,zN}\{z_1, z_2, \dots, z_N\} を全部ため込んでから、それを一括で処理して1つの推定値を出す、いわゆるバッチ処理だったということです。新しい観測が1個増えるたびに、また最初から N+1N+1 個ぶんの正規方程式を解き直す必要がありました。

しかし現実の運用では、観測データは連続的に、次々と地上局に届き続けます。ランデブー中の有人宇宙船や、着陸直前の探査機のように「今まさにどこにいるか」をリアルタイムで知りたい局面では、観測が来るたびにバッチ処理をゼロからやり直すのは非効率であるだけでなく、間に合いません。欲しいのは、「今の推定値」と「新しく届いた1個の観測」だけから、計算量一定で「次の推定値」を更新できる仕組みです。

これを実現するのが カルマンフィルタ (Kalman Filter) です。1960年にRudolf E. Kalmanが発表したこのアルゴリズムは、アポロ計画の月着陸船誘導コンピュータへの実装を皮切りに、以後60年以上にわたって航法・誘導・制御のあらゆる場面で使われ続けている、工学史上もっとも影響力の大きいアルゴリズムの1つです。この回では、カルマンフィルタを軌道決定の文脈で、状態空間モデルの定式化から、予測ステップと更新ステップの導出、そしてなぜそれが(ある条件下で)最適な推定量なのかまでを、数式を追いながら理解します。

直感的な全体像

カルマンフィルタの気持ちを、数式に入る前に言葉で掴んでおきましょう。基本の動きは、次の2ステップの繰り返しです。

  1. 予測 (Predict): 前の時刻の推定値を、軌道力学の運動方程式に従って「今の時刻まで動かす」。まだ新しい観測は見ていない、力学モデルだけに基づく予測値です。
  2. 更新 (Update): その予測値に対して新しい観測値(測距やドップラーの値)を照らし合わせ、両者の食い違いに応じて予測値を補正する。

ポイントは、予測にも観測にも、それぞれ「どれくらい信用できるか」という不確かさが伴っているということです。力学モデルには大気抵抗や太陽輻射圧のようなモデル化しきれない摂動力があり、観測にも測距装置や熱雑音に起因する誤差があります。カルマンフィルタは、この2つの不確かさを確率分布(共分散行列)として明示的に持ち歩き、「予測をどれだけ信じるか」と「観測をどれだけ信じるか」の重み付けを、不確かさの大きさに応じて自動的に決めます。

直感的には、これは「2つの独立な測定値を、それぞれの分散の逆数で重み付けして合成する」という、統計学ではおなじみの操作の一般化です。分散が小さい(=信頼できる)情報ほど強く効き、分散が大きい(=あやふやな)情報は弱くしか効かない。カルマンフィルタは、この重み付け平均を、時間発展する多次元の状態ベクトルに対して、逐次的かつ最適に行う仕組みだと考えると見通しが良くなります。この「重み」に相当する量が、後で導出するカルマンゲイン KkK_k です。

状態空間モデルの定式化

まず、推定したい対象を状態空間モデル (state-space model) として定式化します。時刻 tkt_k における探査機の状態(たとえば位置3成分と速度3成分をまとめた6次元ベクトル)を xk∈Rn\mathbf{x}_k \in \mathbb{R}^n とします。

状態遷移方程式

状態は、軌道力学の運動方程式に従って時間発展します。これを離散時刻 tk−1→tkt_{k-1} \to t_k の線形な遷移として書くと、

xk=Fk−1 xk−1+wk−1,wk−1∼N(0,Qk−1)\mathbf{x}_k = F_{k-1}\, \mathbf{x}_{k-1} + \mathbf{w}_{k-1}, \qquad \mathbf{w}_{k-1} \sim \mathcal{N}(\mathbf{0}, Q_{k-1})

ここで Fk−1F_{k-1} は状態遷移行列で、実際の軌道決定では二体問題や多体問題の運動方程式(万有引力・摂動力を含む)を数値積分することで得られる状態遷移行列 Φ(tk,tk−1)\Phi(t_k, t_{k-1}) そのもの、あるいはその近似です。wk−1\mathbf{w}_{k-1} はプロセスノイズと呼ばれ、大気抵抗のゆらぎ、太陽輻射圧のモデル化誤差、姿勢制御スラスタの不確かさなど、力学モデルに含めきれていない加速度成分を確率的に表現したもので、共分散行列 Qk−1Q_{k-1} を持つ平均ゼロのガウス性白色雑音として扱います。

短い時間間隔 T=tk−tk−1T = t_k - t_{k-1} のあいだ加速度がほぼ一定とみなせる単純なケースでは、位置 rr と速度 r˙\dot r からなる状態について、

F=[1T01]F = \begin{bmatrix} 1 & T \\ 0 & 1 \end{bmatrix}

という等速直線運動モデルがしばしば近似として使われます(実務ではもちろんケプラー運動やJ2摂動まで含めたより精密な FF が使われますが、フィルタの構造そのものはこの単純化でも変わりません)。

観測方程式

一方、地上局が実際に測るのは状態ベクトルそのものではなく、レンジング(測距)やドップラー(速度)から得られる観測量 zk\mathbf{z}_k です。これを状態ベクトルの線形写像として、

zk=Hk xk+vk,vk∼N(0,Rk)\mathbf{z}_k = H_k\, \mathbf{x}_k + \mathbf{v}_k, \qquad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, R_k)

と表します。HkH_k は観測行列で、状態ベクトルのどの成分がどのように観測量に寄与するかを表します。たとえば距離(レンジ)だけを観測する場合、状態が [r, r˙]⊤[r,\ \dot r]^\top なら

H=[10]H = \begin{bmatrix} 1 & 0 \end{bmatrix}

となり、ドップラー(視線方向の速度、レンジレート)も同時に観測する場合は

H=[1001]H = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}

のように行が追加されます。vk\mathbf{v}_k は観測ノイズで、測距装置の熱雑音や量子化誤差などに由来し、共分散行列 RkR_k を持ちます。前回学んだ最小二乗法では、この HkH_k に相当する行列(観測とパラメータを結ぶ偏微分行列)を、ある時間窓ぶん積み上げて一括で正規方程式を解いていました。カルマンフィルタでは、これを1時刻ぶんずつ、逐次的に処理します。

予測ステップ

時刻 tk−1t_{k-1} までの観測を使った状態推定値を x^k−1\hat{\mathbf{x}}_{k-1}、その誤差共分散を Pk−1P_{k-1} とします。まだ時刻 tkt_k の観測 zk\mathbf{z}_k を見ていない段階での予測値を、上付きマイナス記号 (−)(-) で表すと、状態遷移方程式の期待値・共分散をそのまま伝播させることで、

x^k−=Fk−1 x^k−1\hat{\mathbf{x}}_k^{-} = F_{k-1}\, \hat{\mathbf{x}}_{k-1} Pk−=Fk−1 Pk−1 Fk−1⊤+Qk−1P_k^{-} = F_{k-1}\, P_{k-1}\, F_{k-1}^\top + Q_{k-1}

が得られます。共分散の式は、線形変換 FF による誤差の伝播 FPk−1F⊤F P_{k-1} F^\top に、新たに加わった不確かさ Qk−1Q_{k-1} を単純に加算した形になっていることに注目してください。運動方程式が正しくても、モデル化しきれない外乱があるぶん、時間が進むほど不確かさは必ず増大します。これは直感とも一致します。次にいつ観測が来るか分からない期間が長くなるほど、「今どこにいるか」の確信度は下がっていくはずです。

更新ステップ — 最小分散推定によるカルマンゲインの導出

時刻 tkt_k で新しい観測 zk\mathbf{z}_k が届いたら、予測値 x^k−\hat{\mathbf{x}}_k^{-} をこの観測で補正します。更新後の推定値を、次の線形な形で仮定します。

x^k=x^k−+Kk(zk−Hkx^k−)\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^{-} + K_k\big(\mathbf{z}_k - H_k \hat{\mathbf{x}}_k^{-}\big)

括弧の中身 zk−Hkx^k−\mathbf{z}_k - H_k \hat{\mathbf{x}}_k^{-} は**イノベーション(残差)**と呼ばれ、「実際に観測された値」と「予測値から予想される観測値」の食い違いを表します。カルマンフィルタの核心は、この食い違いをどれだけの重み KkK_k(カルマンゲイン)で補正に反映させるかを、統計的に最適な形で決めることにあります。

推定誤差を ek≡xk−x^k\mathbf{e}_k \equiv \mathbf{x}_k - \hat{\mathbf{x}}_k、更新前の誤差を ek−≡xk−x^k−\mathbf{e}_k^{-} \equiv \mathbf{x}_k - \hat{\mathbf{x}}_k^{-}(共分散 Pk−P_k^{-})とします。観測方程式 zk=Hkxk+vk\mathbf{z}_k = H_k \mathbf{x}_k + \mathbf{v}_k を更新式に代入すると、

x^k=x^k−+KkHk(xk−x^k−)+Kkvk\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^{-} + K_k H_k(\mathbf{x}_k - \hat{\mathbf{x}}_k^{-}) + K_k \mathbf{v}_k

なので、誤差は

ek=xk−x^k=(I−KkHk) ek−−Kkvk\mathbf{e}_k = \mathbf{x}_k - \hat{\mathbf{x}}_k = (I - K_k H_k)\, \mathbf{e}_k^{-} - K_k \mathbf{v}_k

と書けます。vk\mathbf{v}_k は ek−\mathbf{e}_k^{-}(観測ノイズが乗る前の予測誤差)と統計的に独立なので、更新後の誤差共分散 Pk=E[ekek⊤]P_k = E[\mathbf{e}_k \mathbf{e}_k^\top] は、

Pk=(I−KkHk) Pk− (I−KkHk)⊤+KkRkKk⊤P_k = (I - K_k H_k)\, P_k^{-}\, (I - K_k H_k)^\top + K_k R_k K_k^\top

という形になります。ここで「KkK_k をどう選べば、x^k\hat{\mathbf{x}}_k の誤差(の分散)が最小になるか」という最小分散推定 (minimum variance estimation) の問題を解きます。PkP_k のトレース(対角成分の和、つまり各状態成分の分散の合計)を KkK_k について最小化する条件は、

∂∂Kk tr(Pk)=−2(I−KkHk)Pk−Hk⊤+2KkRk=0\frac{\partial}{\partial K_k}\,\text{tr}(P_k) = -2(I - K_k H_k) P_k^{-} H_k^\top + 2 K_k R_k = \mathbf{0}

これを KkK_k について解くと、

Kk(HkPk−Hk⊤+Rk)=Pk−Hk⊤K_k \big(H_k P_k^{-} H_k^\top + R_k\big) = P_k^{-} H_k^\top Kk=Pk−Hk⊤(HkPk−Hk⊤+Rk)−1\boxed{K_k = P_k^{-} H_k^\top \big(H_k P_k^{-} H_k^\top + R_k\big)^{-1}}

というカルマンゲインの式が得られます。分母の HkPk−Hk⊤+RkH_k P_k^{-} H_k^\top + R_k は「予測誤差が観測空間に投影されたときの不確かさ」と「観測そのものの不確かさ」の和で、これをイノベーション共分散 SkS_k と呼びます。

Sk≡HkPk−Hk⊤+Rk,Kk=Pk−Hk⊤Sk−1S_k \equiv H_k P_k^{-} H_k^\top + R_k, \qquad K_k = P_k^{-} H_k^\top S_k^{-1}

このカルマンゲインを使うと、更新後の共分散は(上の一般式に代入して整理すると)簡潔な形にまとまります。

Pk=(I−KkHk) Pk−P_k = (I - K_k H_k)\, P_k^{-}

まとめると、予測ステップと更新ステップを合わせたカルマンフィルタの1サイクルは次の5本の式で完結します。

x^k−=Fk−1x^k−1,Pk−=Fk−1Pk−1Fk−1⊤+Qk−1\hat{\mathbf{x}}_k^{-} = F_{k-1}\hat{\mathbf{x}}_{k-1}, \qquad P_k^{-} = F_{k-1}P_{k-1}F_{k-1}^\top + Q_{k-1} Kk=Pk−Hk⊤(HkPk−Hk⊤+Rk)−1K_k = P_k^{-}H_k^\top\big(H_k P_k^{-}H_k^\top + R_k\big)^{-1} x^k=x^k−+Kk(zk−Hkx^k−),Pk=(I−KkHk)Pk−\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^{-} + K_k\big(\mathbf{z}_k - H_k\hat{\mathbf{x}}_k^{-}\big), \qquad P_k = (I-K_k H_k)P_k^{-}

これがカルマンフィルタの全アルゴリズムです。観測が1個届くたびにこの5本の式を評価するだけでよく、過去の観測データを保持し直す必要も、行列サイズが時間とともに大きくなることもありません。計算量が観測数 NN に依存せず一定であることが、バッチ最小二乗との決定的な違いです。

カルマンゲインの直感 — 1次元の場合

行列の式だけだと直感が掴みにくいので、状態もスカラー、観測もスカラーの1次元の場合を見ておきましょう。予測分散を p−p^{-}、観測ノイズ分散を rr、観測行列を h=1h=1 とすると、カルマンゲインは

k=p−p−+rk = \frac{p^{-}}{p^{-} + r}

というきわめて簡単な形になり、更新式は

x^=x^−+p−p−+r(z−x^−)=rp−+rx^−+p−p−+rz\hat{x} = \hat{x}^{-} + \frac{p^{-}}{p^{-}+r}(z - \hat{x}^{-}) = \frac{r}{p^{-}+r}\hat{x}^{-} + \frac{p^{-}}{p^{-}+r} z

これはまさに「予測値 x^−\hat{x}^{-} と観測値 zz を、それぞれの不確かさの逆数(1/r1/r, 1/p−1/p^{-})に比例する重みで合成した加重平均」そのものです。観測ノイズ rr が小さい(信頼できる観測)ほど k→1k \to 1 に近づいて観測をほぼそのまま採用し、逆に rr が大きい(あやふやな観測)ほど k→0k \to 0 に近づいて予測値をほぼそのまま維持します。多次元・行列の場合のカルマンゲインの式も、本質的にはこの重み付け平均の考え方を、状態ベクトルの各成分間の相関まで含めて一般化したものに他なりません。

なぜカルマンフィルタは最適推定量なのか

ここまでは「誤差分散を最小化する」という最小分散推定の立場からカルマンゲインを導出しました。この結果は、実はベイズ推定の枠組みから見ても、まったく同じ答えにたどり着くことが知られています。

状態 xk\mathbf{x}_k の事前分布(予測分布)を、予測ステップの結果を使ってガウス分布

p(xk∣z1:k−1)=N(x^k−, Pk−)p(\mathbf{x}_k \mid \mathbf{z}_{1:k-1}) = \mathcal{N}\big(\hat{\mathbf{x}}_k^{-},\ P_k^{-}\big)

とし、観測モデルから尤度を

p(zk∣xk)=N(Hkxk, Rk)p(\mathbf{z}_k \mid \mathbf{x}_k) = \mathcal{N}\big(H_k\mathbf{x}_k,\ R_k\big)

とすると、ベイズの定理 p(xk∣z1:k)∝p(zk∣xk) p(xk∣z1:k−1)p(\mathbf{x}_k\mid\mathbf{z}_{1:k}) \propto p(\mathbf{z}_k\mid\mathbf{x}_k)\, p(\mathbf{x}_k\mid\mathbf{z}_{1:k-1}) により、事後分布はガウス分布どうしの積になります。ガウス分布の積は(正規化すれば)再びガウス分布になるという性質(共役性)があり、実際にこの積を計算すると、事後分布の平均・共分散はちょうど

p(xk∣z1:k)=N(x^k, Pk)p(\mathbf{x}_k \mid \mathbf{z}_{1:k}) = \mathcal{N}\big(\hat{\mathbf{x}}_k,\ P_k\big)

すなわち、先ほど最小分散推定から導出したのとまったく同じ x^k\hat{\mathbf{x}}_k, PkP_k に一致します。つまりカルマンフィルタは、状態遷移・観測モデルがともに線形で、雑音がすべてガウス性であるという条件のもとでは、事後分布の厳密なベイズ更新そのものを実行していることになります。この場合、カルマンフィルタの出力する x^k\hat{\mathbf{x}}_k は、条件付き期待値 E[xk∣z1:k]E[\mathbf{x}_k \mid \mathbf{z}_{1:k}] に一致し、これは平均二乗誤差を最小化する意味での最良推定量(MMSE推定量)であり、同時に事後分布のピークを取るMAP推定量とも一致します。

さらに重要なのは、雑音がガウス性でない場合でも、カルマンフィルタは線形推定量の中で最良のもの(BLUE, Best Linear Unbiased Estimator) であり続けるという事実です(ガウス・マルコフの定理)。ガウス性の仮定は「厳密にベイズ最適」であることを保証するために必要ですが、「線形推定量として最適」であることは、雑音の1次・2次モーメント(平均ゼロ・共分散が既知)さえ分かっていれば成り立ちます。この頑健性の高さが、カルマンフィルタが理論的にも実務的にも信頼されている大きな理由です。

非線形な軌道力学への拡張: 拡張カルマンフィルタ (EKF)

ここまでの議論は、状態遷移も観測もどちらも線形であることを前提にしていました。しかし実際の軌道力学は、逆二乗則の重力や非線形な摂動項を含むため、真の状態遷移関数 xk=f(xk−1)\mathbf{x}_k = f(\mathbf{x}_{k-1}) や観測関数 zk=h(xk)\mathbf{z}_k = h(\mathbf{x}_k) は一般に非線形です(たとえばレンジ観測は探査機と地上局の位置ベクトルの差のノルムであり、線形ではありません)。

拡張カルマンフィルタ (Extended Kalman Filter, EKF) は、この非線形性を、現在の推定点のまわりでテイラー展開1次まで(線形化)して扱うことで対処します。具体的には、非線形関数 ff, hh のヤコビ行列

Fk≡∂f∂x∣x^k−1,Hk≡∂h∂x∣x^k−F_k \equiv \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k-1}}, \qquad H_k \equiv \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_k^{-}}

を、推定値が更新されるたびに現在の推定点で評価し直し、それを線形カルマンフィルタの FkF_k, HkH_k の代わりに使います。状態そのものの伝播には線形近似 FkxF_k \mathbf{x} ではなく非線形関数 f(x)f(\mathbf{x}) をそのまま数値積分で使い、共分散の伝播にだけヤコビ行列による線形化を使う、という非対称な扱いをするのがEKFの要点です。

x^k−=f(x^k−1),Pk−=FkPk−1Fk⊤+Qk−1\hat{\mathbf{x}}_k^{-} = f(\hat{\mathbf{x}}_{k-1}), \qquad P_k^{-} = F_k P_{k-1} F_k^\top + Q_{k-1}

この「毎ステップ、現在の推定点で線形化し直す」というアイデアによって、真に非線形な軌道力学モデルにもカルマンフィルタの枠組みを適用できるようになります。ただし線形化は近似であるため、推定誤差が大きい(=線形化点から真の状態が大きく離れている)場合や、観測関数の曲率が強い場合には、EKFの推定は理論的な最適性を失い、発散することさえあります。この弱点を軽減する改良として、無香料カルマンフィルタ(UKF)や、複数の線形化点候補を試す反復EKFなどが実務では使われますが、それらの詳細は本シリーズの範囲を超えるため、ここでは名前を挙げるにとどめます。

実務での使われ方

軌道決定の実務では、バッチ最小二乗法とカルマンフィルタ(逐次フィルタ)は対立する技術というより、目的に応じて使い分けられる補完的な道具として位置づけられています。

JPLの軌道決定ソフトウェアには、長年使われてきた ODP (Orbit Determination Program) と、その後継として2010年代以降に主力となった MONTE (Mission-analysis, Operations, and Navigation Toolkit Environment) があります。これらのソフトウェアは、ミッション終了後や長期の解析で使う高精度な確定軌道(definitive orbit)の算出には前回学んだバッチ最小二乗(あるいはその一般化である情報フィルタ・平方根情報フィルタ)を用いる一方、日々の運用における迅速な軌道更新や、リアルタイムに近い速報値の算出には逐次フィルタ(カルマンフィルタ系のアルゴリズム)を組み込んで使い分けています。深宇宙ネットワーク(DSN)から届く測距・ドップラーデータは、パス(1回の可視時間帯)ごとに逐次処理され、フィルタが刻々と軌道推定値と誤差共分散を更新していきます。

カルマンフィルタが決定的に重要になるのは、リアルタイム性が要求される航法の場面です。

  • 有人ミッションのランデブー・ドッキング: アポロ計画の月着陸船・司令船のランデブーでは、NASAエイムズ研究センターのStanley F. Schmidtらが、Kalmanの発表からわずか数年でこのアルゴリズムを実際の誘導コンピュータに実装しました(「シュミット・カルマンフィルタ」として知られる、非線形性に対処する初期のEKF的手法もここから生まれています)。これはカルマンフィルタの宇宙工学への最初期の実用化として知られています。国際宇宙ステーション(ISS)へのドッキングを行う現代の補給船・有人宇宙船でも、レーダーやLIDAR、カメラによる相対航法センサの出力をカルマンフィルタ(多くはEKF)で融合し、相対位置・相対速度をリアルタイムに推定し続けています。
  • 惑星着陸・降下誘導: 火星探査機の大気突入・降下・着陸(EDL)フェーズのように、地上との通信遅延のために地上からのリアルタイム介入がまったくできない局面では、探査機は搭載コンピュータ上でIMU(慣性計測装置)やレーダー高度計、地形照合カメラなどのセンサ出力をカルマンフィルタで統合し、自律的に自己の位置・速度を推定し続けます。
  • GNSS(GPS等)による測位: 地球周回衛星や、深宇宙探査機とは異なりますが、身近な例としてGPS受信機の内部でも、複数衛星からの擬似距離・ドップラー観測をカルマンフィルタで統合して位置・速度・時刻誤差を逐次推定しています。

一方で、精密科学(重力場推定、相対性理論の検証実験など)のための最終的な確定軌道の算出には、今なお前回学んだバッチ最小二乗(とその発展形)が標準的に使われ続けています。これは、バッチ処理のほうが「ある観測区間全体を見渡した上で、過去に遡って整合的な最良推定を出す」という平滑化(スムージング)的な性質を持ちやすく、後戻りのできない逐次フィルタよりも高精度な確定解を出しやすいという事情によります。カルマンフィルタとバッチ最小二乗は、いわば「今すぐ知りたい」か「後で正確に知りたい」かという、時間軸に対する要求の違いによって使い分けられる、車の両輪のような関係にあると言えるでしょう。

演習問題

  1. 状態が位置と速度 x=[r, r˙]⊤\mathbf{x}=[r,\ \dot r]^\top、時間間隔 T=10T=10 秒の等速直線運動モデル F=[1T01]F=\begin{bmatrix}1 & T\\ 0 & 1\end{bmatrix}、プロセスノイズを無視(Q=0Q=0)、初期共分散を P0=[100001]P_0 = \begin{bmatrix}100 & 0\\ 0 & 1\end{bmatrix}(単位: km2^2, (km/s)2^2)とするとき、予測ステップ後の共分散 P1−=FP0F⊤P_1^{-} = FP_0F^\top を計算してください。位置の分散がどう変化したか、速度の不確かさが位置の不確かさに漏れ込む様子を確認してください。

  2. 上の1次元カルマンゲインの式 k=p−/(p−+r)k = p^{-}/(p^{-}+r) を使って、予測分散 p−=25p^{-} = 25 km2^2、観測ノイズ分散 r=4r=4 km2^2 のときのカルマンゲイン kk を求め、更新後の分散 p=(1−k)p−p = (1-k)p^{-} を計算してください。また r→0r \to 0 および r→∞r \to \infty の極限で kk がどのような値に近づくか、それぞれの物理的な意味を説明してください。

  3. カルマンゲインの式 Kk=Pk−Hk⊤(HkPk−Hk⊤+Rk)−1K_k = P_k^{-}H_k^\top(H_kP_k^{-}H_k^\top+R_k)^{-1} を、本文の最小分散推定の導出過程を再現する形で自分の手で導出してください(トレースの微分 ∂ tr(AXB)/∂X=A⊤B⊤\partial\,\text{tr}(AXB)/\partial X = A^\top B^\top の公式を使ってよい)。

  4. なぜEKFでは状態の時間伝播そのもの(x^k−=f(x^k−1)\hat{\mathbf{x}}_k^{-}=f(\hat{\mathbf{x}}_{k-1}))には非線形関数をそのまま使うのに、共分散の伝播にはヤコビ行列による線形近似を使うのか。両者を同じレベルで近似しない理由を、線形化の目的(何を推定し、何を「不確かさの伝わり方」として近似しているか)に立ち返って説明してください。

まとめと次回予告

カルマンフィルタは、状態空間モデル(状態遷移方程式と観測方程式)の上で、予測ステップと更新ステップを繰り返すことで、観測が届くたびに一定の計算量で推定値を最適に更新し続ける逐次推定アルゴリズムです。カルマンゲイン KkK_k が、予測の不確かさと観測の不確かさのバランスを自動的に取る「重み」として働き、線形・ガウス性の条件下ではベイズ最適な推定量に、より緩い条件下でも最良線形不偏推定量(BLUE)になることを見ました。また、真に非線形な軌道力学に対しては、逐次的な線形化によって同じ枠組みを拡張するEKFという道具があることも確認しました。

これで私たちは、最小二乗法とカルマンフィルタという、軌道決定における2つの主要な推定アルゴリズムを手に入れました。しかし、まだ触れていない重要な問いが残っています。それは「与えられた観測データの精度・配置のもとで、原理的にどこまで良い推定が可能なのか」という、推定の理論的な下限についての問いです。次回は、この問いに答える クラメール・ラオ限界 (Cramér-Rao Lower Bound) に軽く触れ、最小二乗法やカルマンフィルタが実際にその下限にどれだけ近づいているのかを考える視点を導入します。

参考文献

  • R. E. Kalman, “A New Approach to Linear Filtering and Prediction Problems,” Journal of Basic Engineering, 1960
  • G. Welch and G. Bishop, An Introduction to the Kalman Filter, UNC-Chapel Hill TR 95-041
  • B. D. Tapley, B. E. Schutz, G. H. Born, Statistical Orbit Determination, Elsevier Academic Press
  • J. H. Yuen (ed.), Deep Space Telecommunications Systems Engineering, JPL Publication 82-76
  • DSN Telecommunications Link Design Handbook, DSN No. 810-005
  • S. F. Schmidt, “Application of State-Space Methods to Navigation Problems,” Advances in Control Systems, 1966(アポロ計画への応用に関する古典的文献)