EKFの視覚化とデバッグ

現在、EKF（拡張カルマンフィルター）のデバッグと調整を行っています。タスクは、ランドマークがARマーカーである従来の移動ロボットポーズトラッキングです。

ある測定値が推定値にどのように影響するかに驚かされることがあります。関係する数値と行列を調べて計算すると、更新手順がどのように実行されたのか、何がなぜ起こったのかがわかりますが、これは非常に退屈です。

だから、EKF更新ステップで何が起こっているのかをよりよく感じるために、誰かが何らかのテクニック、トリックまたは巧妙な視覚化を使用しているのだろうか？

更新＃1（より具体的で、私が念頭に置いているものの最初の近似値を表示します）

私が探しているのは、測定の各コンポーネントが状態の各コンポーネントにどのように影響するかの感覚を与える方法で、1つの更新ステップを視覚化する方法です。

私の最初のアイデアは、測定値とその予測をKマトリックスから取得したいくつかのベクトルと一緒にプロットすることです。Kのベクトルは、イノベーションベクトル（測定-測定予測、プロットではない）が状態の各コンポーネントにどのように影響するかを表します。

現在、状態が2Dポーズ（x、y、angle）であり、測定も2DポーズであるEKFで作業しています。

添付画像（新しいページ/タブで開いてフル解像度で表示）で、（スケーリングされた）ベクトルK（1,1：2）（3x3マトリックスからサブマトリックスを取得するMATLAB構文）は、最初のEKF状態のコンポーネントは、現在のイノベーションベクトルK（2,1：2）によって変化します。EKFの2番目のコンポーネントがどのように変化するかなど。この例では、イノベーションベクトルは比較的大きなxコンポーネントを持ち、ベクトルK（2,1：2）-状態の2番目のコンポーネント（y座標）が最も変化します。

このプロットの問題の1つは、イノベーションベクトルの3番目のコンポーネント（角度）が状態にどのように影響するかを感じさせないことです。状態の最初のコンポーネントは、K（1：1：2）が示すものとは反対に少し増加します-イノベーションの3番目のコンポーネントがこれを引き起こしますが、現在これを視覚化することはできません。

最初の改善点は、イノベーションの3番目の要素がどのように状態に影響するかを視覚化することです。それから、共分散データを追加して、K行列がどのように作成されるかを感じてください。

更新＃2現在、プロットの状態空間には、測定の各コンポーネントが位置をどのように変更するかを示すベクトルがあります。このプロットから、測定の3番目のコンポーネントが最も状態を変化させることがわかります。

（私にとって）測定の効果を視覚化する非常に有益な方法は、各測定の前後にロボットの状態（平均、共分散楕円）をプロットすることです。次に、測定の個々のコンポーネント（方位、ARマーカーの範囲）を取得し、それらを個別に適用して、その感覚を取得します。

これをする：

$a,b$$\theta$$[0, 2\pi]$

通常、EKFの方程式が正しく適用されているかどうかを確認するには、前の仮説、測定された状態、および後の仮説の共分散を追跡するだけで十分です。

幸運を祈ります。質問を頻繁に更新しないでください。代わりに、新しい質問で戻ってきてください。

よく行われることは、状態変数とその3シグマ間隔を経時的にプロットすることです。この間隔が短くなるポイントは更新であり、関連する測定のソースに注釈を付けることができます。

チェックする必要のある実装エラー（間違った方程式だけでなく、数値的に不安定な方程式も）に加えて、更新の効果は、「期待」と「測定」の違いとそれぞれの不確実性によってのみ直接影響を受けます。したがって、このバランスを最初のプロットの時間経過に関して視覚化する方法を見つけ出すことに興味があるかもしれません。