Visualisasi dan debugging EKF

Saat ini saya sedang men-debug dan menyetel EKF (Extended Kalman Filter). Tugasnya adalah pelacakan pose robot mobile klasik di mana landmark adalah penanda AR.

Terkadang saya terkejut bagaimana beberapa pengukuran mempengaruhi estimasi. Ketika saya melihat dan menghitung angka dan matriks yang terlibat, saya bisa mengetahui bagaimana langkah pembaruan dijalankan, apa dan mengapa sebenarnya terjadi, tetapi ini sangat membosankan.

Jadi saya bertanya-tanya apakah ada yang menggunakan beberapa teknik, trik atau visualisasi cerdas untuk mendapatkan perasaan yang lebih baik tentang apa yang terjadi pada langkah pembaruan EKF?

UPDATE # 1 (akan lebih spesifik dan menunjukkan perkiraan pertama dari apa yang ada dalam pikiran saya)

Apa yang saya cari, adalah beberapa cara untuk memvisualisasikan satu langkah pembaruan dengan cara yang memberi saya perasaan bagaimana masing-masing komponen pengukuran mempengaruhi masing-masing komponen negara.

Ide pertama saya adalah untuk merencanakan pengukuran dan prediksi bersama-sama dengan beberapa vektor yang diambil dari matriks K. Vektor dari K menunjukkan bagaimana vektor inovasi (pengukuran - prediksi pengukuran, tidak diplot) akan mempengaruhi masing-masing komponen negara.

Saat ini saya sedang bekerja dengan EKF di mana keadaannya adalah pose 2D (x, y, sudut) dan pengukuran juga pose 2D.

Pada gambar terlampir (buka di halaman / tab baru untuk melihat dalam resolusi penuh), vektor (diskalakan) K (1,1: 2) (sintaks MATLAB untuk mengambil submatrix dari matriks 3x3) harus memberikan gambaran bagaimana yang pertama komponen keadaan EKF akan berubah dengan vektor inovasi saat ini, K (2,1: 2) bagaimana komponen kedua EKF akan berubah, dll. Dalam contoh ini, vektor inovasi memiliki komponen x yang relatif besar dan disejajarkan dengan vektor K (2,1: 2) - komponen kedua dari keadaan (koordinat y) akan paling berubah.

Salah satu masalah dalam plot ini adalah tidak memberikan kesan bagaimana komponen ketiga (sudut) vektor inovasi mempengaruhi keadaan. Komponen pertama dari negara sedikit meningkat, bertentangan dengan apa yang K (1: 1: 2) tunjukkan - komponen ketiga dari inovasi menyebabkan ini, tetapi saat ini saya tidak dapat memvisualisasikan ini.

Peningkatan pertama adalah memvisualisasikan bagaimana komponen ketiga dari inovasi mempengaruhi negara. Maka akan menyenangkan untuk menambahkan data kovarian untuk merasakan bagaimana matriks K dibuat.

UPDATE # 2 Sekarang plot memiliki vektor di ruang-negara yang menunjukkan bagaimana setiap komponen pengukuran mengubah posisi. Dari plot ini, saya dapat melihat bahwa komponen ketiga dari pengukuran paling banyak mengubah keadaan.

Cara yang sangat informatif untuk memvisualisasikan efek pengukuran (bagi saya) adalah memplot keadaan robot (rata-rata, dengan elips kovarians) sebelum dan sesudah setiap pengukuran. Kemudian, ambil masing-masing komponen pengukuran (bantalan, jarak untuk penanda AR), dan terapkan secara terpisah untuk merasakannya.

Untuk melakukan ini:

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

Melacak kovarians dari hipotesis sebelumnya, keadaan terukur, dan hipotesis posterior biasanya cukup untuk menemukan apakah persamaan EKF diterapkan dengan benar.

Semoga berhasil, dan jangan terlalu sering memperbarui pertanyaan Anda. Sebaliknya, kembali dengan pertanyaan baru.

Sesuatu yang sering dilakukan adalah memplot variabel keadaan dari waktu ke waktu serta interval 3-sigma mereka. Poin di mana interval ini menyusut adalah pembaruan, di mana Anda mungkin dapat menjelaskan sumber pengukuran yang terlibat.

Selain kesalahan dalam implementasi yang harus diperiksa (tidak hanya persamaan yang salah, tetapi persamaan numerik yang tidak stabil juga), efek pembaruan hanya secara langsung dipengaruhi oleh perbedaan antara apa yang "diharapkan" dan "diukur" dan ketidakpastian masing-masing. Jadi, Anda mungkin tertarik untuk mencari cara untuk memvisualisasikan keseimbangan ini dalam hal perkembangan waktu di plot pertama.