Saat ini saya sedang mengerjakan proyek untuk sekolah di mana saya perlu menerapkan Filter Kalman untuk robot titik dengan pemindai laser. Robot dapat berputar dengan radius putar 0 derajat dan bergerak maju. Semua gerakan sedikit demi sedikit linear (drive, rotate, drive).
Simulator yang kami gunakan tidak mendukung akselerasi, semua gerakan bersifat instan.
Kami juga memiliki peta yang dikenal (gambar png) yang perlu dilokalkan. Kami dapat menelusuri jejak gambar untuk mensimulasikan pemindaian laser.
Saya dan mitra saya sedikit bingung dengan model sensor dan gerak yang harus kami gunakan.
Sejauh ini kami memodelkan negara sebagai vektor .
Kami menggunakan persamaan pembaruan sebagai berikut
void kalman::predict(const nav_msgs::Odometry msg){
this->X[0] += linear * dt * cos( X[2] ); //x
this->X[1] += linear * dt * sin( X[2] ); //y
this->X[2] += angular * dt; //theta
this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
this->F(1,2) = linear * dt * cos( X[2] ); //t+1 ?
P = F * P * F.t() + Q;
this->linear = msg.twist.twist.linear.x;
this->angular = msg.twist.twist.angular.z;
return;
}
Kami pikir kami memiliki semuanya berfungsi sampai kami menyadari bahwa kami lupa untuk menginisialisasi P
dan itu nol, artinya tidak ada koreksi yang terjadi. Tampaknya propagasi kami sangat akurat karena kami belum memasukkan noise ke dalam sistem.
Untuk model gerak kami menggunakan matriks berikut untuk F:
Sebagai ini adalah Jacobian dari formula pembaruan kami. Apakah ini benar?
Untuk model sensor kami mendekati Jacobian (H) dengan mengambil perbedaan yang terbatas dari posisi robot , dan dan penelusuran sinar di peta. Kami berbicara dengan TA yang mengatakan bahwa ini akan berhasil tetapi saya masih tidak yakin itu akan berhasil. Prof kami sedang pergi sehingga kami tidak bisa bertanya padanya. Kami menggunakan 3 pengukuran laser per langkah koreksi sehingga H adalah 3x3.
Masalah lain di mana memiliki cara menginisialisasi P. Kami mencoba 1,10.100 dan mereka semua menempatkan robot di luar peta di (-90, -70) ketika peta hanya 50x50.
Kode untuk proyek kami dapat ditemukan di sini: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp
Setiap saran sangat dihargai.
EDIT:
Pada titik ini saya sudah mendapatkan filter untuk stabil dengan noise gerakan dasar tetapi tidak ada gerakan yang sebenarnya. Segera setelah robot mulai bergerak menyaring filter cukup cepat dan keluar dari peta.