Filter Kalman (FC) adalah algoritma linier yang optimal untuk memfilter parameter sistem linear dinamis dengan adanya pengamatan yang tidak lengkap dan berisik. Filter ini banyak digunakan dalam sistem kontrol teknis untuk menilai dinamika perubahan dalam situasi ekonomi makro atau opini publik.
Artikel ini bertujuan untuk memperkenalkan pembaca dengan pendekatan standar untuk transisi dari model kontinu sistem dinamis yang dijelaskan oleh sistem persamaan diferensial linear sewenang-wenang ke model diskrit.
Teks tersembunyiserta menghemat waktu pembaca dengan menghilangkan upaya untuk menciptakan sepeda dan mengekspos diri kepada rekan kerja dalam cahaya yang buruk. Jangan seperti penulis
Artikel ini juga dimaksudkan untuk mendorong pembaca untuk menggunakan FC dalam masalah-masalah tersebut di mana pada pandangan pertama kelihatannya FC linear tidak berlaku, tetapi pada kenyataannya mungkin tidak demikian.
Untuk menulis artikel oleh penulis, fakta bahwa meskipun kesederhanaan hal-hal berikut dalam hasil pencarian untuk Google baik dalam bahasa Rusia dan bahasa Inggris (setidaknya di halaman pertama), penulis tidak dapat menemukannya.
Model dinamis untuk filter Kalman diskrit
Teks tersembunyiPada dasarnya, bagian ini diperlukan untuk memperkenalkan pembaca pada sistem notasi yang diterima, yang sangat berbeda dari buku ke buku dan dari artikel ke artikel. Penjelasan tentang arti semua jumlah yang termasuk dalam persamaan berada di luar cakupan artikel ini, sementara itu dipahami bahwa orang yang datang ke cahaya memiliki beberapa gagasan tentang hal ini. Jika tidak, selamat datang di
sini , di
sini dan di
sini .
FC dapat dilakukan dalam bentuk diskrit dan kontinu. Yang menarik dari sudut pandang implementasi praktis pada komputer digital modern adalah FC diskrit yang akan ditekankan dalam artikel ini.
Linear discrete FC dijelaskan oleh ekspresi berikut. Biarkan model sistem direpresentasikan sebagai berikut:
$$ menampilkan $$ \ mathbf {x} _ {k} = F \ mathbf {x} _ {k-1} + \ Psi \ mathbf {u} _k + \ Gamma \ mathbf {w} _k $$ tampilan $$
dimana $ inline $ f $ inline $ - matriks transisi, $ inline $ \ Psi $ inline $ - matriks kontrol transisi, $ inline $ \ Gamma $ inline $ - matriks perturbasi transisi, $ inline $ \ mathbf {x} _k $ inline $ , $ inline $ \ mathbf {u} _k $ inline $ , $ inline $ \ mathbf {w} _k $ inline $ - menyatakan vektor, kontrol dan kebisingan (gangguan) sistem menyala $ inline $ k $ inline $ langkah itu. Model Pengamatan:$$ menampilkan $$ \ mathbf {z} _k = H \ mathbf {x} _k + \ mathbf {n} _k $$ menampilkan $$
dimana $ inline $ \ mathbf {z} _k $ inline $ , $ inline $ \ mathbf {n} _k $ inline $ - vektor pengamatan dan kebisingan pengamatan pada $ inline $ k $ inline $ langkah itu. 5 persamaan kerja FC dalam artikel ini tidak menarik, oleh karena itu, jika ada yang membutuhkannya mereka diberikan di bawah spoiler.
Teks tersembunyiTahap pertama, ekstrapolasi:
$$ menampilkan $$ \ mathbf {x} _ {k | k-1} = F \ topi {\ mathbf {x}} _ {k-1} + \ mathbf {w} _k $$ menampilkan $$
$$ menampilkan $$ P_ {k | k-1} = FP_ {k-1} F ^ T + Q_k $$ menampilkan $$
Tahap ini disebut ekstrapolasi. Langkah selanjutnya, yang disebut koreksi:
$$ menampilkan $$ K = PH ^ T (HP_ {k | k-1} H ^ T + R) ^ {- 1} $$ menampilkan $$
penilaian itu sendiri
$$ menampilkan $$ \ hat {\ mathbf {x}} _ {k} = x_ {k | k-1} + K (H \ mathbf {z} _k- \ mathbf {x} _ {k | k-1 }) $$ menampilkan $$
$$ menampilkan $$ P_k = (E-KH) P_ {k | k-1} $$ menampilkan $$
Selanjutnya, kita berbicara tentang sistem stasioner (dengan koefisien konstan) yang menjadi matriksnya $ inline $ f $ inline $ , $ inline $ \ Psi $ inline $ dan $ inline $ \ Gamma $ inline $ terlepas dari angka $ inline $ k $ inline $ .
Model sistem dinamis berkelanjutan. Ruang negara.
Dalam sebagian besar aplikasi praktis, FC menyaring parameter sistem dinamis berkelanjutan yang dijelaskan oleh persamaan diferensial untuk waktu kontinu. Perhitungan FC dalam hal ini terjadi pada komputer digital, yang secara otomatis membuat FC diskrit dan modelnya harus diskrit. Untuk mendapatkan model diskrit dari sistem kontinu ini, pertama-tama perlu untuk menyusun vektor keadaan itu sendiri (vektor fase), sistem persamaan keadaan, kemudian mendiskritisasi mereka, dengan demikian memperoleh matriks $ inline $ f $ inline $ , $ inline $ \ Psi $ inline $ dan $ inline $ \ Gamma $ inline $ .
Biarkan perilaku sistem dijelaskan oleh satu set $ inline $ n $ inline $ persamaan diferensial dari urutan pertama:$$ menampilkan $$ \ dot {\ mathbf {x}} (t) = A \ mathbf {x} (t) + B \ mathbf {u} (t) + G \ mathbf {w} (t) $$ display $$
disini $ inline $ \ mathbf {x} $ inline $ - $ inline $ n $ inline $ Vektor keadaan -dimensi sistem. Vektor keadaan (alias fase vektor) adalah vektor yang berisi variabel yang menggambarkan sistem dan turunannya hingga urutan yang diperlukan. $ inline $ \ mathbf {u} $ inline $ - $ inline $ r $ inline $ -dimensi kontrol vektor sistem yang menggambarkan efek terkontrol yang diberikan pada sistem.
$ inline $ \ mathbf {w} $ inline $ $ inline $ p $ inline $ -dimensi vektor yang berisi efek acak yang tidak terkendali pada sistem, atau noise. $ inline $ a $ inline $ - matriks ukuran status sistem $ inline $ n \ kali n $ inline $ . $ inline $ B $ inline $ - Matriks kontrol ukuran $ inline $ n \ kali r $ inline $ . $ inline $ g $ inline $ - matriks perturbasi ukuran $ inline $ n \ kali p $ inline $ . Dalam ungkapan ini, semua produk dihitung sesuai dengan aturan perkalian matriks. Dalam kasus umum, elemen-elemen dari semua matriks adalah fungsi waktu, namun, hanya sistem stasioner yang dipertimbangkan dalam artikel, di mana elemen-elemennya independen terhadap waktu.
Contoh transisi dari deskripsi sistem menggunakan persamaan diferensial orde tinggi ke deskripsi melalui ruang keadaan diberikan di bawah ini.
ContohBiarkan gerakan suatu titik di sepanjang beberapa sumbu
$ inline $ ox $ inline $ dijelaskan oleh persamaan diferensial orde kedua:
$$ menampilkan $$ \ ddot {x} = - \ omega ^ 2 x $$ menampilkan $$
Jika seseorang tidak ingat, gerakan osilasi dengan demikian diwakili. Kami pindah dari persamaan orde kedua ke sistem dua persamaan dengan memperkenalkan variabel baru
$ inline $ x_1 = \ dot {x} $ inline $ . Sekarang kita memiliki:
$$ display $$ \ begin {aligned} \ dot {x} & = x_1 \\ \ dot {x} _1 & = - \ omega ^ 2 x \ end {aligned} $$ display $$
Sistem persamaan ini dapat ditulis dalam bentuk matriks, sedangkan vektor keadaan
$ inline $ \ mathbf {x} = [x \, x_1] ^ T $ inline $ , matriks negara akan
$$ tampilkan $$ A = \ mulai {bmatrix} 0 & 1 \\ - \ omega ^ 2 & 0 \ end {bmatrix} $$ tampilkan $$
Variabel dimasukkan
$ inline $ x_1 $ inline $ memainkan peran kecepatan. Matriks
$ inline $ B $ inline $ dan
$ inline $ g $ inline $ dalam contoh ini, mereka nol, karena tidak ada kontrol dan pengaruh yang mengganggu.
Transisi terpisah
Untuk transisi yang benar ke daerah diskrit (dengan kata lain, diskritisasi model), kita perlu memperkenalkan konsep eksponen matriks . Eksponen matriks adalah fungsi matriks yang diperoleh dengan analogi dengan perluasan fungsi eksponensial dalam deret Taylor sebenarnya dari Maclaurin :
$$ menampilkan $$ e ^ {At} = E + Pada + \, ... \, \ dfrac {A ^ nt ^ n} {n!} + \, ... \, = \ sum_ {k = 0 } ^ {\ infty} \ dfrac {A ^ nt ^ n} {n!} $$ menampilkan $$
di mana di bawah $ inline $ E $ inline $ menyiratkan matriks satuan.
Transisi yang tepat dari model kontinu dalam ruang keadaan ke model diskrit membutuhkan pencarian solusi untuk sistem yang homogen $ inline $ \ dot {\ mathbf {x}} (t) = A (t) \ mathbf {x} (t) $ inline $ , kemudian transisi ke sistem asli, menemukan solusi umum dan integrasi dari saat awal $ inline $ t_0 $ inline $ untuk beberapa orang $ inline $ t $ inline $ . Kesimpulan yang ketat dapat ditemukan di [1], di sini, hasil akhir disajikan.
Dalam kasus stasioneritas model dinamis kontinu (independen dari matriks $ inline $ a $ inline $ , $ inline $ B $ inline $ , $ inline $ g $ inline $ dari waktu) untuk mendapatkan model diskrit, kita dapat memperkenalkan matriks transisi bantu sistem $ inline $ \ Phi (t, \ tau) $ inline $ dari saat ini $ inline $ \ tau $ inline $ saat ini $ inline $ t $ inline $ dimana $ inline $ t> \ tau $ inline $ :$$ menampilkan $$ \ Phi (t, \ tau) = e ^ {A (t- \ tau)} = \ sum_ {k = 0} ^ {\ infty} \ dfrac {A ^ nt ^ n} {n! } $$ tampilkan $$
Selanjutnya, dengan menggunakan matriks bantu ini, matriks yang diperlukan untuk model diskrit dapat diperoleh:$$ menampilkan $$ F = \ Phi (t + T, t) = e ^ {AT} = E + AT + \ dfrac {A ^ 2T ^ 2} {2!} + \ dfrac {A ^ 3T ^ 3} {3!} + ... $$ tampilkan $$
$$ menampilkan $$ \ Gamma = \ int_ {kT} ^ {(k + 1) T} \ Phi (t_ {k + 1}, \ tau) G (\ tau) d \ tau $$ menampilkan $$
$$ menampilkan $$ \ Psi = \ int_ {kT} ^ {(k + 1) T} \ Phi (t_ {k + 1}, \ tau) B (\ tau) d \ tau $$ menampilkan $$
Di sini di bawah $ inline $ B (\ tau) $ inline $ dan $ inline $ G (\ tau) $ inline $ maksud kami matriks dari persamaan kontinu, oleh $ inline $ \ Psi $ inline $ dan $ inline $ \ Gamma $ inline $ diperlukan matriks model diskrit.
Contoh-contoh praktis
Teks tersembunyiSayangnya, contoh-contoh hanya akan memiliki penyimpangan dengan matriks $ inline $ f $ inline $ , karena penulis terlalu malas untuk menemukan contoh dengan tindakan kontrol, dan secara umum, sebagai bagian dari disertasinya, ia berurusan dengan masalah navigasi di mana tidak ada tindakan kontrol. Selain itu, dengan pengetahuan dasar analisis matematika, setelah menguraikan contoh, tindakan ini seharusnya tidak menimbulkan masalah. Untuk contoh bukan nol $ inline $ \ Gamma $ inline $ dan $ inline $ \ Psi $ inline $ bisa ke [2].
Untuk menggambarkan matematika di atas, perhatikan dua contoh. Salah satunya adalah pemanasan, dan yang kedua adalah ilustratif, untuk menunjukkan kemampuan metode yang dijelaskan.
Sepele
Biarkan objek bergerak di sepanjang sumbu $ inline $ ox $ inline $ dengan kecepatan awal $ inline $ v_0 $ inline $ dan akselerasi yang konstan $ inline $ a $ inline $ . Maka modelnya dapat direpresentasikan sebagai:
$$ tampilan $$ \ ddot {x} = tampilan $$ $$
Kami mewakili model ini dalam bentuk sistem persamaan diferensial homogen. Untuk melakukan ini, kami membagi persamaan menjadi sistem tiga kontrol jarak jauh:$$ menampilkan $$ \ mulai {sejajar} \ dot {x} & = v_x \\ \ dot {v} _x & = a_x \\ \ dot {a} _x & = 0 \ end {sejajar} $$ tampilan $$
Saat menulis sistem persamaan, turunan berikut ditambahkan di sana sejauh ini diperlukan untuk menghitung yang sekarang. Kemudian di sistem saat ini Anda tidak bisa berhenti di $ inline $ v_x $ inline $ , karena perhitungan membutuhkan $ inline $ a_x $ inline $ . Sekaligus menghitung $ inline $ a_x $ inline $ turunan $ inline $ \ dot {a} _x $ inline $ Oleh karena itu, tidak diperlukan untuk memperkenalkan turunan dari pesanan di atas $ inline $ a_x $ inline $ dalam keadaan vektor tidak masuk akal.
Gabungkan tiga variabel menjadi vektor keadaan $ inline $ \ mathbf {x} = [x \, v_x \, a_x] ^ T $ inline $ dan menulis sistem persamaan dalam bentuk matriks untuk transisi ke bentuk ruang keadaan:$$ menampilkan $$ \ dot {\ mathbf {x}} = A \ mathbf {x} $$ menampilkan $$
dimana matriksnya
$$ tampilkan $$ A = \ mulai {bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \ akhir {bmatrix} $$ tampilkan $$
Sekarang kita dapat menghitung matriks transisi dari sistem dinamis diskrit sesuai dengan kontinu yang sedang dipertimbangkan:
$ inline $ \ begin {aligned} F = E + A \ cdot T + A \ kali A \ cdot \ dfrac {T ^ 2} {2} = \ begin {bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \ end {bmatrix} + \ begin {bmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \ end {bmatrix} \ cdot T + \\ \ begin {bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \ end {bmatrix} \ cdot \ dfrac {T ^ 2} {2} = \ begin {bmatrix} 1 & T & T ^ 2/2 \\ 0 & 1 & T \\ 0 & 0 & 1 \ end {bmatrix} \ end {aligned} $ inline $
Pembaca dapat memverifikasi itu $ inline $ A ^ 3 $ inline $ dan di atas adalah matriks nol.
Dengan cara ini, matriks transisi yang diketahui semua orang diperoleh, yang disimpulkan tanpa menerapkan asumsi apa pun.
Contoh nontrivial
Kita berasumsi bahwa objek kita bergerak dalam ruang tiga dimensi dengan kecepatan linear konstan (modulo) tertentu dan dengan kecepatan sudut yang diwakili oleh pseudovector:$$ menampilkan $$ \ omega = [\ omega_x \, \ omega_y \, \ omega_z] ^ T $$ menampilkan $$
Pertama, Anda perlu membuat persamaan ruang negara. Kami menulis akselerasi saat bergerak di sekitar lingkaran. Dari kursus fisika selama 1 semester, diketahui bahwa percepatan centripetal adalah produk vektor dari kecepatan sudut dan linier:$$ menampilkan $$ \ dot {v} = \ omega \ kali v $$ menampilkan $$
Di sini vektor kecepatannya $ inline $ v = [v_x \, v_y \, v_z] ^ T $ inline $ .
Kami akan menulis produk vektor secara lebih rinci:
$$ menampilkan $$ \ omega \ kali v = \ mulai {bmatrix} \ omega_x \\ \ omega_y \\ \ omega_z \ end {bmatrix} \ kali \ begin {bmatrix} v_x \\ v_y \\ v_z \ end {bmatrix} = \ mulai {bmatrix} \ omega_yz- \ omega_zy \\ \ omega_zx- \ omega_xz \\ \ omega_xy- \ omega_yx \ end {bmatrix} $$ tampilkan $$
Sekarang kita menulis sistem persamaan
$$ menampilkan $$ \ begin {aligned} \ dot {x} & = v_x \\ \ dot {y} & = v_y \\ \ dot {z} & = v_z \\ \ dot {v} _x & = \ omega_yz - \ omega_zy \\ \ dot {v} _y & = \ omega_zx- \ omega_xz \\ \ dot {v} _z & = \ omega_xy- \ omega_yx \ end {sejajar} $$ tampilkan $$ menampilkan $$
Setelah transisi ke bentuk matriks, matriks $ inline $ a $ inline $ akan:
$$ menampilkan $$ A = \ mulai {bmatrix} 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & \ 0 & 0 & 0 & 0 & - \ omega_z & \ omega_y \\ 0 & 0 & 0 & \ omega_z & 0 & - \ omega_x \\ 0 & 0 & 0 & - \ omega_y & \ omega_x & 0 \ end { bmatrix} $$ tampilkan $$
Selanjutnya, kita lanjutkan ke matriks $ inline $ f $ inline $ oleh ekspresi yang sesuai. Karena secara lisan gandakan matriks berdasarkan ukuran $ inline $ 6 \ kali 6 $ inline $ tiga kali cukup sulit, probabilitas kesalahan tinggi, dan ini bukan masalah kerajaan, maka kita akan menulis skrip menggunakan pustaka Python sympy:
from sympy import symbols, Matrix, eye x, y, z, T = symbols('xyz T') vx, vy, vz = symbols('v_x v_y v_z') wx, wy, wz = symbols('w_x w_y w_z') A = Matrix([ [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, 0, -wz, wy], [0, 0, 0, wz, 0, -wx], [0, 0, 0, -wy, wx, 0] ]) F = eye(6) + A*T + A*A*T**2/2 from sympy import latex print(latex(F))
Dan menjalankannya, kita mendapatkan sesuatu seperti ini:
Teks tersembunyi \left[\begin{matrix}1 & 0 & 0 & T & - \frac{T^{2} w_{z}}{2} & \frac{T^{2} w_{y}}{2}\\0 & 1 & 0 & \frac{T^{2} w_{z}}{2} & T & - \frac{T^{2} w_{x}}{2}\\0 & 0 & 1 & - \frac{T^{2} w_{y}}{2} & \frac{T^{2} w_{x}}{2} & T\\0 & 0 & 0 & \frac{T^{2} \left(- w_{y}^{2} - w_{z}^{2}\right)}{2} + 1 & \frac{T^{2} w_{x} w_{y}}{2} - T w_{z} & \frac{T^{2} w_{x} w_{z}}{2} + T w_{y}\\0 & 0 & 0 & \frac{T^{2} w_{x} w_{y}}{2} + T w_{z} & \frac{T^{2} \left(- w_{x}^{2} - w_{z}^{2}\right)}{2} + 1 & \frac{T^{2} w_{y} w_{z}}{2} - T w_{x}\\0 & 0 & 0 & \frac{T^{2} w_{x} w_{z}}{2} - T w_{y} & \frac{T^{2} w_{y} w_{z}}{2} + T w_{x} & \frac{T^{2} \left(- w_{x}^{2} - w_{y}^{2}\right)}{2} + 1\end{matrix}\right]
Bahwa setelah membingkai dengan tag yang sesuai dan menempel ke kode sumber artikel berubah menjadi:$$ menampilkan $$ F = \ kiri [\ mulai {matrix} 1 & 0 & 0 & T & - \ frac {T ^ {2} w_ {z}} {2} & \ frac {T ^ {2} w_ {y}} {2} \\ 0 & 1 & 0 & \ frac {T ^ {2} w_ {z}} {2} & T & - \ frac {T ^ {2} w_ {x}} {2 } \\ 0 & 0 & 1 & - \ frac {T ^ {2} w_ {y}} {2} & \ frac {T ^ {2} w_ {x}} {2} & T \\ 0 & 0 & 0 & \ frac {T ^ {2} \ kiri (- w_ {y} ^ {2} - w_ {z} ^ {2} \ kanan)} {2} + 1 & \ frac {T ^ {2} w_ {x} w_ {y}} {2} - T w_ {z} & \ frac {T ^ {2} w_ {x} w_ {z}} {2} + T w_ {y} \\ 0 & 0 & 0 & \ frac {T ^ {2} w_ {x} w_ {y}} {2} + T w_ {z} & \ frac {T ^ {2} \ kiri (- w_ {x} ^ {2} - w_ {z} ^ {2} \ kanan)} {2} + 1 & \ frac {T ^ {2} w_ {y} w_ {z}} {2} - T w_ {x} \\ 0 & 0 & 0 & \ frac {T ^ {2} w_ {x} w_ {z}} {2} - T w_ {y} & \ frac {T ^ {2} w_ {y} w_ {z}} {2} + T w_ {x} & \ frac {T ^ {2} \ kiri (- w_ {x} ^ {2} - w_ {y} ^ {2} \ kanan)} {2} + 1 \ end {matrix} \ kanan] $$ menampilkan $$
Dengan demikian, matriks transisi filter Kalman untuk gerakan melingkar dapat diturunkan.
Berbeda dengan kasus sebelumnya, hasil konstruksi $ inline $ a $ inline $ untuk kekuatan yang lebih tinggi dari 3 bukan matriks nol.
mis. <math> $ inline $ A ^ 3 $ inline $ </math>$$ menampilkan $$ \ kiri [\ mulai {matrix} 0 & 0 & 0 & - w_ {y} ^ {2} - w_ {z} ^ {2} & w_ {x} w_ {y} & w_ {x } w_ {z} \\ 0 & 0 & 0 & w_ {x} w_ {y} & - w_ {x} ^ {2} - w_ {z} ^ {2} & w_ {y} w_ {z} \ \ 0 & 0 & 0 & w_ {x} w_ {z} & w_ {y} w_ {z} & - w_ {x} ^ {2} - w_ {y} ^ {2} \\ 0 & 0 & 0 & 0 & w_ {x} ^ {2} w_ {z} - w_ {z} \ kiri (- w_ {y} ^ {2} - w_ {z} ^ {2} \ kanan) & - w_ {x} ^ {2} w_ {y} + w_ {y} \ kiri (- w_ {y} ^ {2} - w_ {z} ^ {2} \ kanan) \\ 0 & 0 & 0 & 0 & - w_ {y} ^ {2} w_ {z} + w_ {z} \ kiri (- w_ {x} ^ {2} - w_ {z} ^ {2} \ kanan) & 0 & w_ {x} w_ {y} ^ { 2} - w_ {x} \ kiri (- w_ {x} ^ {2} - w_ {z} ^ {2} \ kanan) \\ 0 & 0 & 0 & 0 & w_ {y} w_ {z} ^ {2 } - w_ {y} \ kiri (- w_ {x} ^ {2} - w_ {y} ^ {2} \ kanan) & - w_ {x} w_ {z} ^ {2} + w_ {x} \ kiri (- w_ {x} ^ {2} - w_ {y} ^ {2} \ kanan) & 0 \ end {matrix} \ kanan] $$ tampilkan $$
atau <math> $ inline $ A ^ 4 $ inline $ </math>$$ tampilkan $$ \ kiri [\ mulai {matrix} 0 & 0 & 0 & 0 & w_ {x} ^ {2} w_ {z} - w_ {z} \ kiri (- w_ {y} ^ {2} - w_ {z} ^ {2} \ kanan) & - w_ {x} ^ {2} w_ {y} + w_ {y} \ kiri (- w_ {y} ^ {2} - w_ {z} ^ { 2} \ kanan) \\ 0 & 0 & 0 & - w_ {y} ^ {2} w_ {z} + w_ {z} \ kiri (- w_ {x} ^ {2} - w_ {z} ^ { 2} \ kanan) & 0 & w_ {x} w_ {y} ^ {2} - w_ {x} \ kiri (- w_ {x} ^ {2} - w_ {z} ^ {2} \ kanan) \ \ 0 & 0 & 0 & w_ {y} w_ {z} ^ {2} - w_ {y} \ kiri (- w_ {x} ^ {2} - w_ {y} ^ {2} \ kanan) & - w_ {x} w_ {z} ^ {2} + w_ {x} \ kiri (- w_ {x} ^ {2} - w_ {y} ^ {2} \ kanan) & 0 \\ 0 & 0 & 0 & - w_ {y} \ kiri (- w_ {x} ^ {2} w_ {y} + w_ {y} \ kiri (- w_ {y} ^ {2} - w_ {z} ^ {2} \ kanan ) \ kanan) + w_ {z} \ kiri (w_ {x} ^ {2} w_ {z} - w_ {z} \ kiri (- w_ {y} ^ {2} - w_ {z} ^ {2} \ kanan) \ kanan) & w_ {x} \ kiri (- w_ {x} ^ {2} w_ {y} + w_ {y} \ kiri (- w_ {y} ^ {2} - w_ {z} ^ {2} \ kanan) \ kanan) & - w_ {x} \ kiri (w_ {x} ^ {2} w_ {z} - w_ {z} \ kiri (- w_ {y} ^ {2} - w_ { z} ^ {2} \ kanan) \ kanan) \\ 0 & 0 & 0 & - w_ {y} \ kiri (w_ {x} w_ {y} ^ {2} - w_ {x} \ kiri (- w_ {x} ^ {2} - w_ {z} ^ {2} \ kanan) \ kanan) & w_ {x} \ kiri (w_ {x} w_ {y} ^ {2} - w_ {x} \ kiri ( - w_ {x} ^ {2} - w_ {z} ^ {2} \ kanan) \ kanan) - w_ {z} \ kiri (- w _ {y} ^ {2} w_ {z} + w_ {z} \ kiri (- w_ {x} ^ {2} - w_ {z} ^ {2} \ kanan) \ kanan) & w_ {y} \ kiri (- w_ {y} ^ {2} w_ {z} + w_ {z} \ kiri (- w_ {x} ^ {2} - w_ {z} ^ {2} \ kanan) \ kanan) \\ 0 & 0 & 0 & w_ {z} \ kiri (- w_ {x} w_ {z} ^ {2} + w_ {x} \ kiri (- w_ {x} ^ {2} - w_ {y} ^ {2 } \ kanan) \ kanan) & - w_ {z} \ kiri (w_ {y} w_ {z} ^ {2} - w_ {y} \ kiri (- w_ {x} ^ {2} - w_ {y} ^ {2} \ kanan) \ kanan) & - w_ {x} \ kiri (- w_ {x} w_ {z} ^ {2} + w_ {x} \ kiri (- w_ {x} ^ {2} - w_ {y} ^ {2} \ kanan) \ kanan) + w_ {y} \ kiri (w_ {y} w_ {z} ^ {2} - w_ {y} \ kiri (- w_ {x} ^ {2 } - w_ {y} ^ {2} \ kanan) \ kanan) \ end {matrix} \ kanan] $$ menampilkan $$
Oleh karena itu, representasi dari matriks semacam itu dimungkinkan dengan akurasi terbatas. Namun kapan $ inline $ \ omega T \ ll 1 $ inline $ seri yang diperoleh dalam elemen matriks $ inline $ f $ inline $ bertemu cukup cepat. Untuk penggunaan praktis, cukup anggota untuk tingkat kedua, jarang ke tingkat ketiga dan bahkan lebih ke tingkat keempat.
Selain itu, kami menggambarkan operasi matriks $ inline $ f $ inline $ meminta vektor $ inline $ \ omega $ inline $ , $ inline $ \ bf {x} _0 $ inline $ , $ inline $ \ bf {v} _0 $ inline $ , dan urutan pengulangan formulir:$$ menampilkan $$ \ mathbf {x} _k = F \ mathbf {x} _ {k-1} $$ menampilkan $$
Kami menghitung urutan perulangan ini untuk $ inline $ \ omega T \ approx \ frac {1} {100} $ inline $
Kode python import numpy as np from numpy import pi T = 1 wx, wy, wz = 0, 2*pi/100/2**.5, 2*pi/100/2**.5 vx0 = 10 A = np.array([ [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1], [0, 0, 0, 0, -wz, wy], [0, 0, 0, wz, 0, -wx], [0, 0, 0, -wy, wx, 0] ]) F = np.eye(6) + A * T + A @ A * T**2/2 + A @ A @ A * T**3/6 X = np.zeros((6, 101)) X[:, 0] = np.array([0, 0, 0, vx0, 0, 0]) for k in range(X.shape[1] - 1): X[:, k + 1] = F @ X[:, k] import matplotlib as mpl import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D fig = plt.figure() ax = fig.gca(projection='3d') ax.plot(X[0, :], X[1, :], X[2, :]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') plt.show()
Biarkan saya mengingatkan Anda bahwa untuk tipe np.array simbol "@" menunjukkan perkalian matriks. Jarak dan kecepatan diukur dalam parrot, kecepatan sudut dalam rad / s. Perlu juga diingat bahwa untuk mendapatkan lingkaran, perlu bahwa vektor kecepatan dan kecepatan sudut adalah tegak lurus, jika tidak, spiral akan muncul sebagai ganti lingkaran.
Sebagai hasilnya, dengan mengatur posisi awal tertentu, kecepatan dan kecepatan sudut, seseorang dapat memperoleh lintasan seperti itu
Keakuratan dari kebetulan poin pertama dan terakhir dapat diperoleh sebagai
>>> print(X[:3, 0] - X[:3,-1]) [-0.00051924 -0.0072984 0.0072984 ]
Dengan radius putar urutan 150 unit, kesalahan relatif tidak melebihi nilai urutan $ inline $ 5 \ cdot 10 ^ {- 5} $ inline $ . Akurasi ini cukup untuk model FC, yang memonitor target balik.
Kesimpulan
Jika sebelumnya FC digunakan terutama untuk menyelesaikan masalah navigasi, di mana penggunaan model gerak linier menghasilkan hasil yang baik, maka dengan pengembangan aplikasi modern seperti robot, visi komputer, dll. Kebutuhan akan model pergerakan benda yang lebih kompleks semakin meningkat. Selain itu, penerapan pendekatan di atas memungkinkan untuk mensintesis model FC diskrit tanpa biaya khusus, yang akan memudahkan tugas bagi pengembang. Satu-satunya batasan dari pendekatan ini adalah bahwa model kontinu dari sistem dinamis harus dijelaskan oleh seperangkat persamaan linear, atau setidaknya linier, dalam ruang negara.
Meringkas di atas, kita dapat memberikan algoritma untuk sintesis matriks transisi FC:
- Menulis persamaan diferensial sistem
- Transisi ke state vector dan state space
- Linierisasi jika perlu
- Representasi matriks transisi dalam bentuk eksponen matriks dan pemotongan seri jika perlu
- Perhitungan matriks yang tersisa dengan mempertimbangkan matriks transisi
Penulis menyambut kritik membangun mengenai kesalahan yang dibuat, ketidakakuratan, formulasi yang salah, metode yang tidak disebutkan, dan banyak lagi. Terima kasih atas perhatian anda!
Literatur bekas
[1] Medic J. Estimasi linear optimal dan kontrol. Per. dari bahasa inggris Ed. A.S. Shatalova Moscow. Rumah penerbitan "Energi", 1973, 440 hal.
[2] Matveev V.V. Dasar-dasar pembangunan sistem inersia strapdown dari St. Petersburg: Pusat Penelitian Negara Federasi Rusia OJSC Concern TsNII Elektribribor, 2009. - 280-an. ISBN 978-5-900180-73-3