Extended Kalman Filter dengan Laser Scan + Map yang Diketahui

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 .$(x,y,\theta)$

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 Pdan 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:

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

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.$x$$y$$\theta$

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.

• Menggunakan EKF untuk lokalisasi berdasarkan pemindaian laser dan peta yang diketahui adalah ide yang buruk dan tidak akan berfungsi karena salah satu asumsi utama EKF tidak valid: Model pengukuran tidak dapat dibedakan.

Saya akan menyarankan melihat ke Lokalisasi Monte Carlo (Filter Partikel). Ini tidak hanya akan menyelesaikan masalah dengan model pengukuran Anda, tetapi juga memungkinkan pelokalan global (pose awal dalam peta sama sekali tidak diketahui).

Sunting: Saya minta maaf saya lupa menyebutkan poin penting lainnya:

• $z_t = h(x_t) + N(0, Q_t)$$h(x_t)$ bagian dan menambahkan noise Gaussian.

Pertama-tama, Anda tidak menyebutkan apa pun tentang pelokalan yang Anda gunakan. Apakah dengan korespondensi yang dikenal atau tidak dikenal?

Sekarang untuk mendapatkan Jacobian di Matlab Anda dapat melakukan hal berikut

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

Hasil

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

Extended Kalman Filter membutuhkan sistem menjadi linier dan noise menjadi Gaussian. Sistem di sini berarti model gerak dan pengamatan harus linier. Sensor laser memberikan rentang dan sudut ke arah tengara dari bingkai robot, sehingga model pengukurannya tidak linier. Tentang P, jika Anda menganggapnya besar maka estimator EKF Anda akan menjadi buruk di awal dan bergantung pada pengukuran karena vektor keadaan sebelumnya tidak tersedia. Namun, begitu robot Anda mulai bergerak dan merasakan, EKF akan menjadi lebih baik karena menggunakan gerakan dan model pengukuran untuk memperkirakan pose robot. Jika robot Anda tidak merasakan landmark apa pun, ketidakpastian meningkat secara drastis. Juga, Anda perlu berhati-hati tentang sudut. Dalam C ++,cos and sin, sudutnya harus dalam radian. Tidak ada dalam kode Anda tentang masalah ini. Jika Anda mengasumsikan noise sudut dalam derajat saat perhitungan Anda dalam radian, maka Anda mungkin mendapatkan hasil yang aneh karena satu derajat sebagai noise sementara perhitungan dalam radian dianggap besar.