Konum ve hız için Kalman filtresi: hız tahminlerini tanıtmak

Dün sorguma yorum yazan / cevaplayan herkese teşekkürler ( Konum, hız, ivme için bir Kalman filtresi uygulamak ). Ne önerildiğine ve özellikle (a) tek boyutlu konum ve hızdaki wikipedia örneğine ve benzer bir şey düşünen başka bir web sitesine bakıyorum .

Güncelleme 26-Nisan-2013 : Buradaki asıl soru , wikipedia örneğini bir boyutlu pozisyon ve hızda tam olarak anlamadığım gerçeğiyle ilgili bazı hatalar içeriyordu . Neler olup bittiğini anlama konusundaki anlayışım sayesinde, şimdi soruyu yeniden tasarladım ve daha sıkı bir şekilde odakladım.

Yukarıdaki giriş paragrafında bahsettiğim her iki örnek de, bunun yalnızca ölçülen konum olduğunu varsaymaktadır. Bununla birlikte, hiçbir örnek hız için herhangi bir hesaplama . Örneğin, Wikipedia örneği, matrisini olarak belirtir, bu, yalnızca konumun giriş olduğu anlamına gelir. Wikipedia örneğine odaklanarak , Kalman filtresinin durum vektörü , konumunu ve speed konumunu , yani $(x_k-x_{k-1})/dt$ ${\bf H}$ ${\bf H} = [1\ \ \ 0]$ ${\bf x}_k$ $x_k$ $\dot{x}_{k}$

\begin{aligned} x_{k} & = (\begin{matrix} x_{k} \\ {\dot{x}}_{k} \end{matrix}) \end{aligned}

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

Zaman pozisyon ölçümü varsayalım olan . O zaman sırasındaki konum ve hız ve ise ve , ila arasındaki aralıkta uygulanan sabit bir ivme ise , ölçümünden , aşağıdaki formülü kullanmak için değer çıkarmak mümkündür. $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} d t + \frac{1}{2} a d t^{2}

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

Bu kez de ima , bir ölçüm hızının verilir $k$ $\hat{\dot{x}}_k$

{\hat{\dot{x}}}_{k} = {\dot{x}}_{k - 1} + a d t = 2 \frac{{\hat{x}}_{k} - x_{k - 1}}{d t} - {\dot{x}}_{k - 1}

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

Bu denklemin sağ tarafındaki tüm miktarlar (yani, , ve ) normalde bilinen yollara ve standart sapmalara sahip rastgele değişkenler dağıtılır. , böylece ölçüm vektörü için matrisi $\hat{x}_k$ $x_{k-1}$ $\dot{x}_{k-1}$ $\bf R$

\begin{aligned} {\hat{x}}_{k} & = (\begin{matrix} {\hat{x}}_{k} \\ {\hat{\dot{x}}}_{k} \end{matrix}) \end{aligned}

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

hesaplanabilir. Bu, hız tahminlerini sürece dahil etmenin geçerli bir yolu mu?

kalman-filters

— stokastik
kaynak

Bütün hesaplarına bakmadım. Bununla birlikte, Wikipedia örneğinden bahsedersek, yapısı üzerinde biraz kafanız karışmış gibi görünmektedir. Sadece pozisyon ölçülürken haklısın. Bununla birlikte, "sabit hız" adı verilen bir model kullanılır. Bu, durum geçiş matrisinde hızın sabit olduğu anlamına gelir.

— Jason R,

Hızdaki değişiklikler proses gürültü matrisi kullanılarak modellenmiştir. Bu nedenle, doğal olarak, hızın belirli bir kovaryansla rasgele değişeceğini varsayıyorsunuz. Şaşırtıcı bir şekilde, bu genellikle iyi çalışıyor. Bu şekilde en yüksek durum değişkenli türevinizin üzerinde bir işlem türevi kullanmak yaygındır. Örneğin, modelinize ivme eklerseniz, işlem gürültünüze dahil olan rastgele bir sarsıntı bileşeniniz olabilir.

— Jason R,

Wikipedia modelinde @JasonR (konum ve hız arasında sıfır başlangıç kovaryansı varsayarsak), hız tahmini her zaman başlangıç değeridir (dediğiniz gibi, "sabit hız" modeli). Bununla birlikte, hızdaki varyans, işlem gürültüsü aracılığıyla monoton şekilde büyür ve bunu azaltabilecek hiçbir ölçüm yoktur. Bunun sadece pozisyon alan ve sabit bir hız belirleyen bir model üzerinde avantajı nedir?

— Stokastik olarak

Hız tahmininin varyansı monoton şekilde artmamalıdır. İşlem gürültüsü basitçe durum geçiş denklemine stokastik bir bileşen getirerek, sistem durumunun zaman zaman adım adım adım nasıl gelişeceği konusunda bazı belirsizlikleri ifade etmenize izin verir. İşlem gürültüsünü dahil etmezseniz, filtreniz gerçekten sabit bir hız verir. Muhtemelen istediğin bu değildir.

— Jason R,

@JasonR, wikipedia modeline bakarsanız, monotinik olarak artan hız farkının size verdiği şeyi göreceksiniz!

— Stokastik olarak

Bu, hız tahminlerini sürece dahil etmenin geçerli bir yolu mu?

Durumunuzu uygun şekilde seçerseniz, hız tahminleri "bedava" olarak gelir. Aşağıdaki sinyal modelinin türetmesine bakın (baktığımız basit 1-D durum için).

Sinyal modeli, 2 almak

Bu yüzden, bunu ilerletmeden önce bir sinyal modelinde hemfikir olmamız gerekiyor. Düzenlemenizde, konumunuzu modeline benziyor, , şudur: $x_k$

\begin{matrix} x_{k + 1} & = & x_{k} + {\dot{x}}_{k} Δ t + \frac{1}{2} a (Δ t)^{2} \\ {\dot{x}}_{k + 1} & = & {\dot{x}}_{k} + a Δ t \end{matrix}

$\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}$

: sonra durum güncelleme denklemi şöyle olur: burada normalde dağıtılmış hızlanmadır.

\begin{aligned} x_{k} & = (\begin{matrix} x_{k} \\ {\dot{x}}_{k} \end{matrix}) \end{aligned}

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

x_{k + 1} = (\begin{matrix} 1 Δ t \\ 0 1 \end{matrix}) x_{k} + (\begin{matrix} \frac{(Δ t)^{2}}{2} \\ Δ t \end{matrix}) a_{k}

$\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}

$a_k$

Bu önceki sürümden farklı matrisi verir , ancak ve matrisleri aynı olmalıdır. $\mathbf{G}$ $\mathbf{F}$ $\mathbf{H}$

Bunu uygularsam scilab(üzgünüm, matlab'a erişim yok), şöyle görünüyor:

// 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

Ardından, Kalman filtre denklemlerini bu uygulayabilirim (gürültülü ölçümler). $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

Bu nedenle, gürültülü ölçümlerimizi alıyoruz ve Kalman filtresini kendilerine uyguladık ve Kalman filtresini (bazen oldukça büyük bir varsayım!) Uyguladığımız gibi üretmek için aynı sinyal modelini kullandık . $y$ $y$

Sonra aşağıdaki grafikler sonucu göstermektedir.

Plot 1 : ve karşı. $y$ $x_k$

görüntü tanımını buraya girin

Plot 2 : İlk birkaç örneğin büyütülmüş görüntüsü:

görüntü tanımını buraya girin

Arsa 3 : Asla gerçek hayatta elde edemeyeceğiniz bir şey, gerçek pozisyon, pozisyonun durum tahmini ile karşılaştırılır.

görüntü tanımını buraya girin

Grafik 4 : Gerçek hayatta asla elde edemeyeceğiniz bir şey, gerçek hız, hızın durum tahmini ile karşılaştırılır.

görüntü tanımını buraya girin

Grafik 5 : Durum kovaryansı matrisinin normu (gerçek hayatta her zaman izlemeniz gereken bir şey!). Çok hızlı bir şekilde ilk çok büyük değerinden çok küçük bir şeye gittiğini unutmayın, bu yüzden sadece ilk birkaç örneği gösterdim.

görüntü tanımını buraya girin

Grafik 6 : Gerçek konum ve hız arasındaki farkın çizimleri ve tahminleri.

görüntü tanımını buraya girin

Konum ölçümlerinin kesin olduğu durumu incelerseniz, Kalman udpate denklemlerinin HIZ pozisyonu ve hızı için kesin sonuçlar verdiğini görürsünüz. Matematiksel olarak nedenini görmek basittir. Wikipedia makalesiyle aynı gösterimi kullanarak , kesin ölçümler . Başlangıç konumu ve hızın , olarak bilindiğini varsayarsanız, ve Kalman kazanç matrisi tarafından verilir $\mathbf{z}_{k+1}=x_{k+1}$ $\mathbf{P}_k=0$ $\mathbf{P}_{k+1}^{-}=\mathbf{Q}$ $\mathbf{K}_{k+1}$

K_{k + 1} = (\begin{matrix} 1 \\ 2 / d t \end{matrix})

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

Bu, Kalman güncelleme prosedürünün ürettiği anlamına gelir

\begin{aligned} {\hat{x}}_{k + 1} & = F_{k + 1} x_{k} + K_{k + 1} (z_{k + 1} - H_{k + 1} F_{k + 1} x_{k}) \\ = (\begin{matrix} x_{k} + {\dot{x}}_{k} d t \\ {\dot{x}}_{k} \end{matrix}) + (\begin{matrix} 1 \\ 2 / d t \end{matrix}) (x_{k + 1} - (x_{k} + {\dot{x}}_{k} d t)) \\ = (\begin{matrix} x_{k + 1} \\ 2 (x_{k + 1} - x_{k}) / d t - {\dot{x}}_{k} \end{matrix}) \end{aligned}

$\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*}$

Gördüğünüz gibi, hızın değeri tam olarak hız tahmini için kullanmayı düşündüğünüz formüle göre verilmiştir. Yani hız için herhangi bir hesaplama , aslında sonuçta orada gizli. $(x_k-x_{k-1})/dt$

— Peter K.
kaynak

Bu zamana kadarki tüm yardımların için teşekkürler. Asıl sorum bazı yanlış anlamalar içeriyordu, bu yüzden yeniden odaklamaya ve z_k hakkındaki sorunuza cevap vermeye çalıştım.

— Stokastik olarak

tüm çabalarınız için teşekkürler vm :-). Çalışmanız beni cevabınıza çektiğim bir miktar matematik yapmaya zorladı, umarım sakıncası yoktur. Her neyse, şimdi% 100 standart yöntemin iyi olduğuna ikna oldum, çünkü sonuçta orada hız için formüller görebiliyorum. Tekrar teşekkürler

— Stokastik olarak

Yardım edebilmek için sevindim! Cevabınıza ekleme ile sorun değil.

— Peter K.