拡張カルマンフィルタ

推定理論において、拡張カルマンフィルタ（EKF）は、現在の平均値と共分散の推定値について線形化するカルマンフィルタの非線形バージョンである。明確に定義された遷移モデルの場合、EKFは非線形状態推定、ナビゲーションシステム、GPSの理論における事実上の標準であると考えられてきた^[1]。^[2]

カルマンフィルタの数学的基礎を確立した論文は、1959年から1961年にかけて発表されました。^{[3] [4] [5]} カルマンフィルタは、遷移システムと測定システムの両方で加法的な独立ホワイトノイズを持つ線形 システムモデルの最適な線形推定量です。残念ながら、工学ではほとんどのシステムが非線形であるため、このフィルタリング方法を非線形システムに適用する試みがなされ、この作業のほとんどはNASAエイムズ研究センターで行われました。^{[6] [7]} EKFは、微積分学の手法、つまり多変量テイラー級数展開を採用して、動作点を中心にモデルを線形化しました。システムモデル（以下で説明する）がよくわかっていないか不正確な場合は、モンテカルロ法、特に粒子フィルタが推定に使用されます。モンテカルロ手法はEKFが存在する前から存在していましたが、中程度の大きさの状態空間では計算コストが高くなります。

拡張カルマン フィルタでは、状態遷移モデルと観測モデルは状態の線形関数である必要はなく、微分可能な関数である場合があります。

${\displaystyle {\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1})+{\boldsymbol {w}}_{k-1}}$

${\displaystyle {\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k})+{\boldsymbol {v}}_{k}}$

ここで、w _kとv _{k は}プロセスノイズと観測ノイズであり、どちらも共分散がそれぞれQ _kとR _kであるゼロ平均の多変量ガウスノイズであると想定されます。u _kは制御ベクトルです。

関数f は、以前の推定値から予測された状態を計算するために使用でき、同様に関数hは、予測された状態から予測された測定値を計算するために使用できます。ただし、fとh は共分散に直接適用することはできません。代わりに、偏導関数の行列 (ヤコビアン) が計算されます。

各時間ステップで、ヤコビアンは現在予測された状態で評価されます。これらの行列は、カルマン フィルター方程式で使用できます。このプロセスは、基本的に現在の推定値を中心に非線形関数を線形化します。

表記上の注意については、 カルマン フィルタの記事を参照してください。

表記は、時刻m ≤ nまでの観測値に基づく時刻nにおけるの推定値を表します。 ${\displaystyle {\hat {\mathbf {x} }}_{n\mid m}}$${\displaystyle \mathbf {x} }$

ここで、状態遷移行列と観測行列は、次のヤコビ行列として定義される。

${\displaystyle {{\boldsymbol {F}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k}}}$

${\displaystyle {{\boldsymbol {H}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}}$

線形カルマンフィルタとは異なり、拡張カルマンフィルタは一般に最適な推定器ではありません（測定値と状態遷移モデルが両方とも線形である場合は最適であり、その場合、拡張カルマンフィルタは通常のフィルタと同一です）。さらに、状態の初期推定が間違っている場合、またはプロセスが誤ってモデル化されている場合、フィルタは線形化のためにすぐに発散する可能性があります。拡張カルマンフィルタのもう1つの問題は、推定された共分散行列が真の共分散行列を過小評価する傾向があるため、「安定化ノイズ」を追加しないと統計的な意味で 矛盾するリスクがあることです^[8] 。

より一般的には、非線形フィルタリング問題の無限次元の性質と、最適なフィルタを完全に表すための単純な平均および分散共分散推定量の不十分さを考慮する必要があります。また、拡張カルマンフィルタは、立方体センサ^[9]などの非常に単純な1次元システムでもパフォーマンスが低下する可能性があることにも注意する必要があります。この場合、最適フィルタはバイモーダル^[10]になる可能性があり、そのため、豊富な構造を持つ単一の平均および分散推定量では効果的に表すことができません。二次元のセンサでも同様です。^[11] このような場合、投影フィルタが代替手段として研究されており、ナビゲーションにも適用されています。^{[12]この場合、完全}粒子フィルタなどの他の一般的な非線形フィルタリング方法を検討できます。

このように述べた上で、拡張カルマン フィルタは妥当なパフォーマンスを発揮でき、ナビゲーション システムと GPS における 事実上の標準であると言えます。

モデル

${\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}+\mathbf {w} (t)&\mathbf {w} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {Q} (t){\bigr )}\\\mathbf {z} (t)&=h{\bigl (}\mathbf {x} (t){\bigr )}+\mathbf {v} (t)&\mathbf {v} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {R} (t){\bigr )}\end{aligned}}}$

初期化

${\displaystyle {\hat {\mathbf {x} }}(t_{0})=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]}{\text{, }}\mathbf {P} (t_{0})=Var{\bigl [}\mathbf {x} (t_{0}){\bigr ]}}$

予測更新

${\displaystyle {\begin{aligned}{\dot {\hat {\mathbf {x} }}}(t)&=f{\bigl (}{\hat {\mathbf {x} }}(t),\mathbf {u} (t){\bigr )}+\mathbf {K} (t){\Bigl (}\mathbf {z} (t)-h{\bigl (}{\hat {\mathbf {x} }}(t){\bigr )}{\Bigr )}\\{\dot {\mathbf {P} }}(t)&=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{T}-\mathbf {K} (t)\mathbf {H} (t)\mathbf {P} (t)+\mathbf {Q} (t)\\\mathbf {K} (t)&=\mathbf {P} (t)\mathbf {H} (t)^{T}\mathbf {R} (t)^{-1}\\\mathbf {F} (t)&=\left.{\frac {\partial f}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t),\mathbf {u} (t)}\\\mathbf {H} (t)&=\left.{\frac {\partial h}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t)}\end{aligned}}}$

離散時間拡張カルマンフィルタとは異なり、連続時間拡張カルマンフィルタでは予測と更新のステップが結合されている。^[13]

ほとんどの物理システムは連続時間モデルとして表現されますが、離散時間測定はデジタルプロセッサを介して状態推定のために頻繁に行われます。したがって、システムモデルと測定モデルは次のように表されます。

${\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}+\mathbf {w} (t)&\mathbf {w} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {Q} (t){\bigr )}\\\mathbf {z} _{k}&=h(\mathbf {x} _{k})+\mathbf {v} _{k}&\mathbf {v} _{k}&\sim {\mathcal {N}}(\mathbf {0} ,\mathbf {R} _{k})\end{aligned}}}$

どこ。 ${\displaystyle \mathbf {x} _{k}=\mathbf {x} (t_{k})}$

初期化

${\displaystyle {\hat {\mathbf {x} }}_{0|0}=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]},\mathbf {P} _{0|0}=E{\bigl [}\left(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0})\right)\left(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0})\right)^{T}{\bigr ]}}$

予測する

${\displaystyle {\begin{aligned}{\text{solve }}&{\begin{cases}{\dot {\hat {\mathbf {x} }}}(t)=f{\bigl (}{\hat {\mathbf {x} }}(t),\mathbf {u} (t){\bigr )}\\{\dot {\mathbf {P} }}(t)=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{T}+\mathbf {Q} (t)\end{cases}}\qquad {\text{with }}{\begin{cases}{\hat {\mathbf {x} }}(t_{k-1})={\hat {\mathbf {x} }}_{k-1|k-1}\\\mathbf {P} (t_{k-1})=\mathbf {P} _{k-1|k-1}\end{cases}}\\\Rightarrow &{\begin{cases}{\hat {\mathbf {x} }}_{k|k-1}={\hat {\mathbf {x} }}(t_{k})\\\mathbf {P} _{k|k-1}=\mathbf {P} (t_{k})\end{cases}}\end{aligned}}}$

どこ

${\displaystyle \mathbf {F} (t)=\left.{\frac {\partial f}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t),\mathbf {u} (t)}}$

アップデート

${\displaystyle \mathbf {K} _{k}=\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{T}{\bigl (}\mathbf {H} _{k}\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{T}+\mathbf {R} _{k}{\bigr )}^{-1}}$

${\displaystyle {\hat {\mathbf {x} }}_{k|k}={\hat {\mathbf {x} }}_{k|k-1}+\mathbf {K} _{k}{\bigl (}\mathbf {z} _{k}-h({\hat {\mathbf {x} }}_{k|k-1}){\bigr )}}$

${\displaystyle \mathbf {P} _{k|k}=(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k})\mathbf {P} _{k|k-1}}$

どこ

${\displaystyle {\textbf {H}}_{k}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k|k-1}}}$

更新方程式は、離散時間拡張カルマン フィルタの方程式と同一です。

上記の再帰は、1次の拡張カルマンフィルタ（EKF）です。テイラー級数展開のより多くの項を保持することで、より高次のEKFを得ることができます。たとえば、2次および3次のEKFが説明されています。^[14]ただし、高次のEKFは、測定ノイズが小さい場合にのみパフォーマンス上の利点を提供する傾向があります。

EKFの典型的な定式化では、加法的なプロセスノイズと測定ノイズが仮定されます。しかし、この仮定はEKFの実装には必要ありません。^[15] 代わりに、より一般的な次の形式を考えてみましょう。

${\displaystyle {\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1},{\boldsymbol {w}}_{k-1})}$

${\displaystyle {\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k},{\boldsymbol {v}}_{k})}$

ここでw _kとv _kはプロセスノイズと観測ノイズであり、共分散がそれぞれQ _kとR _kであるゼロ平均多変量ガウスノイズであると仮定します。すると共分散予測方程式とイノベーション方程式は次のようになります。

${\displaystyle {\boldsymbol {P}}_{k|k-1}={{\boldsymbol {F}}_{k-1}}{{\boldsymbol {P}}_{k-1|k-1}}{{\boldsymbol {F}}_{k-1}^{T}}{+}{{\boldsymbol {L}}_{k-1}}{{\boldsymbol {Q}}_{k-1}}{{\boldsymbol {L}}_{k-1}^{T}}}$

${\displaystyle {\boldsymbol {S}}_{k}={{\boldsymbol {H}}_{k}}{{\boldsymbol {P}}_{k|k-1}}{{\boldsymbol {H}}_{k}^{T}}{+}{{\boldsymbol {M}}_{k}}{{\boldsymbol {R}}_{k}}{{\boldsymbol {M}}_{k}^{T}}}$

ここで、行列と行列はヤコビ行列です。 ${\displaystyle {\boldsymbol {L}}_{k-1}}$${\displaystyle {\boldsymbol {M}}_{k}}$

${\displaystyle {{\boldsymbol {L}}_{k-1}}=\left.{\frac {\partial f}{\partial {\boldsymbol {w}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}}$

${\displaystyle {{\boldsymbol {M}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {v}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}}$

予測された状態推定値と測定残差は、プロセスノイズ項と測定ノイズ項の平均で評価され、これはゼロであると想定されます。それ以外の場合、非加法ノイズ定式化は、加法ノイズEKFと同じ方法で実装されます。

場合によっては、非線形システムの観測モデルは について解くことができませんが、暗黙の関数によって表現できます。 ${\displaystyle {\boldsymbol {z}}_{k}}$

${\displaystyle h({\boldsymbol {x}}_{k},{\boldsymbol {z'}}_{k})={\boldsymbol {0}}}$

ノイズの多い観測はどこにありますか。 ${\displaystyle {\boldsymbol {z}}_{k}={\boldsymbol {z'}}_{k}+{\boldsymbol {v}}_{k}}$

従来の拡張カルマンフィルタは、以下の置換で適用できる。^{[16] [17]}

${\displaystyle {{\boldsymbol {R}}_{k}}\leftarrow {{\boldsymbol {J}}_{k}}{{\boldsymbol {R}}_{k}}{{\boldsymbol {J}}_{k}^{T}}}$

${\displaystyle {\tilde {\boldsymbol {y}}}_{k}\leftarrow -h({\hat {\boldsymbol {x}}}_{k|k-1},{\boldsymbol {z}}_{k})}$

どこ：

${\displaystyle {{\boldsymbol {J}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {z}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1},{\boldsymbol {z}}_{k}}}$

ここでは、元の観測共分散行列が変換され、イノベーションが異なって定義されます。ヤコビ行列は前と同じように定義されますが、暗黙の観測モデルから決定されます。 ${\displaystyle {{\boldsymbol {R}}_{k}}}$${\displaystyle {\tilde {\boldsymbol {y}}}_{k}}$${\displaystyle {{\boldsymbol {H}}_{k}}}$${\displaystyle h({\boldsymbol {x}}_{k},{\boldsymbol {z}}_{k})}$

反復拡張カルマンフィルタは、テイラー展開の中心点を再帰的に修正することで、拡張カルマンフィルタの線形化を改善します。これにより、計算要件が増加する代わりに線形化誤差が減少します。^[17]

ロバストな拡張カルマンフィルタは、現在の状態推定値について信号モデルを線形化し、線形カルマンフィルタを使用して次の推定値を予測することによって生成されます。これは局所的に最適なフィルタを生成しようとしますが、基礎となるリカッチ方程式の解が正定値であるとは限らないため、必ずしも安定しているわけではありません。パフォーマンスを向上させる方法の 1 つは、最適性と安定性をトレードオフする擬似代数リカッチ手法 ^[18]です 。拡張カルマンフィルタの一般的な構造は保持されますが、ゲイン設計に擬似代数リカッチ方程式の正定値解を選択することで安定性が実現されます。

拡張カルマンフィルタの性能を向上させる別の方法は、ロバスト制御からのH無限大の結果を利用することです。ロバストフィルタは、設計リカッチ方程式に正定値項を追加することで得られます。^[19]追加項はスカラー値でパラメータ化され、設計者はこれを微調整して平均二乗誤差とピーク誤差の性能基準の間のトレードオフを実現できます。

不変拡張カルマン フィルタ (IEKF) は、対称性 (または不変性) を持つ非線形システム用の EKF の修正版です。これは、EKF と最近導入された対称性保存フィルタの両方の利点を兼ね備えています。線形出力誤差に基づく線形補正項を使用する代わりに、IEKF は不変出力誤差に基づく幾何学的に適応された補正項を使用します。同様に、ゲイン マトリックスは線形状態誤差からではなく、不変状態誤差から更新されます。主な利点は、ゲイン方程式と共分散方程式が、EKF の場合と同様に平衡点よりもはるかに大きな軌道セットで定数値に収束し、推定がより適切に収束することです。

EKF の改良版として有望な非線形カルマン フィルタは、アンセンテッド カルマン フィルタ(UKF) です。UKF では、確率密度は、基になる分布をガウス分布として表す点の決定論的サンプリングによって近似されます。これらの点の非線形変換は事後分布の推定を目的としており、事後分布のモーメントは変換されたサンプルから導出できます。この変換は、アンセンテッド変換として知られています。UKF は、すべての方向の誤差の推定において、EKF よりも堅牢で正確である傾向があります。

「拡張カルマンフィルタ（EKF）は、おそらく非線形システムで最も広く使用されている推定アルゴリズムです。しかし、推定コミュニティにおける35年以上の経験から、実装が難しく、調整が難しく、更新の時間スケールでほぼ線形であるシステムに対してのみ信頼できることがわかっています。これらの困難の多くは、線形化の使用から生じています。」^[1]

2012年の論文には、UKFのいくつかの公開された変種が、拡張カルマンフィルタとしても知られる2次拡張カルマンフィルタ（SOEKF）ほど正確ではないことを示唆するシミュレーション結果が含まれています。^[20] SOEKFは、Bassらによって最初に説明されたモーメントダイナミクスで、UKFより約35年前にさかのぼります。^[21] 非線形状態遷移に対するカルマン型フィルタを実装する際の難しさは、精度に必要な数値安定性の問題に起因します。 ^{[22]ただし、UKFも線形化、つまり}線形回帰 を使用しているため、この困難から逃れることはできません。UKFの安定性の問題は、一般に共分散行列の平方根への数値近似に起因しますが、EKFとSOEKFの両方の安定性の問題は、軌跡に沿った テイラー級数近似の問題の可能性に起因します。

UKF は、実際には1994 年に Evensen によって発明されたアンサンブル カルマン フィルタよりも古いものです。UKF と比較すると、アンサンブル メンバーの数は状態次元よりもはるかに少なくて済むという利点があり、状態空間のサイズが 10 億以上の天気予報など、非常に高次元のシステムに適用できます。

可能性分布を表現する新しい方法を備えたファジィカルマンフィルタは、確率分布を可能性分布に置き換えて真の可能性フィルタを得るために最近提案され、非対称のプロセスおよび観測ノイズの使用と、プロセスモデルと観測モデルの両方でのより高い不正確さを可能にしました。 ^[23]

• カルマンフィルタ
• アンサンブルカルマンフィルタ
• 高速カルマンフィルタ
• 不変拡張カルマンフィルタ
• 移動地平線推定
• 粒子フィルター
• 無香料カルマンフィルター
• 非線形フィルタリング問題
• 投影フィルター

• オドメトリとランドマークに基づく差動車輪ロボットの位置推定