Filter Chaining Kalman

Tim saya sedang membangun robot untuk menavigasi secara mandiri di lingkungan luar. Kami baru-baru ini mendapat sensor IMU / GPS terintegrasi baru yang tampaknya melakukan beberapa on-chip filter Kalman diperpanjang. Ini memberikan pitch, roll, dan yaw, utara, timur, dan kecepatan turun, dan lintang dan bujur.

Namun, kami juga memiliki beberapa enkoder yang terpasang pada roda kami, yang memberikan kecepatan linier dan sudut. Sebelum kami mendapatkan sensor IMU / GPS baru ini, kami membuat EKF kami sendiri untuk memperkirakan keadaan kami menggunakan enkoder dan beberapa sensor berbiaya rendah lainnya. Kami ingin menggunakan filter on-chip sensor baru ini, tetapi juga memasukkan pembuat enkode kami ke dalam campuran.

Apakah ada masalah dengan merantai filter? Maksud saya adalah, kita akan menggunakan output EKF on-chip sensor IMU / GPS sebagai pembaruan untuk EKF kita sendiri, sama seperti kita menggunakan data yang dibaca dari pembuat enkode sebagai pembaruan EKF kita. Tampaknya masuk akal bagi saya, tetapi saya bertanya-tanya apa yang seharusnya dilakukan dalam kasus ini.

Anda dapat menggunakan INS / GPS sebagai pembaruan untuk output EKF pertama Anda. Ini sebenarnya bukan rantai, tetapi hanya mengkondisikan estimasi berdasarkan informasi tambahan dari INS / GPS.

Misalkan kita memiliki fungsi-fungsi berikut:

, P t + 1 | t = EKF_PREDICT ( x t , P t , u t ), untuk masukan sebagai negara x , kovarians P , dan input kontrol (diperkirakan oleh odometry) u t .$x_{t+1|t}$$P_{t+1|t}$$x_t$$P_t$$u_t$$x$$P$$u_t$

dan

, P t + 1 | t + 1 = EKF_UPDATE ( x t + 1 | t , P t + 1 | t , x t + 1 ). $x_{t+1|t+1}$$P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}$

Perkiraan dari sensor adalah x t + 1 . Kami memiliki hal-hal seperti:$\hat{x}_{t+1}$

$\hat{x}^{gps}_{t+1} = f(GPS)$

$\hat{x}^{map}_{t+1} = f(map)$

$\hat{x}^{ins}_{t+1} = f(INS)$

dll untuk semua cara lain memperkirakan keadaan robot. Jadi menjalankan fungsi EKF_UPDATE untuk semua sensor itu sudah cukup baik.

Lingkaran Anda akan menjadi seperti ini:

untuk semua waktu $t$

• Mari menjadi saat odometry / perkiraan kinematik pose, dan R u menjadi suara pada perkiraan itu.$u_t$$R_u$

• $x_{t+1|t}$$P_{t+1|t}$$x_t$$P_t$$u_t$$R_u$

• $S$

  • $\hat{x}^{S}_{t+1}$$R_{S}$

  • $x_{t+1|t+1}$$P_{t+1|t+1}$$x_{t+1|t}$$P_{t+1|t}$$\hat{x}_{t+1}, R_S$

  • end-for

• end-for

Beberapa peringatan adalah:

• Karena kami menggunakan EKF, tidak ada jaminan bahwa perkiraan tersebut tidak tergantung pada urutan pembaruan. Artinya, jika Anda melakukan INS lalu GPS, estimasi yang dihasilkan mungkin berbeda dari jika Anda memperbarui dengan GPS lalu INS. Ini biasanya bukan masalah besar, tetapi filter akan membutuhkan penyetelan yang lebih signifikan.

• Harap perhatikan bahwa INS Anda memiliki bias dan pergeseran, yang mungkin memengaruhi keandalan jangka panjang Anda. GPS dapat membantu Anda banyak di sini. Sebagian besar literatur secara bersamaan memperkirakan bias dan pergeseran dalam INS.