文章目錄

• 卡爾曼濾波器
• 擴展卡爾曼濾波器
• 協方差
• Ref：

卡爾曼濾波器

首先從工程上看卡爾曼濾波算法。

引入一個離散控制過程的系統，該系統可用一個線性隨機微分方程（linear stochastic difference equation）來描述：
$$X(k)=A?X(k?1)+B?U(k)+W(k)$$

這里：$X(k)$ 是 $k$ 時刻的系統狀態，$U(k)$ 是 $k$ 時刻對系統的控制量。$A$ 和 $B$ 是系統參數，對于多模系統，它們為矩陣。

再加上系統的測量值：
$$Z(k)=H?X(k)+V(k)$$

這里：$Z(k)$ 是 $k$ 時刻的測量值，$H$ 是測量參數，對于多模系統，$H$ 為矩陣。$W(k)$ 和 $V(k)$ 分別表示過程和測量的噪聲，它們被假設成高斯白噪聲，它們的 covariance 分別是 $Q,R$。

對于滿足上面條件（線性隨機微分系統，過程和測量都是高斯白噪聲），卡爾曼濾波器是最優的信息處理器。

---

下面介紹卡爾曼濾波器算法流程及核心公式。

首先利用系統過程模型，來預測下一個狀態的系統：
$$X(k|k?1)=A?X(k?1|k?1)+B?U(k)\text{\hspace{1em}(1)}$$

$X(k|k?1)$ 是利用上一狀態預測的結果，$X(k?1|k?1)$ 是上一狀態的最優結果。$U(k)$ 為現在狀態的控制量，若沒有，則為 $0$。

現在更新 $X(k|k?1)$ 的 covariance，用 $P$ 表示：
$$P(k|k?1)=A?P(k?1|k?1)?A^{\prime }+Q\text{\hspace{1em}(2)}$$

這里：$P(k|k?1)$ 是 $X(k|k?1)$ 對應的 covariance，$P(k?1|k?1)$ 是 $X(k?1|k?1)$ 對應的 covariance。$A^{\prime }$ 表示 $A$ 的轉置，$Q$ 是系統的 covariance。

式（1）（2）用來對系統進行預測。

然后再收集系統的觀測值 $Z(k)$，則最優值：
$$X(k|k)=X(k|k?1)+K_g(k)?(Z(k)?H?X(k|k?1))\text{\hspace{1em}(3)}$$

這里 $K_g$ 為卡爾曼增益（Kalman Gain）。

$$K_g(k)=\frac{P(k|k?1)?H^{\prime }}{H?P(k|k?1)?H^{\prime }+R}\text{\hspace{1em}(4)}$$

到目前為止，已經得到 $k$ 狀態下的最優估算值 $X(k|k?1)$。

---

但是為了要卡爾曼濾波器不斷得運行下去直到系統過程結束，需要更新 $k$ 狀態下 $X(k|k?1)$ 的 covariance：
$$P(k|k)=(I?K_g(k)?H)P(k|k?1)\text{\hspace{1em}(5)}$$

這里 $I$ 為 $1$ 的矩陣，對于單模系統 $I=1$。

當系統進入 $k+1$ 狀態時，$P(k|k)$ 就是式（2）中的 $P(k?1|k?1)$。這樣，算法就可以自回歸地運算下去。

擴展卡爾曼濾波器

由上述的介紹可以得知，卡爾曼濾波器只適用于線性系統模型，然而實際中的系統往往都是非線性模型。所示必須對卡爾曼濾波器進行修改。

首先要了解一下線性化卡爾曼濾波。它和線性卡爾曼濾波器在濾波器的算法方面有同樣的算法結構。不一樣的地方在于這兩者的系統模型不同。線性卡爾曼濾波器的系統本身就是線性系統，而線性化卡爾曼濾波器的系統本身是非線性系統，但是機智的大神們將非線性的系統進行了線性化，于是卡爾曼濾波就可以用在非線性系統中了。對于一個卡爾曼濾波器的設計者，就可以不管模型到底是一開始就是線性系統還是非線性系統線性化得到的線性系統，反正只要是線性系統就好了。

但是線性化卡爾曼濾波器會發散。為什么會發散呢？是這樣，我們在對非線性系統進行線性化的過程中，只有被線性化的那個點附近的線性化模型和真實的模型相近，遠的誤差就大了，那么這個時候卡爾曼濾波器的效果就不好。所以線性化的這個限制要時刻考慮，這也就是為什么要把線性卡爾曼濾波器和線性化卡爾曼濾波器區分開的理由。而決定一個線性化濾波器成功與否的關鍵就在于這個濾波器系統模型線性化得好不好。

EKF 基于線性化系統的思想，將系統函數的非線性函數作一階Taylor展開，得到線性化系統方程。擴展的卡爾曼濾波器算法就是適用于非線性系統的卡爾曼濾波器。它與經典的線性卡爾曼濾波器很相似，算法步驟和結構都相同。不同在于系統模型和矩陣 $A$ 和 $H$。

協方差

協方差（英語：Covariance），在概率論與統計學中用于衡量兩個隨機變量的聯合變化程度。

若變量X的較大值主要與另一個變量Y的較大值相對應，而兩者的較小值也相對應，則可稱兩變量傾向于表現出相似的行為，協方差為正。在相反的情況下，當一個變量的較大值主要對應于另一個變量的較小值時，則兩變量傾向于表現出相反的行為，協方差為負。即協方差之正負號顯示著變量的相關性。

方差為協方差的一種特殊情況，即該變量與其自身之協方差。

期望值分別為 $E(X)=\mu$ 和 $E(Y)=\upsilon$ 的兩個具有有限二階矩的實數隨機變量 $X$ 和 $Y$ 之間的協方差定義為：
$$\text{cov}(X,Y)=E((X?\mu )(Y?\upsilon ))=E(X?Y)?\mu \upsilon$$

協方差表示的是兩個變量的總體的誤差，這與只表示一個變量誤差的方差不同。如果兩個變量的變化趨勢一致，也就是說如果其中一個大于自身的期望值，另外一個也大于自身的期望值，那么兩個變量之間的協方差就是正值。如果兩個變量的變化趨勢相反，即其中一個大于自身的期望值，另外一個卻小于自身的期望值，那么兩個變量之間的協方差就是負值。

Ref：

MATLAB實現卡爾曼濾波器(KF、EKF)

協方差 - WikiPedia

卡爾曼濾波（Kalman Filter）原理與公式推導

總結

以上是生活随笔為你收集整理的【UWB】Kalman filter, KF卡尔曼滤波, EKF 扩展卡尔曼滤波的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。