Obecnie debuguję i dostrajam EKF (Extended Kalman Filter). Zadanie polega na klasycznym śledzeniu pozy robotów mobilnych, w których punktami orientacyjnymi są znaczniki AR.
Czasami jestem zaskoczony, jak jakiś pomiar wpływa na oszacowanie. Kiedy patrzę i obliczam zaangażowane liczby i macierze, mogę dowiedzieć się, jak wykonano krok aktualizacji, co i dlaczego dokładnie się wydarzyło, ale jest to bardzo żmudne.
Zastanawiam się więc, czy ktoś używa jakiejś techniki, sztuczki lub sprytnej wizualizacji, aby lepiej zrozumieć, co dzieje się na etapie aktualizacji EKF?
AKTUALIZACJA # 1 (będzie bardziej szczegółowa i pokaże pierwsze przybliżenie tego, co mam na myśli)
To, czego szukam, to sposób wizualizacji jednego kroku aktualizacji w sposób, który daje mi odczucie, jak każdy składnik pomiaru wpływa na każdy element stanu.
Moim pierwszym pomysłem jest wykreślenie pomiaru i jego prognozy wraz z niektórymi wektorami pobranymi z macierzy K. Wektory z K reprezentują wpływ wektora innowacji (pomiar - prognoza pomiaru, nie wykreślona) wpłynie na każdy element stanu.
Obecnie pracuję z EKF, w którym stanem jest pozycja 2D (x, y, kąt), a pomiary są również pozycjami 2D.
Na załączonym obrazku (otwórz go na nowej stronie / karcie, aby zobaczyć w pełnej rozdzielczości), (skalowany) wektor K (1,1: 2) (składnia MATLAB do pobrania submatrix z matrycy 3x3) powinna dać wyobrażenie o tym, jak pierwsza składnik stanu EKF zmieni się wraz z bieżącym wektorem innowacji, K (2,1: 2), jak zmieni się drugi składnik EKF itp. W tym przykładzie wektor innowacji ma stosunkowo duży składnik x i jest wyrównany z wektor K (2,1: 2) - drugi składnik stanu (współrzędna y) najbardziej się zmieni.
Jednym z problemów na tym wykresie jest to, że nie daje odczuć, w jaki sposób trzeci składnik (kąt) wektora innowacji wpływa na stan. Pierwszy składnik stanu nieco wzrasta, w przeciwieństwie do tego, co wskazuje K (1: 1: 2) - powoduje to trzeci składnik innowacji, ale obecnie nie mogę tego sobie wyobrazić.
Pierwsza poprawa polegałaby na wizualizacji wpływu trzeciego elementu innowacji na państwo. Wtedy byłoby miło dodać dane kowariancji, aby poczuć, jak tworzona jest macierz K.
AKTUALIZACJA # 2 Teraz wykres zawiera wektory stanu w przestrzeni stanu, które pokazują, jak każdy element pomiaru zmienia pozycję. Z tego wykresu widzę, że trzeci składnik pomiaru najbardziej zmienia stan.
źródło
Odpowiedzi:
Bardzo pouczającym sposobem wizualizacji efektu pomiarów (jak dla mnie) jest wykreślenie stanu robota (średnio z elipsą kowariancyjną) przed i po każdym pomiarze. Następnie weź poszczególne składniki pomiaru (łożysko, zasięg dla znaczników AR) i zastosuj je osobno, aby go wyczuć.
Aby to zrobić:
Śledzenie kowariancji wcześniejszej hipotezy, zmierzonego stanu i hipotezy tylnej jest zwykle wystarczające do stwierdzenia, czy równania EKF są stosowane poprawnie.
Powodzenia i nie aktualizuj pytania zbyt często. Zamiast tego wróć z nowymi pytaniami.
źródło
Często wykonuje się wykreślanie zmiennych stanu w czasie, a także ich odstępów 3 sigma. Punkty, w których ten odstęp zmniejsza się, są aktualizacjami, w których można ewentualnie opisać źródło zaangażowanych pomiarów.
Oprócz błędów w implementacji, które należy sprawdzić (nie tylko nieprawidłowe równania, ale również równania niestabilne numerycznie), na wpływ aktualizacji ma bezpośredni wpływ tylko różnica między tym, co „oczekiwane” i „mierzone”, a ich odpowiednimi niepewnościami. Być może zainteresuje Cię wymyślenie sposobu wizualizacji tej równowagi pod kątem przebiegu czasu w pierwszym wykresie.
źródło