Filter Kalman - Implementasi, Parameter dan Tuning

Pertama-tama, ini adalah pertama kalinya saya mencoba membuat filter Kalman.

Saya sebelumnya diposting pertanyaan berikut Menyaring kebisingan dan variasi dari nilai kecepatan pada StackOverflow yang menggambarkan latar belakang untuk posting ini. Ini adalah contoh khas dari nilai yang saya coba filter. Mereka tidak harus mengurangi yang terjadi di sini. Tetapi laju perubahan biasanya seperti ini

X ------- Y
16 --- 233.75
24 --- 234.01
26 --- 234.33
32 --- 234.12
36 --- 233.85
39 --- 233.42
47 --- 233.69
52 --- 233.68
55 --- 233.76
60 --- 232.97
66 --- 233.31
72 --- 233.99

Saya telah menerapkan Filter Kalman saya sesuai dengan tutorial ini: Filter Kalman untuk Dummies .

Implementasi saya terlihat seperti ini (kodesemu).

//Standard deviation is 0.05. Used in calculation of Kalman gain

void updateAngle(double lastAngle){
  if(firsTimeRunning==true)
     priorEstimate = 0;               //estimate is the old one here
     priorErrorVariance = 1.2;        //errorCovariance is the old one
  else
     priorEstimate = estimate;              //estimate is the old one here
     priorErrorVariance = errorCovariance;  //errorCovariance is the old one
  rawValue = lastAngle;          //lastAngle is the newest Y-value recieved
  kalmanGain = priorErrorVariance / (priorErrVariance + 0.05);
  estimate = priorEstimate + (kalmanGain * (rawValue - priorEstimate));
  errorCovariance = (1 - kalmanGain) * priorErrVariance;
  angle = estimate;              //angle is the variable I want to update
}                                //which will be lastAngle next time

Saya mulai dengan perkiraan sebelumnya sebesar 0. Ini sepertinya berfungsi dengan baik. Tetapi yang saya perhatikan adalah bahwa kalmanGain akan berkurang setiap kali pembaruan ini dijalankan, yang berarti bahwa saya mempercayai nilai-nilai baru saya semakin sedikit semakin lama filter saya dijalankan (?). Saya tidak menginginkan itu.

Saya beralih dari hanya menggunakan moving average (sederhana dan eksponensial tertimbang) untuk menggunakan ini. Saat ini saya bahkan tidak bisa mendapatkan hasil sebagus itu.

Pertanyaan saya adalah apakah ini implementasi yang tepat dan apakah varians kesalahan saya sebelumnya dan standar deviasi terlihat bagus sesuai dengan nilai sampel yang telah saya posting? Parameter saya sebenarnya hanya dipilih secara acak untuk melihat apakah saya bisa mendapatkan hasil yang baik. Saya telah mencoba beberapa rentang berbeda tetapi dengan hasil yang buruk. Jika Anda memiliki saran untuk perubahan yang dapat saya lakukan, itu akan sangat dihargai. Maaf jika ada beberapa hal yang hilang. Pertama kali memposting di sini juga.

Filter Kalman berguna ketika sinyal input Anda terdiri dari pengamatan bising dari beberapa kondisi sistem dinamika linear. Diberikan serangkaian pengamatan kondisi sistem, filter Kalman bertujuan untuk secara rekursif memberikan estimasi yang lebih baik dan lebih baik dari keadaan sistem yang mendasarinya. Agar dapat menerapkannya dengan sukses, Anda harus memiliki model untuk dinamika sistem yang keadaannya Anda perkirakan. Seperti yang dijelaskan secara terperinci di Wikipedia , model ini menjelaskan bagaimana sistem yang mendasari keadaan diharapkan berubah selama satu langkah waktu, mengingat status sebelumnya, input apa pun ke sistem, dan komponen stokastik yang didistribusikan Gaussian yang disebut proses noise.

Dengan itu, tidak jelas dari pertanyaan Anda apakah Anda memiliki model yang mendasarinya. Pos yang ditautkan menunjukkan bahwa Anda menggunakan nilai kecepatan dari sensor. Ini dapat dimodelkan sebagai pengamatan langsung dari keadaan sistem (di mana negara adalah kecepatannya), atau pengamatan tidak langsung dari keadaannya (di mana negara adalah posisinya, misalnya). Tetapi, untuk menggunakan kerangka kerja Kalman, Anda harus memilih model untuk bagaimana negara itu diharapkan berkembang seiring berjalannya waktu; informasi tambahan ini digunakan untuk menghasilkan estimasi optimal. Filter Kalman bukanlah kotak hitam ajaib yang hanya akan "membersihkan" sinyal yang diterapkan padanya.

Dengan itu, fenomena yang Anda singgung, di mana filter Kalman akan menjadi semakin percaya diri dalam outputnya sendiri ke titik di mana pengamatan input menjadi semakin diabaikan, memang terjadi dalam praktik. Ini dapat dikurangi dengan secara manual meningkatkan nilai-nilai dalam matriks kovarians proses kebisingan. Kemudian, secara kualitatif, model untuk transisi keadaan sistem mengandung komponen stokastik yang lebih besar, sehingga kemampuan estimator untuk secara akurat memprediksi keadaan berikutnya mengingat keadaan saat ini berkurang. Ini akan mengurangi ketergantungannya pada perkiraan saat ini dari keadaan sistem dan meningkatkan ketergantungannya pada pengamatan selanjutnya, mencegah efek "mengabaikan input".

Jika saya memahaminya dengan benar, Anda memiliki sesuatu yang bergerak dan Anda dapat mengamati kecepatan dan kecepatan ini berisik. Dari pengukuran Anda, Anda mengamati 2 macam variasi. \

1. Variasi disebabkan oleh kebisingan
2. Variasi karena objek benar-benar mengubah kecepatan (misalnya berputar)

Alasan kenaikan Kalman Anda menjadi nol adalah karena Anda secara implisit berasumsi bahwa kecepatan objek konstan dan yang perlu Anda lakukan adalah memperkirakan kecepatan sebenarnya ini.

" Hei, aku punya objek yang bergerak dengan kecepatan konstan dan aku ingin memperkirakan kecepatan konstan ini "

$x_k$$k$$y_k$

$$x_k = x_{k-1}$$ $$y_k = x_k + q_k$$

Tapi objekmu tidak bergerak seperti itu. Kecepatannya berubah dan Anda tidak tahu bagaimana dan kapan itu akan berubah.

Yang harus Anda katakan adalah:

" Hei, aku punya benda yang bergerak dengan kecepatan tapi aku tidak yakin bagaimana itu mengubah kecepatannya "

Ada banyak cara Anda bisa melakukan ini: Cara paling sederhana adalah menambahkan ketidakpastian keadaan Anda.

$$x_{k} = x_{k-1} + \underbrace{v_{k-1}}_{\text{you add uncertainty}}$$ $$y_k = x_k + q_k$$ $q_k$$v_k$

Persamaan Filter Kalman Anda akan terlihat seperti ini:

$$\hat y_{k|k-1} = \hat x_{k|k-1}$$ $$K_{k} = \frac{P_{k|k-1}}{P_{k|k-1} + Q^o}$$ $$\hat x_{k|k}= \hat x_{k|k-1} + K_k(y_k - \hat y_{k|k-1})$$ $$P_{k|k} = P_{k|k-1} - K_kP_{k|k-1}$$ $$P_{k+1|k} =P_{k|k} + Q^s$$

0.05$Q^o$$Q^s$

Dalam kode Anda sedikit modifikasi adalah:

stateVariance = 0.5

errorCovariance = (1 - kalmanGain) * priorErrVariance + stateVariance;

stateVariance$Q^s$

stateVarianceNilai ini bisa apa saja yang Anda inginkan. Ini didasarkan pada kepercayaan diri Anda pada seberapa banyak kecepatan sebenarnya akan berubah. Jika menurut Anda kecepatannya akan tetap konstan, tetapkan ini ke angka kecil.

Dengan cara ini, Keuntungan Kalman Anda tidak akan menjadi nol.

Anda memerlukan sistem dinamis untuk menggunakan Filter Kalman.

saya akan menyarankan

$$y = \sum\limits_{i =0}^n a_i\, x^i$$

$$a[k+1] = a[k] + w$$ $$cov(w) = Q$$ $$z = \sum\limits_{i =0}^n a_i\, x^i = y$$

$x$$a$

Saya pikir Anda bisa menggunakan beberapa ide dari teori kontrol klasik, misalnya kontroler PID .

Sinyal Y Anda dapat menjadi setpoint controller u (t). Pabrik proses hanya 1 dan y (t) akan difilter output. Yang harus Anda lakukan adalah mengatur parameter (tune) P, I dan D untuk mendapatkan yang Anda inginkan.

Output y (t) akan mencoba "mengikuti" input u (t), tetapi parameter mengontrol bagaimana pelacakan ini nantinya.

Gain diferensial D akan membuat respons Anda sensitif terhadap perubahan kesalahan yang cepat. Dalam kasus Anda, saya pikir D harus kecil. Anda tidak ingin y (t) berubah jika u (t) tiba-tiba berubah.

Gain integral 'I' akan membuat respons Anda sensitif terhadap akumulasi kesalahan. Anda harus memberi nilai tinggi di sana. Jika u (t) berubah level dan menyimpannya di sana, kesalahan akan meningkat dan kemudian Anda ingin y (t) melakukan hal yang sama.

Keuntungan P dapat memberikan nada yang bagus. Pokoknya, cobalah bermain dengan parameter dan lihat apa yang Anda dapatkan.

Ada metode penyetelan yang rumit, tetapi saya tidak yakin Anda akan membutuhkannya.

Semoga berhasil.

Berikut ini adalah implementasi sederhana dan bersih dari Filter Kalman yang mengikuti notasi seperti pada halaman Wikipedia. https://github.com/zziz/kalman-filter