Durum denetleyicisi ve Kalman Filtreleri

Durum gözlemcileri, sistemin doğru state nu tahmin etmek için bir sistemin davranışı ve harici ölçümler hakkındaki bilgileri birleştirir. Doğrusal sistemler için kullanılan yaygın bir gözlemci Kalman Filtresidir. Kalman filtreleri diğerlerine göre avantajlıdır filters, bir sistemin durumunu en iyi şekilde tahmin etmek için sistemin durum-uzay modeliyle bir veya daha fazla sensörden alınan ölçümleri birleştirir.

Bu görüntü, çeşitli farklı filtrelerden geçen zaman içindeki dönenteker hızı ölçümlerini gösterir. İyi ayarlanmış bir Kalman filtresinin dönerteker dönüşü sırasında hiçbir ölçüm gecikmesi göstermediğini ve gürültülü verileri hala reddederken ve toplar içinden geçerken rahatsızlıklara hızlı tepki verdiğini unutmayın. Filtrelerle ilgili daha fazla bilgi filters sectionbulunabilir.

Filter comparison between: Kalman, Median, and IIR.

Gauss Fonksiyonları

Kalman filtreleri, bir süreçteki gürültüyü modellemek için Gauss dağılımlarını (veya çan eğrilerini) kullanır. Bir Gauss fonksiyonunun grafiği bir “çan eğrisi” şeklidir. Bu fonksiyon, ortalaması (çan eğrisinin “tepe noktasının-peak” konumu) ve varyansı (çan eğrisinin nasıl “yayıldığının” bir ölçüsü) ile tanımlanır. Kalman filtresi durumunda, sistemin tahmini state ortalamadır, varyans ise filtrenin gerçek state-durum hakkında ne kadar kesin (veya belirsiz) olduğunun bir ölçüsüdür.

../../../../_images/normal-distribution.png

Varyans ve kovaryans fikri, bir Kalman filtresinin işlevinin merkezinde yer alır. Kovaryans, iki rastgele değişkenin nasıl ilişkilendirildiğinin bir ölçüsüdür. Tek durumlu bir sistemde kovaryans matrisi basitçe \(\mathbf{\text{var}(x_1)}\) veya varyansı içeren bir matristir \(\mathbf{\text{var}(x_1)}\) durum \(x_1\). Bu varyansın büyüklüğü, mevcut durum tahminini açıklayan Gauss fonksiyonunun standart sapmasının karesidir. Kovaryans için nispeten büyük değerler gürültülü verileri gösterebilirken, küçük kovaryanslar filtrenin tahmini konusunda daha emin olduğunu gösterebilir. Varyans veya kovaryans için “büyük” ve “küçük” değerlerin kullanılan temel birime göre olduğunu unutmayın - örneğin, eğer \(\mathbf{x_1}\) metre cinsinden ölçüldüyse, \(\mathbf{\text{cov}(x_1, x_1)}\) metre kare cinsinden olacaktır.

Kovaryans matrisleri aşağıdaki biçimde yazılır:

\[\begin{split}\mathbf{\Sigma} &= \begin{bmatrix} \text{cov}(x_1, x_1) & \text{cov}(x_1, x_2) & \ldots & \text{cov}(x_1, x_n) \\ \text{cov}(x_2, x_1) & \text{cov}(x_2, x_2) & \ldots & \text{cov}(x_1, x_n) \\ \vdots & \vdots & \ddots & \vdots \\ \text{cov}(x_n, x_1) & \text{cov}(x_n, x_2) & \ldots & \text{cov}(x_n, x_n) \\ \end{bmatrix}\end{split}\]

Kalman Filtreleri

Önemli

Bir Kalman filtresinin gerçekte ne yaptığına dair bir sezgi geliştirmek önemlidir. Kalman and Bayesian Filters in Python by Roger Labbe tarafından kitap Bayes filtrelerine harika bir görsel ve etkileşimli giriş sağlar. WPILib’deki Kalman filtreleri matematiği kolay hale getirmek için doğrusal cebir kullanır, ancak fikirler tek boyutlu duruma benzer. Bu filtrelerin ne yaptığına dair bir fikir edinmek için Bölüm 4’ü okumanızı öneririz.

Özetlemek gerekirse, Kalman filtreleri (ve tüm Bayes filtreleri) iki bölümden oluşur: tahmin ve düzeltme. Tahmin, sistemimizin dinamiklerine göre zaman içinde durumumuzun tahminini ileriye doğru yansıtır ve tahmini durumu ölçülen duruma doğru yönlendirir. Filtreler genellikle aynı zaman diliminde önceden oluşturulurken, bu kesinlikle gerekli değildir - Örneğin, WPILib’in poz tahmincileri sık sık tahmin çağırır ve yalnızca yeni ölçüm verileri mevcut olduğunda (örneğin, düşük frame oranlı bir görme sisteminden) düzelir.

Aşağıda, ayrık zamanlı bir Kalman filtresinin denklemleri gösterilmektedir:

\[\begin{split}\text{Predict step} \nonumber \\ \hat{\mathbf{x}}_{k+1}^- &= \mathbf{A}\hat{\mathbf{x}}_k + \mathbf{B} \mathbf{u}_k \\ \mathbf{P}_{k+1}^- &= \mathbf{A} \mathbf{P}_k^- \mathbf{A}^T + \mathbf{\Gamma}\mathbf{Q}\mathbf{\Gamma}^T \\ \text{Update step} \nonumber \\ \mathbf{K}_{k+1} &= \mathbf{P}_{k+1}^- \mathbf{C}^T (\mathbf{C}\mathbf{P}_{k+1}^- \mathbf{C}^T + \mathbf{R})^{-1} \\ \hat{\mathbf{x}}_{k+1}^+ &= \hat{\mathbf{x}}_{k+1}^- + \mathbf{K}_{k+1}(\mathbf{y}_{k+1} - \mathbf{C} \hat{\mathbf{x}}_{k+1}^- - \mathbf{D}\mathbf{u}_{k+1}) \\ \mathbf{P}_{k+1}^+ &= (\mathbf{I} - \mathbf{K}_{k+1}\mathbf{C})\mathbf{P}_{k+1}^-\end{split}\]
\[\begin{split}\begin{array}{llll} \mathbf{A} & \text{system matrix} & \hat{\mathbf{x}} & \text{state estimate vector} \\ \mathbf{B} & \text{input matrix} & \mathbf{u} & \text{input vector} \\ \mathbf{C} & \text{output matrix} & \mathbf{y} & \text{output vector} \\ \mathbf{D} & \text{feedthrough matrix} & \mathbf{\Gamma} & \text{process noise intensity vector} \\ \mathbf{P} & \text{error covariance matrix} & \mathbf{Q} & \text{process noise covariance matrix} \\ \mathbf{K} & \text{Kalman gain matrix} & \mathbf{R} & \text{measurement noise covariance matrix} \end{array}\end{split}\]

Durum tahmini \(\mathbf{x}\), ile birlikte \(\mathbf{P}\), filtremizin sistemin gerçek durumuna ilişkin tahminini tanımlayan Gauss işlevinin ortalamasını ve kovaryansını açıklar.

Gürültü Kovaryans Matrislerinin İşlenmesi ve Ölçümü

Süreç ve ölçüm gürültüsü kovaryans matrisleri \(\mathbf{Q}\) ve:math:mathbf{R} durumlarımızın ve ölçümlerimizin her birinin varyansını tanımlar. Bir Gauss fonksiyonu için varyansın, fonksiyonun standart sapmasının karesi olduğunu unutmayın. Bir WPILib’de Q ve R, köşegenleri ilgili varyanslarını içeren köşegen matrisleridir. Örneğin, \(\begin{bmatrix}\text{position} \\ \text{velocity} \end{bmatrix}\) ve ölçümler \(\begin{bmatrix}\text{position} \end{bmatrix}\) durum standart sapmalarıyla:math:begin{bmatrix}0.1 \ 1.0end{bmatrix} ve ölçüm standart sapması \(\begin{bmatrix}0.01\end{bmatrix}\) şunlara sahip olacaktır \(\mathbf{Q}\) and \(\mathbf{R}\) matrisler:

\[\begin{split}Q = \begin{bmatrix}0.01 & 0 \\ 0 & 1.0\end{bmatrix}, R = \begin{bmatrix}0.0001\end{bmatrix}\end{split}\]

Error Covariance Matrix-Hata Kovaryans Matrisi

Hata kovaryans matrisi \(\mathbf{P}\), durum tahmininin kovaryansını açıklar \(\mathbf{\hat{x}}\). Gayri resmi olarak, \(\mathbf{P}\), tahmin edilen state hakkındaki kesinliğimizi tanımlar. Eğer \(\mathbf{P}\) büyükse, gerçek durum hakkındaki belirsizliğimiz büyüktür. Tersine, daha küçük elemanlara sahip \(\mathbf{P}\), gerçek durumumuz hakkında daha az belirsizlik anlamına gelir.

Modeli ileriye doğru yansıtırken, sistemin gerçek durumu hakkındaki kesinliğimiz azaldıkça \(\mathbf{P}\) artar.

Tahmin adımı

Tahminde, durum tahminimiz doğrusal sistem dinamiklerine göre güncellenir \(\mathbf{\dot{x} = Ax + Bu}\). Ayrıca, hata kovaryansımız \(\mathbf{P}\), işlem gürültü kovaryans matrisi \(\mathbf{Q}\) ile artar. Daha büyük \(\mathbf{Q}\) değerleri, hata kovaryansımızın \(\mathbf{P}\) daha hızlı büyümesini sağlayacaktır. Bu \(\mathbf{P}\), modeli ve ölçümleri ağırlıklandırmak için düzeltme adımında kullanılır.

Doğru adım

Doğru adımda, durum tahminimiz yeni ölçüm bilgilerini içerecek şekilde güncellenir. Bu yeni bilgi, Kalman kazancı \(\mathbf{K}\) tarafından \(\ mathbf{\hat {x}}\) ‘durum tahminine göre ağırlıklandırılır. \(\mathbf{K}\) ‘nın büyük değerleri gelen ölçümleri daha fazla ağırlıklandırırken, \(\mathbf{K}\) ‘nın daha küçük değerleri durum tahminimize daha fazla ağırlık verir. Çünkü \(\mathbf{K}\), \(\mathbf{P}\) ile ilgilidir, \(\mathbf{P}\) ‘nin daha büyük değerleri \(\mathbf{K}' 'yı arttıracaktır ve ölçümleri daha ağırlıklandıracaktır. Örneğin, uzun bir süre için bir filtre tahmin edilirse, büyük :math:\)mathbf{P}`, yeni bilgileri büyük ölçüde ağırlaştıracaktır.

Son olarak, hata kovaryansı \(\mathbf{P}\) durum tahminine olan güvenimizi artırmak için azalır.

Kalman Filtrelerini Ayarlama

WPILib’in Kalman Filtresi sınıflarının constructorları, doğrusal bir sistemi, işlem gürültüsü standart sapmalarının bir vektörünü ve ölçüm gürültüsü standart sapmalarını alır. Bunlar, köşegenleri her durum veya ölçümün standart sapmalarının veya varyanslarının karesiyle doldurarak \(\mathbf{Q}\) ve \(\mathbf{R}\) matrislerine dönüştürülür. Bir durumun standart sapmasını (ve dolayısıyla, \(\mathbf{Q}\) içindeki karşılık gelen girdisini) azaltarak, filtre gelen ölçümlere daha fazla güvenmeyecektir. Benzer şekilde, bir durumun standart sapmasını artırmak, gelen ölçümlere daha çok güvenecektir. Aynısı, ölçüm standart sapmaları için de geçerlidir - bir girişi azaltmak, filtrenin ilgili durum için gelen ölçüme daha fazla güvenmesini sağlarken, artması ölçüme olan güveni azaltacaktır.

54
55
56
57
58
59
60
  private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
        Nat.N1(), Nat.N1(),
        m_flywheelPlant,
        VecBuilder.fill(3.0), // How accurate we think our model is
        VecBuilder.fill(0.01), // How accurate we think our encoder
        // data is
        0.020);