Filter Kalman

Ada banyak artikel berbeda tentang filter Kalman, tetapi sulit untuk menemukan artikel yang berisi penjelasan, dari mana semua formula filter berasal. Saya pikir bahwa tanpa pemahaman bahwa ilmu ini menjadi sepenuhnya tidak dapat dimengerti. Pada artikel ini saya akan mencoba menjelaskan semuanya dengan cara yang sederhana.

Filter Kalman adalah alat yang sangat kuat untuk menyaring berbagai jenis data. Gagasan utama di balik ini bahwa seseorang harus menggunakan informasi tentang proses fisik. Misalnya, jika Anda memfilter data dari speedometer mobil maka inersianya memberi Anda hak untuk memperlakukan penyimpangan kecepatan besar sebagai kesalahan pengukuran. Filter Kalman juga menarik dengan fakta bahwa dalam beberapa hal itu adalah filter terbaik. Kami akan membahas secara tepat apa artinya. Di akhir artikel saya akan menunjukkan bagaimana mungkin untuk menyederhanakan formula.

Pendahuluan

Pada awalnya, mari kita menghafal beberapa definisi dan fakta dari teori probabilitas.

Variabel acak

Ketika seseorang mengatakan bahwa itu diberikan variabel acak $$$\ xi$ , itu berarti dapat mengambil nilai acak. Nilai berbeda datang dengan probabilitas berbeda. Misalnya, jika seseorang menjatuhkan dadu maka himpunan nilai diskrit $$$\ {1,2,3,4,5,6 \}$ . Ketika Anda berurusan dengan kecepatan partikel yang bergerak maka jelas Anda harus bekerja dengan seperangkat nilai yang berkelanjutan. Nilai yang keluar setelah setiap percobaan (pengukuran) akan ditunjukkan oleh $$$x_1, x_2, ...$ , tapi terkadang kita akan menggunakan huruf yang sama seperti yang kita gunakan untuk variabel acak $$$\ xi$ . Dalam kasus set nilai kontinu, variabel acak ditandai oleh fungsi kerapatan probabilitasnya $$$\ rho (x)$ . Fungsi ini menunjukkan probabilitas bahwa variabel acak jatuh ke lingkungan kecil $$$dx$ intinya $$$x$ . Seperti yang bisa kita lihat pada gambar, probabilitas ini sama dengan luas persegi panjang yang ditetaskan di bawah grafik $$$\ rho (x) dx$ .

Cukup sering dalam hidup kita, variabel acak memiliki Distribusi Gauss, ketika kepadatan probabilitasnya $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ .

Kita bisa melihat fungsinya yang berbentuk lonceng $$$\ rho (x)$ berpusat pada titik $$$\ mu$ dan lebar karakteristiknya adalah sekitar $$$\ sigma$ .
Karena kita berbicara tentang Distribusi Gaussian, maka akan menjadi dosa untuk tidak menyebutkan dari mana asalnya. Serta nomornya $$$e$ dan $$$\ pi$ secara kuat ditembus dalam matematika dan dapat ditemukan di tempat-tempat yang paling tidak terduga, sehingga Distribusi Gaussian memiliki akar yang kuat dalam teori probabilitas. Pernyataan luar biasa berikut sebagian menjelaskan kehadiran Distribusi Gauss dalam banyak proses:
Biarkan variabel acak $$$\ xi$ memiliki distribusi sewenang-wenang (sebenarnya ada beberapa pembatasan kesewenang-wenangan, tetapi mereka tidak membatasi sama sekali). Ayo tampil $$$n$ percobaan dan hitung jumlah $$$\ xi_1 + ... + \ xi_n$ , dari nilai yang jatuh. Mari kita lakukan banyak percobaan. Jelas bahwa setiap kali kita akan mendapatkan nilai penjumlahan yang berbeda. Dengan kata lain, jumlah ini adalah variabel acak dengan hukum distribusinya sendiri. Ternyata itu cukup besar $$$n$ , hukum distribusi jumlah ini cenderung ke Distribusi Gaussian (omong-omong, lebar karakteristik bel tumbuh seperti $$$\ sqrt n$ . Baca lebih lanjut di Wikipedia: teorema limit pusat . Dalam kehidupan nyata ada banyak nilai yang merupakan jumlah dari sejumlah besar variabel acak independen dan terdistribusi secara identik. Jadi nilai-nilai ini memiliki Distribusi Gauss.

Nilai yang berarti

Menurut definisi, nilai rata-rata dari variabel acak adalah nilai yang kita dapatkan dalam batas jika kita melakukan lebih banyak eksperimen dan menghitung rata-rata nilai yang jatuh. Nilai rata-rata dilambangkan dengan berbagai cara: matematikawan dilambangkan dengan $$$E \ xi$ (Ekspektasi), fisikawan menyatakannya oleh $$$\ overline {\ xi}$ atau $$$<\ xi>$ . Kami akan menyatakannya seperti yang dilakukan para ahli matematika.

Misalnya, nilai rata-rata Distribusi Gaussian $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ sama dengan $$$\ mu$ .

Varians

Untuk distribusi Gaussian, kita melihat dengan jelas bahwa variabel acak cenderung jatuh dalam wilayah tertentu dari nilai rata-rata $$$\ mu$ . Mari kita nikmati distribusi Gaussian sekali lagi:

Pada gambar, orang dapat melihat bahwa lebar karakteristik suatu wilayah di mana nilai sebagian besar jatuh adalah $$$\ sigma$ . Bagaimana kita bisa memperkirakan lebar ini untuk variabel acak acak? Kita dapat menggambar grafik fungsi kerapatan probabilitasnya dan hanya mengevaluasi rentang karakteristik secara visual. Namun akan lebih baik untuk memilih cara aljabar yang tepat untuk evaluasi ini. Kami mungkin menemukan panjang penyimpangan rata-rata dari nilai rata-rata: $$$E | \ xi-E \ xi |$ . Nilai ini merupakan estimasi yang baik dari penyimpangan karakteristik $$$\ xi$ . Namun kami tahu betul, betapa problematisnya untuk menggunakan nilai absolut dalam rumus, sehingga rumus ini jarang digunakan dalam praktik.

Pendekatan yang lebih sederhana (sederhana dari sudut pandang perhitungan) adalah menghitung $$$E (\ xi-E \ xi) ^ 2$ .

Nilai ini disebut varians dan dilambangkan dengan $$$\ sigma_ \ xi ^ 2$ . Akar kuadrat dari varian adalah estimasi yang baik dari penyimpangan karakteristik variabel acak. Ini disebut standar deviasi.

Sebagai contoh, seseorang dapat menghitungnya untuk distribusi Gaussian $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ varians sama dengan $$$\ sigma ^ 2$ dengan demikian simpangan baku adalah $$$\ sigma$ . Hasil ini benar-benar sesuai dengan intuisi geometris kami. Sebenarnya kecurangan kecil disembunyikan di sini. Sebenarnya dalam definisi distribusi Gauss Anda melihat nomornya $$$2$ dalam penyebut ekspresi $$$- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}$ . Ini $$$2$ berdiri di sana dengan sengaja, untuk deviasi standar $$$\ sigma_ \ xi$ sama persis dengan $$$\ sigma$ . Jadi rumus distribusi Gauss ditulis dengan cara, yang perlu diingat bahwa seseorang akan menghitung deviasi standarnya.

Variabel acak independen

Variabel acak dapat saling bergantung atau tidak. Bayangkan Anda melempar jarum ke lantai dan mengukur koordinat kedua ujungnya. Dua koordinat ini adalah variabel acak, tetapi mereka bergantung satu sama lain, karena jarak di antara mereka harus selalu sama dengan panjang jarum. Variabel acak independen satu sama lain jika hasil jatuh yang pertama tidak tergantung pada hasil yang kedua. Untuk dua variabel independen $$$\ xi_1$ dan $$$\ xi_2$ rata-rata produk mereka sama dengan produk rata-rata mereka: $$$E (\ xi_1 \ cdot \ xi_2) = E \ xi_1 \ cdot \ xi_2$

Filter kalman

Pernyataan problemm

Biarkan dilambangkan dengan $$$x_k$ nilai yang ingin kami ukur dan kemudian filter. Dapat berupa koordinat, kecepatan, akselerasi, kelembaban, suhu, tekanan, dll

Mari kita mulai dengan contoh sederhana, yang akan membawa kita pada perumusan masalah umum. Bayangkan Anda memiliki mobil mainan radio control yang hanya bisa berjalan maju dan mundur. Mengetahui massa, bentuk, dan parameter lain dari sistem kami telah menghitung bagaimana cara joystick pengontrol bekerja pada kecepatan mobil $$$v_k$ .


Koordinat mobil akan dengan rumus berikut

$$

$$x_ {k + 1} = x_k + v_kdt$$

Dalam kehidupan nyata kita tidak bisa, kita tidak dapat memiliki formula yang tepat untuk koordinat karena beberapa gangguan kecil yang bekerja pada mobil seperti angin, benjolan, batu di jalan, sehingga kecepatan sebenarnya dari mobil akan berbeda dari yang dihitung . Jadi kami menambahkan variabel acak $$$\ xi_k$ di sebelah kanan persamaan terakhir:

$$

$$x_ {k + 1} = x_k + v_kdt + \ xi_k$$

Kami juga memiliki sensor GPS pada mobil yang mencoba mengukur koordinat mobil $$$x_k$ . Tentu saja ada kesalahan dalam pengukuran ini, yang merupakan variabel acak $$$\ eta_k$ . Jadi dari sensor kita akan mendapatkan data yang salah:

$$

$$z_k = x_k + \ eta_k$$

Tujuan kami adalah menemukan estimasi yang baik untuk koordinat yang benar $$$x_k$ , mengetahui data sensor yang salah $$$z_k$ . Estimasi bagus ini akan kami tunjukkan dengan $$$x ^ {opt}$ .
Secara umum koordinat $$$x_k$ dapat mewakili nilai apa pun (suhu, kelembaban, ...) dan bagian pengontrol yang akan kami tunjukkan $$$u_k$ (dalam contoh dengan mobil $$$u_k = v_k \ cdot dt$ ) Persamaan untuk koordinat dan pengukuran sensor adalah sebagai berikut:

(1)

Mari kita bahas, apa yang kita ketahui dalam persamaan ini.

• $$$u_k$ adalah nilai yang diketahui yang mengendalikan evolusi sistem. Kami tahu dari model sistem.
• Variabel acak $$$\ xi$ mewakili kesalahan dalam model sistem dan variabel acak $$$\ eta$ adalah kesalahan sensor. Hukum distribusinya tidak tergantung pada waktu (pada indeks iterasi $$$k$ )
• Sarana kesalahan sama dengan nol: $$$E \ eta_k = E \ xi_k = 0$ .
• Kita mungkin tidak tahu hukum distribusi dari variabel acak, tetapi kita tahu variansnya $$$\ sigma_ \ xi$ dan $$$\ sigma_ \ eta$ . Perhatikan bahwa varians tidak bergantung pada waktu (pada $$$k$ ) karena hukum distribusi yang sesuai tidak.
• Kami menganggap bahwa semua kesalahan acak tidak saling bergantung: kesalahan pada saat itu $$$k$ tidak tergantung pada kesalahan pada saat itu $$$k $$ .

Perhatikan bahwa masalah penyaringan bukan masalah perataan. Tujuan kami bukan untuk memuluskan data sensor, kami hanya ingin mendapatkan nilai yang sedekat mungkin dengan koordinat nyata $$$x_k$ .

Algoritma Kalman

Kami akan menggunakan induksi. Bayangkan itu pada langkah $$$k$ kami telah menemukan nilai sensor yang difilter $$$x ^ {opt}$ , yang merupakan estimasi yang baik dari koordinat sebenarnya $$$x_k$ . Ingatlah bahwa kita tahu persamaan yang mengontrol koordinat nyata:

$$

$$x_ {k + 1} = x_k + u_k + \ xi_k$$

Oleh karena itu sebelum mendapatkan nilai sensor kita mungkin menyatakan bahwa itu akan menunjukkan nilai yang dekat $$$x ^ {opt} + u_k$ . Sayangnya sejauh ini kami tidak bisa mengatakan sesuatu yang lebih tepat. Tetapi pada langkah $$$k + 1$ kami akan memiliki pembacaan yang tidak tepat dari sensor $$$z_ {k + 1}$ .
Gagasan Kalman adalah sebagai berikut. Untuk mendapatkan estimasi terbaik dari koordinat nyata $$$x_ {k + 1}$ kita harus mendapatkan pertengahan emas antara pembacaan sensor yang tidak tepat $$$z_ {k + 1}$ dan $$$x ^ {opt} + u_k$ - prediksi kami, apa yang kami harapkan untuk dilihat pada sensor. Kami akan memberi bobot $$$K$ ke nilai sensor dan $$$(1-K)$ ke nilai prediksi:

$$

$$x ^ {opt} _ {k + 1} = K \ cdot z_ {k + 1} + (1-K) \ cdot (x_k ^ {opt} + u_k)$$

Koefisien $$$K$ disebut koefisien Kalman. Itu tergantung pada indeks iterasi, dan sebenarnya kita harus menulis $$$K_ {k + 1}$ . Tetapi untuk menjaga formula dalam bentuk yang bagus kami akan menghilangkan indeks $$$K$ .
Kita harus memilih koefisien Kalman dengan cara yang diperkirakan berkoordinasi $$$x ^ {opt} _ {k + 1}$ akan sedekat mungkin dengan koordinat nyata $$$x_ {k + 1}$ .
Sebagai contoh, jika kita tahu bahwa sensor kita sangat sangat presisi maka kita akan mempercayai pembacaannya dan memberinya bobot yang besar ( $$$K$ dekat dengan satu). Jika sensor sebaliknya tidak tepat sama sekali, maka kita akan mengandalkan nilai prediksi teoritis kita $$$x ^ {opt} _k + u_k$ .
Secara umum, kami harus meminimalkan kesalahan estimasi kami:

$$

$$e_ {k + 1} = x_ {k + 1} -x ^ {opt} _ {k + 1}$$

Kami menggunakan persamaan (1) (yang ada di bingkai biru), untuk menulis ulang persamaan untuk kesalahan:

$$

$$e_ {k + 1} = (1-K) (e_k + \ xi_k) - K \ eta_ {k + 1}$$

Sekarang tiba saatnya untuk berdiskusi, apa artinya ungkapan “untuk meminimalkan kesalahan”? Kita tahu bahwa kesalahan adalah variabel acak sehingga setiap kali mengambil nilai yang berbeda. Sebenarnya tidak ada jawaban unik untuk pertanyaan itu. Demikian pula halnya dengan varians dari variabel acak, ketika kami mencoba memperkirakan lebar karakteristik dari fungsi kerapatan probabilitasnya. Jadi kami akan memilih kriteria sederhana. Kami akan meminimalkan rata-rata alun-alun:

$$

$$E (e ^ 2_ {k + 1}) \ rightarrow min$$

Mari kita tulis ulang ungkapan terakhir:

$$

$$E (e ^ 2_ {k + 1}) = (1-K) ^ 2 (E_k ^ 2 + \ sigma ^ 2_ \ xi) + K ^ 2 \ sigma ^ 2_ \ eta$$

Ekspresi terakhir mengambil nilai minimalnya, ketika turunannya nol. Jadi kapan:

$$

$$\ displaystyle K_ {k + 1} = \ frac {Ee ^ 2_k + \ sigma ^ 2_ \ xi} {Ee ^ 2_k + \ sigma ^ 2_ \ xi + \ sigma ^ 2_ \ eta}$$

Di sini kita menulis koefisien Kalman dengan subskripnya, jadi kami menekankan fakta bahwa itu tergantung pada langkah iterasi. Kami mengganti dengan persamaan untuk mean square error $$$E (e ^ 2_ {k + 1})$ koefisien Kalman $$$K_ {k + 1}$ yang meminimalkan nilainya:

$$

$$\ displaystyle E (e ^ 2_ {k + 1}) = \ frac {\ sigma ^ 2_ \ eta (Ee ^ 2_k + \ sigma ^ 2_ \ xi)} {Ee ^ 2_k + \ sigma ^ 2_ \ xi + \ sigma ^ 2_ \ eta}$$

Jadi kami telah memecahkan masalah kami. Kami mendapat formula berulang untuk menghitung koefisien Kalman.

Contoh

Pada plot dari awal artikel ini ada datum yang disaring dari sensor GPS fiksi, menetap di mobil fiksi, yang bergerak dengan akselerasi konstan $$$a$ .

$$

$$x_ {t + 1} = x_t + di \ cdot dt + \ xi_t$$

Analisis

Jika dilihat bagaimana koefisien Kalman $$$K_k$ perubahan dari iterasi $$$k$ , adalah mungkin untuk melihat bahwa itu stabil ke nilai tertentu $$$K_ {stab}$ . Misalnya jika kuadrat berarti kesalahan sensor dan model saling menghormati satu sama lain sebagai sepuluh banding satu, maka plot ketergantungan koefisien Kalman dari langkah iterasi adalah sebagai berikut:


Dalam contoh berikut kita akan membahas bagaimana hal itu dapat menyederhanakan hidup kita.

Contoh kedua

Dalam praktiknya, kita hampir tidak tahu apa-apa dari model fisik apa yang kita filter. Bayangkan Anda telah memutuskan untuk memfilter pengukuran itu dari accelerometer favorit Anda. Sebenarnya Anda tidak tahu ke depan bagaimana accelerometer akan dipindahkan. Satu-satunya hal yang mungkin Anda ketahui adalah varians kesalahan sensor $$$\ sigma ^ 2_ \ eta$ . Dalam masalah yang sulit ini kita mungkin meletakkan semua informasi yang tidak diketahui dari model fisik ke variabel acak $$$\ xi_k$ :

$$

$$x_ {k + 1} = x_k + \ xi_k$$

Sebenarnya sistem semacam ini tidak memuaskan kondisi yang telah kami berikan pada variabel acak $$$\ xi_k$ . Karena ia menyimpan informasi yang tidak diketahui bagi kita fisika gerakan. Kita tidak dapat mengatakan bahwa itu adalah momen yang berbeda saat kesalahan independen satu sama lain dan artinya sama dengan nol. Dengan kata lain, ini berarti bahwa untuk situasi seperti ini teori Kalman tidak diterapkan. Tetapi bagaimanapun kita dapat mencoba menggunakan mesin teori Kalman hanya dengan memilih beberapa nilai yang masuk akal $$$\ sigma_ \ xi ^ 2$ dan $$$\ sigma_ \ eta ^ 2$ untuk mendapatkan grafik yang bagus dari datum yang difilter.
Tetapi ada cara yang lebih sederhana. Kami melihat itu dengan meningkatnya langkah $$$k$ Koefisien Kalman selalu stabil ke nilai tertentu $$$K_ {stab}$ . Jadi alih-alih menebak nilai koefisien $$$\ sigma ^ 2_ \ xi$ dan $$$\ sigma ^ 2_ \ eta$ dan menghitung koefisien Kalman $$$K_k$ dengan rumus-rumus yang sulit, kita dapat mengasumsikan bahwa koefisien ini konstan dan hanya memilih konstanta ini. Asumsi ini tidak akan banyak berpengaruh pada hasil penyaringan. Pada mulanya kita toh mesin-mesin Kalman tidak bisa diterapkan pada masalah kita dan kedua, koefisien Kalman dengan cepat stabil ke konstan. Pada akhirnya semuanya menjadi sangat sederhana. Kita tidak memerlukan hampir semua formula dari teori Kalman, kita hanya perlu memilih nilai yang masuk akal $$$K_ {stab}$ dan masukkan ke formula berulang

$$

$$x ^ {opt} _ {k + 1} = K_ {stab} \ cdot z_ {k + 1} + (1-K_ {stab}) \ cdot x_k ^ {opt}$$

Pada grafik berikutnya Anda dapat melihat filter dengan dua cara pengukuran yang berbeda dari sensor imajiner. Metode pertama adalah yang jujur, dengan semua formula dari teori Kalman. Metode kedua adalah yang disederhanakan.


Kami melihat bahwa tidak ada perbedaan besar antara dua metode ini. Ada variasi kecil di awal, ketika koefisien Kalman masih belum stabil.

Diskusi

Kita telah melihat bahwa ide utama filter Kalman adalah memilih koefisien $$$K$ dengan cara nilai yang disaring

$$

$$x ^ {opt} _ {k + 1} = Kz_ {k + 1} + (1-K) (x ^ {opt} _k + u_k)$$

rata-rata sedekat mungkin dengan koordinat sebenarnya $$$x_ {k + 1}$ . Kami melihat bahwa nilai yang difilter $$$x ^ {opt} _ {k + 1}$ adalah fungsi linear dari pengukuran sensor $$$z_ {k + 1}$ dan nilai yang difilter sebelumnya $$$x ^ {opt} _k$ . Namun nilai yang disaring sebelumnya $$$x ^ {opt} _k$ itu sendiri adalah fungsi linear dari pengukuran sensor $$$z_k$ dan nilai filter sebelumnya $$$x ^ {opt} _ {k-1}$ . Demikian seterusnya hingga akhir rantai. Jadi nilai yang difilter secara linear tergantung pada semua bacaan sensor sebelumnya:

$$

$$x ^ {opt} _ {k + 1} = \ lambda + \ lambda_0z_0 + \ ldots + \ lambda_ {k + 1} z_ {k + 1}$$

Itulah alasan mengapa filter Kalman disebut filter linear. Dimungkinkan untuk membuktikan bahwa filter Kalman adalah yang terbaik dari semua filter linier. Yang terbaik dalam arti meminimalkan rata-rata kuadrat dari kesalahan.

Kasus multidimensi

Dimungkinkan untuk menggeneralisasi semua teori Kalman ke kasus multidimensi. Rumus di sana terlihat sedikit lebih rumit tetapi gagasan untuk menurunkannya masih tetap sama seperti dalam satu dimensi. Misalnya, Dalam video yang bagus ini Anda dapat melihat contohnya.

Sastra

Artikel asli yang ditulis oleh Kalman dapat Anda unduh di sini:
http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf