状態空間拡張型KFによる固定ラグ平滑化(IMU加速度 + GNSS位置)

概要

カルマンフィルタの基本形では時刻tにおける状態x(t)を状態変数として時刻t+1の値x(t+1)を,システム誤差の共分散行列Qと計測値の誤差共分散行列Rに従って, 重み付けをして予測修正するので,過去の状態に関するスムージングは行われないのが通例です. スムージングのためには,カルマンフィルタによる予測修正後に,RTS法などにより時間を遡って修正を行ったりします(例: カルマンスムーザ(KF+RTS)によるGNSS補正の模擬 )

しかし,時刻tにおける状態変数として過去の履歴を含む

\( X(t)=[x(t),x(t-1),...,x(t-L)]^T \)

にすることで,カルマンフィルタによる状態修正がt−Lの過去にも波及することになるので,固定ラグ平滑化と同等の効果が期待されます.

この方法では,状態空間の次元がラグ長L+1に比例しt増大するのため,計算量は大きくなりますが, カルマンフィルタという逐次処理でスムージング効果が得られるという利点があります.

ここで示すのは,状態空間ベクトルをラグ L= まで拡張し、 IMUの加速度をシステム入力、 GNSSの位置を観測として用いる状態空間拡張型カルマンフィルタの例題です。

デモ

ソースコード

以下はデモンストレーションで動作している状態空間拡張型カルマンフィルタの抜粋です. htmlを含むコード全体はここからダウンロードできます.

                 
             

Fujita@PARI(2025)