不変拡張カルマンフィルタ

不変拡張カルマンフィルタ（IEKF）（反復拡張カルマンフィルタと混同しないでください）は、対称性（または不変性）を持つ非線形システム用の拡張カルマンフィルタ（EKF）のバージョンとして最初に導入され、^{[ 1 ]}その後一般化され、線形カルマンフィルタリング理論のリー群への適応として再構成されました。^{[ 2 ] [ 3 ]}線形出力誤差に基づく線形補正項を使用する代わりに、IEKF は、不変出力誤差に基づく幾何学的に適応された補正項を使用します。同様に、ゲイン行列は線形状態誤差からではなく、不変状態誤差から更新されます。主な利点は、ゲイン方程式と共分散方程式の状態推定値への依存が少なくなることです。場合によっては、EKF の場合よりもはるかに大きな軌跡セットで定数値に収束し、推定の収束が改善されます。

フィルタ導出

離散時間フレームワーク

状態が時間ステップでリー群の要素によって符号化され、ダイナミクスが次の形状を持つシステムを考える： ^{[ 4 ]}${\displaystyle n}$ ${\displaystyle x_{n}}$ ${\displaystyle G}$

${\displaystyle x_{n}=\phi _{n}(x_{n-1})\cdot u_{n}}$

ここで、 はの群自己同型、は群演算、 の元で ある。この系は、次のような形状の 測定を通して観測されるものとする 。${\displaystyle \phi _{n}}$ ${\displaystyle G}$ ${\displaystyle \cdot}$ ${\displaystyle u_{n}}$ ${\displaystyle G}$ ${\displaystyle y_{n}}$

${\displaystyle y_{n}=x_{n}*b_{n}}$

ここで、 は の 元への左作用を持つベクトル空間に属し、は で再び表される （この作用の2番目の要素は の元であり、 ではないため、群作用と混同することはない）。あるいは、同じ理論は右作用によって定義される測定にも当てはまる。 ${\displaystyle b_{n}}$ ${\displaystyle {\mathcal {Y}}}$ ${\displaystyle G}$ ${\displaystyle *}$ ${\displaystyle {\mathcal {Y}}}$ ${\displaystyle G}$

${\displaystyle y_{n}=b_{n}*x_{n}}$

フィルタ方程式

不変拡張カルマンフィルタは、測定関数が左アクションの場合、次の式で定義されるオブザーバーです。 ${\displaystyle {\hat {x}}_{n}}$

${\displaystyle {\hat {x}}_{n|n-1}=\phi _{n}({\hat {x}}_{n-1|n-1})\cdot u_{n}}$

${\displaystyle {\hat {x}}_{n|n}={\hat {x}}_{n|n-1}\cdot \exp(K_{n}({\hat {x}}_{n|n-1}^{-1}*y_{n}-b_{n}))}$

ここで、 はの指数写像であり、はリカッチ方程式を通じて調整されるゲイン行列です。 ${\displaystyle \exp()}$ ${\displaystyle G}$ ${\displaystyle K_{n}}$

測定関数が正しいアクションである場合、更新状態は次のように定義されます。

${\displaystyle {\hat {x}}_{n|n}=\exp(K_{n}(y_{n}*{\hat {x}}_{n|n-1}^{-1}-b_{n}))\cdot {\hat {x}}_{n|n-1}}$

連続時間フレームワーク

上記の離散時間フレームワークは、次のような形状の連続時間ダイナミクスのために最初に導入されました。

${\displaystyle {\frac {d}{dt}}x_{t}=f_{t}(x_{t}),}$

ここでベクトル場はいつでも次の関係を証明する：^{[ 2 ]}${\displaystyle f_{t}}$ ${\displaystyle t}$

${\displaystyle \forall a,b\in G,f_{t}(a\cdot b)=f_{t}(a)b+af_{t}(b)-af(Id)b}$

ここで、群の単位元は で表され、 はの左平行移動（右平行移動）を表す略記法（それぞれ）として用いられます。ここで はにおけるへの接空間を表します。離散時間フレームワークよりも計算が複雑になりますが、性質は同様です。 ${\displaystyle id}$ ${\displaystyle gu=L_{g}(u)}$ ${\displaystyle ug=R_{g}(u)}$ ${\displaystyle L_{g}:TG_{x}\rightarrow TG_{gx}}$ ${\displaystyle R_{g}:TG_{x}\rightarrow TG_{xg}}$ ${\displaystyle TG_{x}}$ ${\displaystyle G}$ ${\displaystyle x}$

主な特性

不変拡張カルマンフィルタの主な利点は、測定の種類に応じて定義が変化する不変誤差変数の挙動にあります。左作用の場合、左不変誤差変数は次のように定義されます。

${\displaystyle e_{n|n-1}^{L}={\hat {x}}_{n|n-1}^{-1}x_{n}}$ 、

${\displaystyle e_{n|n}^{L}={\hat {x}}_{n|n}^{-1}x_{n}}$ 、

一方、右アクションの場合、右不変エラー変数を次のように定義します。

${\displaystyle e_{n|n-1}^{R}=x_{n}{\hat {x}}_{n|n-1}^{-1}}$ 、

${\displaystyle e_{n|n}^{R}=x_{n}{\hat {x}}_{n|n}^{-1}}$ 、

実際、、をそれらの値で置き換えると、代数計算を経て、左アクションについて次の式が得られます。 ${\displaystyle x_{n}}$ ${\displaystyle {\hat {x}}_{n|n-1}}$ ${\displaystyle {\hat {x}}_{n|n}}$

${\displaystyle e_{n|n-1}^{L}=u_{n}^{-1}\cdot \phi _{n}(e_{n-1|n-1}^{L})\cdot u_{n}}$ 、

${\displaystyle e_{n|n}^{L}=\exp(-K_{n}(e_{n|n-1}^{L}*b_{n}-b_{n}))\cdot e_{n|n-1}^{L}}$ 、

そして正しい行動のために：

${\displaystyle e_{n|n-1}^{R}=\phi _{n}(e_{n-1|n-1}^{R})}$ 、

${\displaystyle e_{n|n}^{R}=e_{n|n-1}^{R}\exp(-K_{n}(b_{n}*e_{n|n-1}^{R}-b_{n}))}$ 、

状態の推定値は、誤差変数が続く方程式には含まれていないことがわかります。これは線形カルマンフィルタの特性であり、古典的な拡張カルマンフィルタには存在しませんが、線形の場合との類似性は実際にははるかに大きくなります。 を、恒等式で定義される誤差変数の線形版とします。 ${\displaystyle \xi _{n|n-1},\xi _{n|n}}$

${\displaystyle e_{n|n-1}=\exp(\xi _{n|n-1}),}$

${\displaystyle e_{n|n}=\exp(\xi _{n|n}).}$

そして、テイラー展開によって定義されるので、実際には次の式が得られる。^{[ 2 ]}${\displaystyle F_{n}}$ ${\displaystyle \xi _{n|n}=F_{n}\xi _{n|n-1}+\circ (||\xi _{n|n-1}||)}$

${\displaystyle \xi _{n|n}=F_{n}\xi _{n|n-1}.}$

言い換えれば、高次の項は存在しない：ダイナミクスは誤差変数に対して線形である。この結果と誤差ダイナミクスの独立性は、IEKFの理論的特性と実用的な性能の中核を成す。^{[ 2 ]}${\displaystyle \xi _{n|n},\xi _{n|n-1}}$

対称性保存観測者との関係

ほとんどの物理システムは自然な対称性（または不変性）を有します。つまり、回転、並進、スケーリングといった変換によってシステムが変化しないということです。数学的および工学的な観点から見ると、対象とするシステムに対して適切に設計されたフィルタは、同じ不変性を保つことが理にかなっています。IEKFのアイデアは、システムの対称性を利用するためにEKF方程式を修正することです。

意味

システムを検討する

${\displaystyle {\dot {x}}{=}f(x,u)+M(x)w}$

${\displaystyle y{=}h(x,u)+N(x)v}$

ここで、 は独立した白色ガウスノイズである。単位元 を持つリー群と、 となる（局所）変換群 （）を考える。ノイズを含む前の系が不変であるとは、変換群 の作用によって変化しないことを意味する。つまり、 ${\displaystyle w,v}$ ${\displaystyle G}$ ${\displaystyle e}$ ${\displaystyle \varphi _{g},\psi _{g},\rho _{g}}$ ${\displaystyle g\in G}$ ${\displaystyle (X,U,Y)=(\varphi _{g}(x),\psi _{g}(u),\rho _{g}(y))}$ ${\displaystyle \varphi _{g},\psi _{g},\rho _{g}}$

${\displaystyle {\dot {X}}{=}f(X,U)+M(X)w}$ 。

${\displaystyle Y{=}h(X,U)+N(X)v}$

フィルター式と主な結果

これは対称性保存フィルタであるため、IEKFの一般的な形式は次のようになる^{[ 5 ]。}

${\displaystyle {\dot {\hat {x}}}=f({\hat {x}},u)+W({\hat {x}})L{\Bigl (}I({\hat {x}},u),E({\hat {x}},u,y){\Bigr )}E({\hat {x}},u,y)}$

どこ

• ${\displaystyle E({\hat {x}},u,y)}$ は不変の出力誤差であり、通常の出力誤差とは異なる。${\displaystyle {\hat {y}}-y}$
• ${\displaystyle W({\hat {x}})={\bigl (}w_{1}({\hat {x}}),..,w_{n}({\hat {x}}){\bigr )}}$ 不変フレームである
• ${\displaystyle I({\hat {x}},u)}$ 不変ベクトルである
• ${\displaystyle L(I,E)}$ 自由に選択されたゲイン行列です。

誤差収束を解析するために、不変状態誤差が定義されます。これは標準出力誤差とは異なります。これは、標準出力誤差では通常、システムの対称性が保持されないためです。 ${\displaystyle \eta ({\hat {x}},x)}$ ${\displaystyle {\hat {x}}-x}$

検討対象のシステムと関連する変換グループが与えられれば、移動フレーム法に基づいて を決定する構成的な方法が存在する。 ${\displaystyle E({\hat {x}},u,y),W({\hat {x}}),I({\hat {x}},u),\eta ({\hat {x}},x)}$

EKFと同様に、ゲイン行列は次式から決定される^{[ 6 ]。}${\displaystyle L(I,E)}$

${\displaystyle L{=}PC^{T}{\bigl (}N(e)N^{T}(e){\bigr )}^{-1}}$ 、

${\displaystyle {\dot {P}}{=}AP+PA^{T}+M(e)M^{T}(e)-PC^{T}{\bigl (}N(e)N^{T}(e){\bigr )}^{-1}CP}$ 、

ここで、行列は標準的なEKFのようにではなく、既知の不変ベクトル のみに依存します。このはるかに単純な依存性とその帰結こそが、IEKFの主な関心事です。実際、行列は、EKFの場合のように平衡点よりもはるかに多くの軌道（いわゆる永久軌道）上で一定です。このような軌道の近くでは、収束が保証されている「真の」、つまり線形カルマンフィルタに戻ります。非公式に言えば、これはIEKFが一般に、EKFの任意の緩やかに変化する平衡点の周りだけでなく、少なくとも任意の緩やかに変化する永久軌道の周りで収束することを意味します。 ${\displaystyle A,C}$ ${\displaystyle I({\hat {x}},u)}$ ${\displaystyle ({\hat {x}},u)}$ ${\displaystyle A,C}$

アプリケーション例

姿勢および方位基準システム

不変拡張カルマンフィルタは、例えば姿勢・方位基準システムに用いられます。このようなシステムでは、航空機などの移動する剛体の向き、速度、位置などは、慣性センサー、磁力計、GPS、ソナーといった様々な埋め込みセンサーから推定されます。IEKFの使用は、必然的に^{[ 6 ]}四元数誤差を考慮することにつながります。これは、四元数群の制約条件を維持するためのアドホックなトリックとしてよく用いられます。EKFと比較したIEKFの利点は、多数の軌道に対して実験的に示されています。^{[ 7 ]}${\displaystyle {\hat {q}}q^{-1}}$

慣性航法

不変拡張カルマンフィルタの主な応用は慣性航法であり、これは状態（姿勢行列、速度ベクトル、位置ベクトルからなる）をグループ演算によって定義される リー群^{[ 8 ]に埋め込んだ後のフレームワークに適合する。}${\displaystyle R}$ ${\displaystyle v}$ ${\displaystyle x}$ ${\displaystyle SE_{2}(3)}$

${\displaystyle (R_{1},v_{1},x_{1})\cdot (R_{2},v_{2},x_{2})=(R_{1}R_{2},x_{1}+R_{1}x_{2},v_{1}+R_{1}v_{2})}$

同時位置特定とマッピング

同時位置推定とマッピングの問題は、状態（姿勢行列、位置ベクトル、静的特徴点のシーケンスから構成される）をグループ演算によって定義されるリー群（または平面システムの場合）^{[ 8 ]}に埋め込んだ後の不変拡張カルマンフィルタリングの枠組みにも適合します。 ${\displaystyle R}$ ${\displaystyle x}$ ${\displaystyle p^{1},\dots ,p^{K}}$ ${\displaystyle SE_{K+1}(3)}$ ${\displaystyle SE_{K+1}(2)}$

${\displaystyle (R_{1},x_{1},p_{1}^{1},\dots ,p_{1}^{K})\cdot (R_{2},x_{2},p_{2}^{2},\dots ,p_{2}^{K})=(R_{1}R_{2},x_{1}+R_{1}x_{2},p_{1}^{1}+R_{1}p_{2}^{1},\dots ,p_{1}^{K}+R_{1}p_{2}^{K})}$

この場合の不変拡張カルマンフィルタの主な利点は、誤った観測可能性の問題を解決することです。^{[ 8 ]}

参考文献

1. Bonnabel, S. (2007年12月). 左不変拡張カルマンフィルタと姿勢推定. 2007年第46回IEEE意思決定制御会議 (pp. 1027-1032). IEEE.
2. Barrau, A., Bonnabel, S. (2016). 安定オブザーバーとしての不変拡張カルマンフィルタ．IEEE Transactions on Automatic Control, 62(4), 1797-1812.
3. Potokar, Easton R.; Beard, Randal W.; Mangelson, Joshua G. (2024). 「不変拡張カルマンフィルタ入門 [講義ノート]」. IEEE制御システム. 44 (6): 50– 71. Bibcode : 2024ICSys..44f..50P . doi : 10.1109/MCS.2024.3466488 . ISSN  1066-033X .
4. Barrau, A., Bonnabel, S. (2019). 群上の線形観測システム. Systems & Control Letters, 129, 36-42.
5. S. Bonnabel、Ph. Martin、P. Rouchon、「対称性を保持するオブザーバー」、 IEEE Transactions on Automatic and Control、vol. 53、no. 11、pp. 2514–2526、2008年。
6. S. Bonnabel、Ph. Martin、E. Salaün、「不変拡張カルマンフィルタ：理論と速度支援姿勢推定問題への応用」、第48回IEEE意思決定および制御会議、pp. 1297–1304、2009年。
7. Ph. MartinとE. Salaün、「補助姿勢および方位基準システムのための一般化乗法拡張カルマンフィルタ」、AIAAガイダンス、ナビゲーション、および制御会議、2010年
8. Barrau, A., Bonnabel, S. (2015). 一貫性特性を持つEKF-SLAMアルゴリズム. arXiv プレプリントarXiv : 1510.06263 .