アンサンブルカルマンフィルタで状態を推定する — モデル予測とセンサ観測をどう融合するか

はじめに

カルマンフィルタは、モデルによる予測とセンサによる観測を組み合わせて、直接は分からない状態を推定するための手法です。 古くはアポロ計画でも、宇宙船の航法や自己位置推定を支える技術として使われました。

現代でも用途は広く、ロケット、ドローン、車両、船舶、設備監視、プロセス制御、CFDと実測の融合などで使われます。 共通しているのは、モデルだけでは現実からずれていき、センサだけではノイズや欠測に振り回されるという状況です。 カルマンフィルタは、予測と観測の重み付けを、不確かさに基づいて決定します。

以前のカルマンフィルタ・Multi-fidelity・Sim-to-Realの記事では、モデルと現実の乖離を埋める考え方を広く扱いました。 今回はその中でも、非線形モデルを扱いやすい アンサンブルカルマンフィルタ（Ensemble Kalman Filter, EnKF） に絞ります。

題材としては、3次元空間を飛ぶロケットの位置と速度を推定するタスクを考えます。

状態推定とは何をする問題か

状態推定では、まず「本当に知りたいが、完全には観測できない量」を状態ベクトルとして定義します。 ロケットの簡略モデルなら、状態は位置と速度です。

$$x = [p_x, p_y, p_z, v_x, v_y, v_z]$$

ここで px, py, pz は3次元位置、vx, vy, vz は3次元速度です。 一方で、センサが毎回この6成分をすべて測れるとは限りません。 たとえば位置センサだけがあるなら、観測値は次の形になります。

$$z = [p_x, p_y, p_z] + \varepsilon_{\mathrm{obs}}$$

この場合、単発の位置観測だけでは速度は決まりません。 速度が推定できるのは、過去から現在までの位置観測の時系列と、制御入力を含む運動モデルを合わせて見るからです。 EnKFでは、位置と速度を含む候補状態の集まりをモデルで予測し、観測された位置とのずれを使って、速度成分も整合する方向に間接的に補正します。

この問題は、次の2本の式で見ると整理しやすくなります。

$$x_{\mathrm{forecast}} = f(x_{\mathrm{previous}}, u) + \varepsilon_{\mathrm{process}}$$ $$z = h(x_{\mathrm{true}}) + \varepsilon_{\mathrm{obs}}$$

f は状態を1ステップ進める予測モデル、u は制御入力、h は状態をセンサで見える量に変換する観測モデルです。 process_noise はモデル化できていない外乱や近似誤差、observation_noise はセンサの測定誤差を表します。

ロケットの制御入力をどう扱うか

ロケットの例では、制御入力 u としてメインスラスタの出力、ジンバル角、補助スラスタ（RCS）を考えます。 これらは推定したい状態そのものではなく、予測モデルを進めるために外から与える入力です。

• メインスラスタ: 主に上向きの加速度を決める
• ジンバル角: 推力方向を傾け、横方向の加速度を作る
• RCS: 姿勢や横方向の微小な補正を与える

推定器から見ると、制御入力は「いま機体にどの向き・どの大きさの力を与えたか」という情報です。 この入力から加速度を計算し、位置と速度を時間発展させます。 車両ならステアリング角と駆動力、設備なら入力電力や流量、CFDなら境界条件や制御パラメータが同じ役割を持ちます。

重要なのは、制御入力も完全ではないという点です。 指令値と実際の推力に差がある、風や空力を十分にモデル化できていない、センサが遅れて届く、といった誤差は必ず残ります。 そのずれをすべてモデル式に押し込むのではなく、プロセスノイズとして持たせることで、フィルタが観測で修正できる余地を残します。

線形カルマンフィルタの基本形

まず、1次元の最も単純な状況を考えます。 同じ未知量 x について、モデルによる予測とセンサ観測の2つが得られているとします。

$$x \sim \mathcal{N}(\hat{x}^-, \sigma_{\mathrm{pred}}^2), \qquad x \sim \mathcal{N}(y, \sigma_{\mathrm{obs}}^2)$$

ここで、$\hat{x}^-$ はモデルによる予測値、$y$ は観測値です。 $\sigma_{\mathrm{pred}}^2$ は予測の不確かさ、$\sigma_{\mathrm{obs}}^2$ は観測の不確かさを表します。

両方をガウス分布として扱えるなら、統合後の推定値は分散の逆数、つまり精度で重み付けした平均になります。

$$\hat{x} = \frac{ \sigma_{\mathrm{obs}}^2 \hat{x}^- + \sigma_{\mathrm{pred}}^2 y }{ \sigma_{\mathrm{pred}}^2 + \sigma_{\mathrm{obs}}^2 }$$

これは次の形に整理できます。

$$\hat{x} = \hat{x}^- + K(y - \hat{x}^-), \qquad K = \frac{ \sigma_{\mathrm{pred}}^2 }{ \sigma_{\mathrm{pred}}^2 + \sigma_{\mathrm{obs}}^2 }$$

この K がカルマンゲインです。 予測の不確かさが大きいほど K は大きくなり、推定値は観測側に強く寄ります。 観測の不確かさが大きいほど K は小さくなり、推定値は予測側に残ります。

diagram rendering…

多次元のカルマンフィルタでは、分散が共分散行列 P に、観測値との差分がベクトルになります。 基本形は同じで、予測値に対して「観測との差分」をカルマンゲインで重み付けして足します。

$$x_a = x_f + K(z - Hx_f)$$

線形・ガウスの前提では、平均 x と共分散 P を解析的に更新できます。 一方、ロケットのように推力方向や空力が非線形になると、分布を解析的に追うのが難しくなります。

EnKFにおける置き換え

EnKFは、確率分布を式で直接持つ代わりに、状態のサンプル集合を持ちます。 このサンプル集合がアンサンブルです。

$$X = \{x_1, x_2, \ldots, x_N\}$$

置き換えるのは、通常のカルマンフィルタで平均 x と共分散 P を解析的に持っていた部分です。

$$x \rightarrow \mathrm{mean}(X), \qquad P \rightarrow \mathrm{cov}(X)$$ $$F x \rightarrow f(x_i, u), \qquad H x \rightarrow h(x_i)$$

つまり、EnKFは分布を厳密に計算するのではなく、複数のサンプルを時間発展させ、その平均とばらつきで分布を近似する方法です。

EnKFの更新式

EnKFでも、予測後に観測で更新する流れは同じです。 通常のカルマンフィルタの更新式は、次の形でした。

$$x_a = x_f + K(z - Hx_f)$$

EnKFでも、各サンプルに対してほぼ同じ形の更新を行います。

$$x_i^a = x_i^f + K(z_i - h(x_i^f))$$

違いは、更新対象が単一の平均状態 x_f ではなく、アンサンブル内の各サンプル x_i^f になる点です。 また、線形観測モデル H x_f の代わりに、非線形の観測モデル h(x_i^f) を各サンプルに適用します。

予測もサンプルごとに行います。

$$x_i^f = f(x_i^a, u) + q_i$$

カルマンゲイン K の役割も同じですが、共分散は解析式ではなくアンサンブルから計算します。

$$\begin{aligned} Y_f &= \{h(x_1^f), h(x_2^f), \ldots, h(x_N^f)\} \\ P_{xy} &= \mathrm{cov}(X_f, Y_f) \\ P_{yy} &= \mathrm{cov}(Y_f, Y_f) + R \\ K &= P_{xy} P_{yy}^{-1} \end{aligned}$$

X_f は予測後の状態アンサンブル、Y_f はそれを観測空間に写したアンサンブルです。 このように、更新式そのものは通常のカルマンフィルタと同型で、共分散と観測予測の作り方がアンサンブルベースに置き換わります。

diagram rendering…

このように、カルマンフィルタの予測と更新という骨格は維持されます。 一方で、平均と共分散を解析式で追う部分は、アンサンブルの平均とばらつきによる近似に置き換えられます。

実装はこの順で進める

EnKFは、最初から現実の全要素を入れると調整が難しくなります。 まずは小さな状態推定問題として作り、段階的に現実へ近づけるのが安全です。

1. 状態ベクトルを決める
   推定したい量を最小限に絞ります。位置だけで足りるのか、速度や姿勢も必要なのか、センサバイアスや外乱パラメータも状態に含めるのかを決めます。

2. 観測モデルを決める
   どのセンサが状態のどの成分を見ているのかを定義します。直接観測できる量と、モデルを通じて間接的に推定する量を分けます。

3. 予測モデルを1ステップ関数にする
   next_state = f(state, input, dt) の形にします。複雑なシミュレータでも、フィルタから見た境界はこの形に寄せると扱いやすくなります。

4. ノイズを仮置きする
   プロセスノイズと観測ノイズをまず大まかに置きます。初期値はセンサ仕様、実験データ、モデル誤差の感覚値から始め、残差を見ながら調整します。

5. 真値が分かる小さなシナリオで検証する
   まずはシミュレーション上で真値を作り、そこにノイズ付き観測を加えます。推定値が真値に寄るか、観測に過剰追従していないかを確認します。

6. 欠測・外れ値・周期ずれを入れる
   現実のセンサはきれいに来ません。観測なしのステップ、明らかな外れ値、周期の揺らぎを入れて、破綻しないかを見ます。

この順で作ると、問題が起きたときに、EnKFそのものの問題なのか、モデルの問題なのか、センサノイズ設定の問題なのかを切り分けやすくなります。

調整で見るべきポイント

EnKFの品質は、ノイズ設定とアンサンブル設計に大きく依存します。 平均値だけでなく、推定分布の広がりも必ず見ます。

• プロセスノイズが小さすぎる: モデルを信じすぎ、観測が来ても十分に補正されません。現実からじわじわずれていきます。
• プロセスノイズが大きすぎる: 予測分布が広がりすぎ、推定がふらつきます。
• 観測ノイズが小さすぎる: センサ値に過剰追従し、ノイズや外れ値で推定が暴れます。
• 観測ノイズが大きすぎる: 観測をほとんど信じず、モデル予測だけに近い挙動になります。
• アンサンブル数が少なすぎる: 共分散推定が不安定になり、カルマンゲインが毎回ぶれます。
• 状態に必要な変数が入っていない: たとえばセンサバイアスや風外乱を状態に含めないと、別の状態量にしわ寄せされます。

平均値が真値に近くても、分布が過度に狭ければ過信している可能性があります。 逆に分布が広がりっぱなしなら、モデルまたは観測が推定に十分な情報を与えていません。

おわりに

アンサンブルカルマンフィルタは、モデル予測とセンサ観測を逐次的に融合するための、実装しやすく拡張しやすい状態推定手法です。 平均と共分散を式で直接追う代わりに、複数の候補状態を非線形モデルで進め、そのばらつきから「どれくらい観測に寄せるか」を決めます。

実装で大事なのは、EnKFの式を先に書くことではなく、状態、入力、観測、ノイズ、更新周期を明確に分けることです。 この境界が整理されていれば、ロケットの位置推定だけでなく、移動体、設備監視、CFDと実測の融合、オンライン補正にも同じ考え方を展開できます。

状態推定の役割は、センサ値をなめらかにすることではありません。 不完全なモデルとノイズを含む観測の間で、いま最も筋の良い状態分布を持ち続けることです。