Perkiraan posisi berganda

Saya memiliki sistem di mana saya memiliki dua subsistem yang terpisah untuk memperkirakan posisi robot. Subsistem pertama terdiri dari 3 kamera yang digunakan untuk mendeteksi penanda yang dilakukan robot dan yang menghasilkan 3 perkiraan posisi dan orientasi robot. Subsistem kedua adalah sistem yang terletak pada robot dan mengukur kecepatan pada dua titik robot. Dengan mengintegrasikan keduanya secara numerik saya bisa mendapatkan perkiraan posisi dan orientasi robot (karena saya melacak dua titik sekaligus).

Sistem pertama kurang akurat tetapi sistem kedua melayang. Sistem pertama memberikan output sekitar satu detik sekali, sedangkan yang kedua memberikan output lebih sering (100-200 kali per detik).

Saya berasumsi harus ada pendekatan yang lebih baik daripada hanya mengatur ulang posisi dengan perkiraan sistem pertama (karena tidak 100% akurat), tetapi juga menggunakan posisi akumulasi dari sistem sensor kedua dan menggabungkannya dengan data baru dari sistem pertama. Juga, ada pertanyaan bagaimana memadukan 3 perkiraan sistem pertama? Harus ada cara yang lebih baik daripada rata-rata murni karena mungkin terjadi bahwa kedua perkiraan itu persis sama dan yang ketiga sama sekali berbeda (artinya mungkin lebih salah)?

Apakah Anda memiliki algoritme fusi yang direkomendasikan untuk digunakan dalam sistem seperti itu? Saya tahu tentang filter Kalman, tetapi saya mengalami kesulitan mencari tahu bagaimana menggunakannya sebagai data output dua sistem pada frekuensi yang berbeda.

Saya harap pertanyaannya cukup jelas, apa pendekatan terbaik untuk menggabungkan estimasi menjadi estimasi yang lebih benar dan akurat?

Terima kasih

Apa yang Anda gambarkan pada dasarnya adalah sebuah kasus buku teks untuk menggunakan filter Kalman. Pertama, Anda perlu langkah prediksi. Mari kita asumsikan Anda memprediksi pose robot$(x,y,\theta)$, mengingat perkiraan pose sebelumnya dan pengukuran kecepatan frekuensi tinggi Anda $(v,\omega)$dimana $v$ adalah kecepatan linier dan $\omega$ adalah kecepatan sudut.

$P$ adalah matriks kovarians 3x3 yang mewakili ketidakpastian pose robot. $Q$ adalah kovarians dari input Anda (yaitu, seberapa berisik pengukuran kecepatan itu?) $F$ adalah Jacobian dari model gerak sehubungan dengan negara dan $G$ adalah Jacobian sehubungan dengan input, yaitu,

Sekarang Anda memiliki pembaruan koreksi yang kurang sering, yang benar-benar mengukur keadaan penuh, membuat ini cukup sederhana, yaitu,

dimana $z_k$ adalah ukuran Anda (dari kamera) dan $R$adalah matriks kovarians yang terkait dengan pengukuran itu (mungkin matriks diagonal). Pengukuran ini dibandingkan dengan pengukuran yang diprediksi (yang dalam kasus Anda hanyalah perkiraan pose terbaru). Dalam kasus sederhana ini, gain Kalman adalah proporsi kovarians pose saat ini dibandingkan dengan jumlah kovarians pose dan kovarians pengukuran.

Untuk menjawab pertanyaan Anda tentang perbedaan tarif, Anda dapat menjalankan pembaruan gerakan Anda berulang kali hingga pembaruan prediksi Anda tiba. Misalnya, mungkin terjadi pembaruan gerakan terjadi 100 kali sebelum Anda melakukan koreksi.

Anda juga bertanya tentang cara menangani tiga kamera. Cara termudah adalah dengan hanya memprosesnya secara berurutan; terapkan saja tiga koreksi berturut-turut. Cara lain adalah dengan menumpuknya dan melakukan pembaruan tunggal. Anda perlu menyesuaikan langkah pembaruan koreksi untuk melakukannya dengan cara ini.