🥗 📧 📗 Pengalaman dengan filter Kalman pada contoh data NMEA 🏀 🕺🏼 😕

Tujuan artikel ini bukan untuk menjelaskan prinsip-prinsip filter Kalman, tetapi untuk menunjukkannya menggunakan data nyata (mentah) sebagai contoh. Mereka yang ingin dapat memodifikasi kode sumber dan bereksperimen dengan algoritma, saya berharap pekerjaan saya akan membantu mereka yang dihadapkan dengan tugas yang sama.

Data yang digunakan adalah dari penerima GPS dalam format NMEA-0183, khususnya pesan GGA dan VTG.

Penyaringan diperlukan karena kebisingan GPS. Penyebab gangguan pada data GPS berbeda. Yang utama:

atmosfer.
hambatan pada sinyal.
Posisi orbit GPS. Misalnya, kecenderungan rendah orbit GPS (sekitar 55 °) secara serius merusak akurasi di wilayah sirkumpolar Bumi.

Semua ini secara total mengarah pada lompatan pada posisi, offset, dan masalah lainnya. Dan dalam pekerjaan, pertama-tama, saya perlu mendapatkan kecepatan yang disaring.

Faktanya adalah bahwa kecepatan yang diukur oleh peralatan dan ditransmisikan dalam pesan VTG memberikan pembacaan yang tidak masuk akal (melompat, dll), yang sangat rumit dalam tugas kontrol.
Oleh karena itu, diputuskan untuk membangun model filter dalam Oktaf, dan setelah menerima kecepatan sebagai turunan dari data GPS yang diwakili oleh pesan GGA, bandingkan dengan data kecepatan asli dari pesan VTG.

Untuk memudahkan perbandingan, data harus ditampilkan pada satu grafik.

Filter Kalman sangat baik dalam memfilter data dan turunannya.

«».

— . ( ) : - - . , Predict, Update, Invariant :

https://habr.com/ru/post/140274/
https://habr.com/ru/post/166693/
https://ru.wikipedia.org/wiki/%D0%A4%D0%B8%D0%BB%D1%8C%D1%82%D1%80_%D0%9A%D0%B0%D0%BB%D0%BC%D0%B0%D0%BD%D0%B0

Octave (open-source Matlab Linux). F . ( ):

, .
— (PAC ) C++.

— Octave .

++ , , Octave — .

GGA :

VTG :

«--» — , . Latitude Longitude.

, .

:
Fk — //, state-transition model.
Hk — /, observation model.
Qk — , covariance of the process noise;.
Rk — , covariance of the observation noise.
K — .
Xp — () .
Xk — .
X(k-1) — .
Pp — , .
P — .
P(k-1) — () .
z- () .

:
Xp = Fk X(k-1); .
Pp = Fk P(k-1) * F'k + Q; .

:
K = Pp H' inv(H Pp H' + R); .
Xk = Xp + K(z — HXp); z.
P = Pp — KHPp; .

KalmanX KalmanY .

X Y , , Q, R, F reset ( Q R, ).

(habrGGA.m) , , .

Q, R . ( Q, R). , .

, :

( ) , Q. R = 0,2.

, :

— X Y (Latitude Longitude) , :

( VTG) , .

Q R, (habrGGA.m):

 if j==1   Q = [ 0.01 0;
                  0   0.01];
            Q1=Q;     
            R=0.2; R1=R;
  elseif j==2   Q = [ 0.05 0;
                      0   0.05]; 
                      Q2=Q;
            R=0.2; R1=R;
  elseif j==3    Q = [1 0;
                       0  1];
                       Q3=Q;
            R=0.2; R1=R;
  end

Untuk tujuan saya, nilai Q rata-rata (0,05) sekarang tampaknya menjadi yang paling disukai pada kit NMEA ini. Seleksi koefisien yang lebih akurat, dan mungkin secara dinamis akan diimplementasikan selama debugging pada uji tambat.

File-file terlampir.
habrGGA.m - skrip utama (membaca data, memanggil fungsi filter, merencanakan grafik)
KalmanX.m - filter Kalman (Latitude)
KalmanY.m - Filter Kalman (Longitude)

Pengalaman dengan filter Kalman pada contoh data NMEA

More articles: