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番目のコンポーネントが最も状態を変化させることがわかります。