このコラムでは、非線形・非ガウスな動的システムにおける状態推定手法として広く用いられる粒子フィルタ（Particle Filter, PF）と、 推定値の時間的な一貫性を高めるための固定ラグ平滑化（Fixed-lag Smoothing）を組み合わせた実装を紹介します。
PFは、ベイズフィルタのモンテカルロ近似です。ベイズフィルタは以下の2ステップを通じて、逐次的に状態を更新します：
1. 予測ステップ：
\[ p(x_t \mid z_{1:t-1}) = \int p(x_t \mid x_{t-1}, u_t) \cdot p(x_{t-1} \mid z_{1:t-1})\,dx_{t-1} \] これは過去の状態 \(x_{t-1}\) の分布に対して、システム入力 \(u_t\) に基づく予測分布を畳み込む操作です。 ここで，
2. 更新ステップ：
\[ p(x_t \mid z_{1:t}) \propto p(z_t \mid x_t) \cdot p(x_t \mid z_{1:t-1}) \] これは観測値 \(z_t\) に基づき、予測分布を重み付けによって修正する操作です。
粒子フィルタは、状態分布を明示的な関数ではなく、粒子（サンプル）と重みの集合 \(\{x_t^{(i)}, w_t^{(i)}\}\) により表現します。 これにより非線形・非ガウスの状態空間でも近似的な推論が可能となります。実装上は以下の手順で構成されます：
propagate()）computeLikelihood()）resample()）により低確率粒子を除去これにより、ベイズ更新 \[ p(x_t^{(i)} \mid z_{1:t}) \propto p(z_t \mid x_t^{(i)}) \cdot p(x_t^{(i)} \mid z_{1:t-1}) \] を粒子集合として近似することが可能になります。
粒子フィルタは時間方向に向かう逐次処理なので，過去の状態に対する修正能力は持ちません。 これに対し、固定ラグスムーザは、\(t-L\) から \(t\) までの履歴を保持し、 未来情報を用いて過去の粒子分布を再評価することができます。
具体的には以下のような後退処理を行います：
この手法により、粒子は未来の情報に整合的なものが選ばれ、時系列的な一貫性を向上することができます。
特徴的なのは、スムーザで行われる予測および修正の処理が、フィルタで用いる操作と本質的に共通している点です。 状態遷移や尤度評価の操作が共通であることは、実装と理論の両面で大きな利点をもたらします。
propagate()）や尤度評価といった基本関数をフィルタとスムーザの両方で共通化できるため、
コードが簡潔になり、実装ミスのリスクも減ります。また、保守や拡張も容易になります。
このように、共通のロジックに基づいて構築されたスムーザは、コードとしても理論としても自然で扱いやすく、 将来的な応用にも強い設計となっていると考えます。
シミュレーションでは、\(z\) 方向に単振動する対象を想定し、 真の加速度からIMU観測を生成し、 GNSSからは一定間隔で位置を観測します。 これに対し粒子フィルタにより逐次推定を行い、さらに固定ラグスムーザで後方の状態推定を改善します。
スムーザでは、状態空間 \([z, \dot{z}]\) 全体を使ったもの（デフォルト）と、 位置 \(z\) のみを用いた簡易的なものが実装されている． 状態空間全体を使った手法では，尤度評価において位置成分と速度成分の重み係数 \(w_z, w_\dot{z} \) を設定可能としており（ハードコーディング）、 粒子のばらつきに応じた柔軟なスムージングを可能にしています。
このコラムを通じて，ベイズフィルタに基づく推定の全体像と、粒子ベースによる実装の具現化を確認してもらえることを期待します．
粒子フィルタおよびスムーザ部分の実装コード（Javascript)を以下に示します． htmlを含むコード全体は，ここからダウンロードできます．