Filter Kalman untuk posisi dan kecepatan: memperkenalkan perkiraan kecepatan

Terima kasih kepada semua orang yang mengirim komentar / jawaban untuk pertanyaan saya kemarin ( Menerapkan filter Kalman untuk posisi, kecepatan, percepatan ). Saya telah melihat apa yang direkomendasikan, dan khususnya pada (a) contoh wikipedia tentang posisi dan kecepatan satu dimensi dan juga situs web lain yang menganggap hal serupa .

Perbarui 26-Apr-2013 : pertanyaan asli di sini berisi beberapa kesalahan, terkait dengan fakta bahwa saya belum memahami contoh wikipedia dengan benar tentang posisi dan kecepatan satu dimensi . Dengan pemahaman saya yang lebih baik tentang apa yang terjadi, saya sekarang telah menyusun ulang pertanyaan dan memfokuskannya lebih erat.

Kedua contoh yang saya rujuk dalam paragraf pengantar di atas menganggap bahwa hanya posisi yang diukur. Namun, contoh baik memiliki jenis perhitungan $(x_k-x_{k-1})/dt$ untuk kecepatan. Sebagai contoh, misalnya Wikipedia menentukan ${\bf H}$ matriks sebagai ${\bf H} = [1\ \ \ 0]$ , yang berarti bahwa hanya posisi input. Fokus pada contoh Wikipedia, negara vektor ${\bf x}_k$ dari filter Kalman berisi posisi $x_k$ dan kecepatan $\dot{x}_{k}$ , yaitu

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$

Misalkan pengukuran posisi pada waktu yaitu x k . Kemudian jika posisi dan kecepatan pada saat k - 1 adalah x k - 1 dan ˙ x k - 1 , dan jika suatu adalah percepatan konstan yang berlaku dalam selang waktu k - 1 untuk k , dari pengukuran x itu mungkin untuk menyimpulkan nilai untuk sebuah menggunakan rumus$k$$\hat{x}_k$$k-1$$x_{k-1}$$\dot{x}_{k-1}$$a$$k-1$$k$$\hat{x}$$a$

$$\hat{x}_k = x_{k-1} + \dot{x}_{k-1} dt + \frac{1}{2} a dt^2$$

Ini berarti bahwa pada saat , pengukuran ˙ x k kecepatan yang diberikan oleh$k$$\hat{\dot{x}}_k$

$$\hat{\dot{x}}_k = \dot{x}_{k-1} + a dt = 2 \frac{\hat{x}_k - {x}_{k-1}}{dt} - \dot{x}_{k-1}$$

Semua jumlah di sisi kanan persamaan itu , x k - 1 dan ˙ x k - 1 ) biasanya didistribusikan variabel acak dengan cara yang dikenal dan standar deviasi, sehingga R matrix untuk vektor pengukuran$\hat{x}_k$$x_{k-1}$$\dot{x}_{k-1}$$\bf R$

$$\begin{align*} \mathbf{\hat{x}}_{k} & =\left(\begin{array}[c]{c}\hat{x}_{k}\\ \hat{\dot{x}}_{k}\end{array} \right) \end{align*}$$

dapat dihitung. Apakah ini cara yang valid untuk memperkenalkan perkiraan kecepatan ke dalam proses?

Apakah ini cara yang valid untuk memperkenalkan perkiraan kecepatan ke dalam proses?

Jika Anda memilih negara Anda dengan tepat, maka perkiraan kecepatan datang "gratis". Lihat derivasi dari model sinyal di bawah ini (untuk kasus 1-D sederhana yang telah kita lihat).

Model Sinyal, Ambil 2

Jadi, kita benar-benar perlu menyepakati model sinyal sebelum kita bisa bergerak maju. Dari mengedit Anda, sepertinya model Anda dari posisi, , adalah:$x_k$

$$\begin{array} xx_{k+1} &=& x_{k} + \dot{x}_{k} \Delta t + \frac{1}{2} a (\Delta t)^2\\ \dot{x}_{k+1} &=& \dot{x}_{k} + a \Delta t \end{array}$$

Jika keadaan kita seperti sebelumnya: maka persamaan pembaruan status adalah hanya: xk+1=( 1 Δ t 0 1 )xk+( ( Δ t )

$$\begin{align*} \mathbf{x}_{k} & =\left(\begin{array}[c]{c}x_{k}\\ \dot{x}_{k}\end{array} \right) \end{align*}$$ mana sekarang kamisebuahkadalah percepatan terdistribusi normal. $$\mathbf{x}_{k+1} = \left(\begin{array}[c]{c} 1\ \ \Delta t\\ 0\ \ 1\end{array} \right) \mathbf{x}_{k} + \left(\begin{array}[c]{c} \frac{(\Delta t)^2}{2} \\ \Delta t \end{array} \right) a_k$$ $a_k$

Itu memberikan matriks yang berbeda dari versi sebelumnya, tetapi matriks F dan H harus sama.$\mathbf{G}$$\mathbf{F}$$\mathbf{H}$

---

Jika saya menerapkan ini dalam scilab(maaf, tidak ada akses ke matlab), sepertinya:

// Signal Model
DeltaT = 0.1;
F = [1 DeltaT; 0 1];
G = [DeltaT^2/2; DeltaT];
H = [1 0];

x0 = [0;0];
sigma_a = 0.1;

Q = sigma_a^2;
R = 0.1;

N = 1000;

a = rand(1,N,"normal")*sigma_a;

x_truth(:,1) = x0;
for t=1:N,
    x_truth(:,t+1) = F*x_truth(:,t) + G*a(t);
    y(t) = H*x_truth(:,t) + rand(1,1,"normal")*sqrt(R);
end

Lalu, saya bisa menerapkan persamaan filter Kalman untuk ini (pengukuran berisik).$y$

// Kalman Filter
p0 = 100*eye(2,2);

xx(:,1) = x0;
pp = p0;
pp_norm(1) = norm(pp);
for t=1:N,
    [x1,p1,x,p] = kalm(y(t),xx(:,t),pp,F,G,H,Q,R);
    xx(:,t+1) = x1;
    pp = p1;
    pp_norm(t+1) = norm(pp);
end

Jadi kami memiliki pengukuran bising kami , dan kami telah menerapkan filter Kalman untuk mereka dan menggunakan model sinyal yang sama untuk menghasilkan y seperti yang kami lakukan untuk menerapkan filter Kalman (asumsi yang cukup besar, kadang-kadang!).$y$$y$

Kemudian plot berikut menunjukkan hasilnya.

Plot 1 : dan x k terhadap waktu.$y$$x_k$

Plot 2 : Tampilan yang diperbesar dari beberapa sampel pertama:

Plot 3 : Sesuatu yang tidak pernah Anda dapatkan dalam kehidupan nyata, posisi sebenarnya vs perkiraan posisi tersebut.

Plot 4 : Sesuatu yang Anda juga tidak pernah dapatkan dalam kehidupan nyata, kecepatan sebenarnya vs perkiraan kecepatan.

Plot 5 : Norma matriks kovarian negara (sesuatu yang harus selalu Anda pantau dalam kehidupan nyata!). Perhatikan bahwa sangat cepat berubah dari nilai awalnya yang sangat besar ke sesuatu yang sangat kecil, jadi saya hanya menunjukkan beberapa sampel pertama.

Plot 6 : Plot kesalahan antara posisi sebenarnya dan kecepatan dan perkiraan mereka.

$\mathbf{z}_{k+1}=x_{k+1}$$\mathbf{P}_k=0$$\mathbf{P}_{k+1}^{-}=\mathbf{Q}$$\mathbf{K}_{k+1}$

$$\mathbf{K}_{k+1} = \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right)$$

Ini berarti bahwa prosedur pembaruan Kalman menghasilkan

$$\begin{align*} \mathbf{\hat{x}}_{k+1} & = \mathbf{F}_{k+1}\mathbf{x}_k + \mathbf{K}_{k+1}\left(\mathbf{z}_{k+1} - \mathbf{H}_{k+1} \mathbf{F}_{k+1}\mathbf{x}_k\right)\\ & = \left(\begin{array}[c]{c}x_k + \dot{x}_k dt\\ \dot{x}_k\end{array} \right) + \left(\begin{array}[c]{c}1\\ 2/dt\end{array} \right) \left(x_{k+1} - \left( x_k + \dot{x}_k dt\right) \right)\\ & = \left(\begin{array}[c]{c}x_{k+1}\\ 2 \left(x_{k+1} - x_k \right) /dt - \dot{x}_k\end{array} \right) \end{align*}$$

Seperti yang Anda lihat, nilai untuk kecepatan diberikan dengan tepat formula yang Anda usulkan untuk digunakan untuk perkiraan kecepatan. Jadi meskipun Anda tidak bisa melihat perhitungan apa pun$(x_k-x_{k-1})/dt$ untuk kecepatan, sebenarnya itu tersembunyi di sana.