文章目錄

• 卡爾曼濾波器
• 擴(kuò)展卡爾曼濾波器
• 協(xié)方差
• Ref：

卡爾曼濾波器

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

引入一個(gè)離散控制過(guò)程的系統(tǒng)，該系統(tǒng)可用一個(gè)線性隨機(jī)微分方程（linear stochastic difference equation）來(lái)描述：
$$X(k)=A?X(k?1)+B?U(k)+W(k)$$

這里：$X(k)$ 是 $k$ 時(shí)刻的系統(tǒng)狀態(tài)，$U(k)$ 是 $k$ 時(shí)刻對(duì)系統(tǒng)的控制量。$A$ 和 $B$ 是系統(tǒng)參數(shù)，對(duì)于多模系統(tǒng)，它們?yōu)榫仃嚒?/p>

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

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

對(duì)于滿足上面條件（線性隨機(jī)微分系統(tǒng)，過(guò)程和測(cè)量都是高斯白噪聲），卡爾曼濾波器是最優(yōu)的信息處理器。

---

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

首先利用系統(tǒng)過(guò)程模型，來(lái)預(yù)測(cè)下一個(gè)狀態(tài)的系統(tǒng)：
$$X(k|k?1)=A?X(k?1|k?1)+B?U(k)\text{\hspace{1em}(1)}$$

$X(k|k?1)$ 是利用上一狀態(tài)預(yù)測(cè)的結(jié)果，$X(k?1|k?1)$ 是上一狀態(tài)的最優(yōu)結(jié)果。$U(k)$ 為現(xiàn)在狀態(tài)的控制量，若沒(méi)有，則為 $0$。

現(xiàn)在更新 $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)$ 對(duì)應(yīng)的 covariance，$P(k?1|k?1)$ 是 $X(k?1|k?1)$ 對(duì)應(yīng)的 covariance。$A^{\prime }$ 表示 $A$ 的轉(zhuǎn)置，$Q$ 是系統(tǒng)的 covariance。

式（1）（2）用來(lái)對(duì)系統(tǒng)進(jìn)行預(yù)測(cè)。

然后再收集系統(tǒng)的觀測(cè)值 $Z(k)$，則最優(yōu)值：
$$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)}$$

到目前為止，已經(jīng)得到 $k$ 狀態(tài)下的最優(yōu)估算值 $X(k|k?1)$。

---

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

這里 $I$ 為 $1$ 的矩陣，對(duì)于單模系統(tǒng) $I=1$。

當(dāng)系統(tǒng)進(jìn)入 $k+1$ 狀態(tài)時(shí)，$P(k|k)$ 就是式（2）中的 $P(k?1|k?1)$。這樣，算法就可以自回歸地運(yùn)算下去。

擴(kuò)展卡爾曼濾波器

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

首先要了解一下線性化卡爾曼濾波。它和線性卡爾曼濾波器在濾波器的算法方面有同樣的算法結(jié)構(gòu)。不一樣的地方在于這兩者的系統(tǒng)模型不同。線性卡爾曼濾波器的系統(tǒng)本身就是線性系統(tǒng)，而線性化卡爾曼濾波器的系統(tǒng)本身是非線性系統(tǒng)，但是機(jī)智的大神們將非線性的系統(tǒng)進(jìn)行了線性化，于是卡爾曼濾波就可以用在非線性系統(tǒng)中了。對(duì)于一個(gè)卡爾曼濾波器的設(shè)計(jì)者，就可以不管模型到底是一開(kāi)始就是線性系統(tǒng)還是非線性系統(tǒng)線性化得到的線性系統(tǒng)，反正只要是線性系統(tǒng)就好了。

但是線性化卡爾曼濾波器會(huì)發(fā)散。為什么會(huì)發(fā)散呢？是這樣，我們?cè)趯?duì)非線性系統(tǒng)進(jìn)行線性化的過(guò)程中，只有被線性化的那個(gè)點(diǎn)附近的線性化模型和真實(shí)的模型相近，遠(yuǎn)的誤差就大了，那么這個(gè)時(shí)候卡爾曼濾波器的效果就不好。所以線性化的這個(gè)限制要時(shí)刻考慮，這也就是為什么要把線性卡爾曼濾波器和線性化卡爾曼濾波器區(qū)分開(kāi)的理由。而決定一個(gè)線性化濾波器成功與否的關(guān)鍵就在于這個(gè)濾波器系統(tǒng)模型線性化得好不好。

EKF 基于線性化系統(tǒng)的思想，將系統(tǒng)函數(shù)的非線性函數(shù)作一階Taylor展開(kāi)，得到線性化系統(tǒng)方程。擴(kuò)展的卡爾曼濾波器算法就是適用于非線性系統(tǒng)的卡爾曼濾波器。它與經(jīng)典的線性卡爾曼濾波器很相似，算法步驟和結(jié)構(gòu)都相同。不同在于系統(tǒng)模型和矩陣 $A$ 和 $H$。

協(xié)方差

協(xié)方差（英語(yǔ)：Covariance），在概率論與統(tǒng)計(jì)學(xué)中用于衡量?jī)蓚€(gè)隨機(jī)變量的聯(lián)合變化程度。

若變量X的較大值主要與另一個(gè)變量Y的較大值相對(duì)應(yīng)，而兩者的較小值也相對(duì)應(yīng)，則可稱兩變量?jī)A向于表現(xiàn)出相似的行為，協(xié)方差為正。在相反的情況下，當(dāng)一個(gè)變量的較大值主要對(duì)應(yīng)于另一個(gè)變量的較小值時(shí)，則兩變量?jī)A向于表現(xiàn)出相反的行為，協(xié)方差為負(fù)。即協(xié)方差之正負(fù)號(hào)顯示著變量的相關(guān)性。

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

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

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

Ref：

MATLAB實(shí)現(xiàn)卡爾曼濾波器(KF、EKF)

協(xié)方差 - WikiPedia

卡爾曼濾波（Kalman Filter）原理與公式推導(dǎo)

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。