カルマンスムーザ（KF+RTS）によるGNSS補正の模擬

概要

このデモンストレーションでは，加速度センサ（IMU）とGNSS（位置観測）から得られるデータを用いて， カルマンフィルタ（KF）によって物体の位置をリアルタイムに推定し，さらにその結果をRTSスムーザによって滑らかに補正する様子を模擬します．

カルマンフィルタの概要

カルマンフィルタは，線形ガウス系における最適な状態推定器であり，時刻ごとに「予測」と「更新」を繰り返すことで，センサのノイズに埋もれた真の状態を推定する手法です．

このシステムでは，状態ベクトル $ x=[z,\dot{z} ] $ （位置と速度）に対して， IMUから得られる加速度$\ddot{z}$を用いて予測を行い，GNSSによる位置観測が得られたときのみ更新を行います．

カルマンフィルタの数式

プロセスノイズ $ \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) $，観測ノイズ $ \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R}) $ と仮定することで，カルマンフィルタの理論が以下のように導かれます．

状態遷移モデル：

$$ \mathbf{x}_k = \mathbf{A} \mathbf{x}_{k-1} + \mathbf{B} \mathbf{u}_{k-1} + \mathbf{w}_{k-1} $$

観測モデル：

$$ \mathbf{y}_k = \mathbf{H} \mathbf{x}_k + \mathbf{v}_k $$

予測ステップ：

$$ \hat{\mathbf{x}}_k^{-} = \mathbf{A} \hat{\mathbf{x}}_{k-1} + \mathbf{B} \mathbf{u}_{k-1} $$ $$ \mathbf{P}_k^{-} = \mathbf{A} \mathbf{P}_{k-1} \mathbf{A}^\top + \mathbf{Q} $$

更新ステップ：

$$ \mathbf{K}_k = \mathbf{P}_k^{-} \mathbf{H}^\top \left( \mathbf{H} \mathbf{P}_k^{-} \mathbf{H}^\top + \mathbf{R} \right)^{-1} $$ $$ \mathbf{r}_k = \mathbf{y}_k - \mathbf{H} \hat{\mathbf{x}}_k^{-} $$ $$ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^{-} + \mathbf{K}_k \mathbf{r}_k $$ $$ \mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}) \mathbf{P}_k^{-} $$

KFの限界とRTSスムーザ

KFはリアルタイムに逐次的な推定を行う点では優れていますが， 過去の推定値を遡って補正することはできません．そのため，観測が間欠的であったり，プロセスノイズが大きい場合には，本来連続的であるべき変数にジャンプが生じるなど推定が不安定になることがあります．

対策として，ここでは，Rauch-Tung-Striebelスムーザ（RTSスムーザ）を導入します．これは，カルマンフィルタによって蓄積された履歴を用いて， 過去の状態に対して後向きに補正を行うスムージング手法であり， KF単独では得難い滑らかな時系列推定を実現します．ただし短所として，後向き補正は履歴の蓄積を前提とするため，RTSスムーザによる推定結果の確定には，所定のラグ分の時間的遅延が伴います．

RTSスムーザの数式

前提：

時刻 $ k $ におけるフィルタ済み推定値： $ \hat{\mathbf{x}}_k $，共分散： $ \mathbf{P}_k $
時刻 $ k+1 $ におけるスムーズ化済み推定値： $ \hat{\mathbf{x}}_{k+1}^{\text{smooth}} $，共分散： $ \mathbf{P}_{k+1}^{\text{smooth}} $
状態遷移行列： $ \mathbf{A} $
プロセスノイズ共分散行列： $ \mathbf{Q} $

予測共分散：

$$ \mathbf{P}_{k+1|k} = \mathbf{A} \mathbf{P}_k \mathbf{A}^\top + \mathbf{Q} $$

スムージングゲイン：

$$ \mathbf{G}_k = \mathbf{P}_k \mathbf{A}^\top \mathbf{P}_{k+1|k}^{-1} $$

状態のスムージング：

$$ \hat{\mathbf{x}}_k^{\text{smooth}} = \hat{\mathbf{x}}_k + \mathbf{G}_k \left( \hat{\mathbf{x}}_{k+1}^{\text{smooth}} - \hat{\mathbf{x}}_{k+1|k} \right) $$

共分散のスムージング：

$$ \mathbf{P}_k^{\text{smooth}} = \mathbf{P}_k + \mathbf{G}_k \left( \mathbf{P}_{k+1}^{\text{smooth}} - \mathbf{P}_{k+1|k} \right) \mathbf{G}_k^\top $$

本デモのポイント

z方向のみの単純なモデル($x=[z,\dot{z}]$)
IMU由来の高頻度な加速度データ($\ddot{z}$)を用いて状態を逐次予測
GNSSによる位置観測($z$)が得られたときのみカルマンゲインを用いた更新を実施
GNSSには高精度測位と低精度測位に2つのモードを仮定(確率的に出現）
一定の履歴長（ラグ L）をもとに，RTSスムーザによる過去補正を実施
スムーズ化された系列は，KFの推定結果よりも真の軌道に近くなる

デモンストレーション

ソースコード

以下はデモンストレーションで動作しているKFとRTS部分の抜粋です．コード全体はここからダウンロードできます．

fujita@PARI(2025)