卡曼濾波器入門教程一維卡曼濾波器 1

本章將描述一維卡爾曼濾波器，主要目標(biāo)是簡單直觀地解釋卡爾曼濾波器的概念，所以，這個過程并不會使用看起來復(fù)雜和令人困惑的數(shù)學(xué)工具,我們將一步一步的向卡爾曼濾波方程靠近。

• 首先，我們通過一個沒有過程噪聲的簡單示例來推導(dǎo)出卡爾曼濾波方程。
• 然后，我們添加過程噪聲。

無過程噪聲的一維卡爾曼濾波器

正如我前面提到的，卡爾曼濾波器有五個方程，我們已經(jīng)熟悉了其中兩個：

• 狀態(tài)更新方程。
• 狀態(tài)推導(dǎo)方程

在本章中，我們將導(dǎo)出另外三個卡爾曼濾波方程。讓我們回憶一下我們的第一個例子（金條重量測量）；我們進(jìn)行了多次測量，并通過計算平均值作為估計值。我們得到了以下結(jié)果：

在上圖中，可以看到不同迭代過程黃金重量的真實值、估計值以及測量值。測量值（藍(lán)色線）和真實值（綠色線）之間的差異被稱為測量誤差，由于測量誤差是隨機的，我們可以用方差（σ2）來描述它。測量誤差的方差可由天平供應(yīng)商提供或通過校準(zhǔn)程序得到，測量誤差的方差也被稱為測量不確定程度。我們用

r

表示測量不確定度。

估計值（紅線）和真實值（綠線）之間的差異被稱為估計誤差，可以看到，當(dāng)我們進(jìn)行的測量越多，估計誤差變得越來越小，并趨向于0，估計值趨向真實值，雖然我們不知道怎么精確描述估計誤差，但我們可以對估計過程中存在的不確定性進(jìn)行近似估計。我們用

p

表示估計的不確定度。

讓我們看看重量測量PDF（概率密度函數(shù)）。下圖顯示了金條重量的十個測量值。

• 藍(lán)色圓圈表示測量值。
• 真實值由紅色虛線表示。
• 綠線描述了測量的概率密度函數(shù)。
• 深綠色區(qū)域是測量的標(biāo)準(zhǔn)偏差（σ），即測量值位于該區(qū)域內(nèi)的概率為68.26%

如圖所見，10次測量中有8次接近真實值，都位于1σ區(qū)域內(nèi)。測量不確定度（r）就是測量的方差（σ2）。

一維下的卡爾曼增益方程

下面我先直接給出五個卡曼方程中的第三個方程：卡爾曼增益方程，數(shù)學(xué)推導(dǎo)將在后續(xù)章節(jié)中給出?？柭鲆妫ㄓ蒏n表示）決定當(dāng)前狀態(tài)估計值和測量值的權(quán)重，與α-β-γ參數(shù)不同，卡爾曼增益是每次迭代過程中動態(tài)計算的。在一維中，卡爾曼增益方程如下：

卡爾曼增益是介于0和1之間的數(shù)字：

0≤Kn≤1

讓我們重寫狀態(tài)更新方程：

由公式可見，卡爾曼增益（Kn）決定了測量值的權(quán)重，（1?Kn）決定了估計值的權(quán)重。當(dāng)測量不確定度非常大且估計不確定度很低時，卡爾曼增益接近于零，說明測量值不太可信，因此，我們對估計值給予很大的權(quán)重，對測量值給予較小的權(quán)重。相反，當(dāng)測量不確定度很低且估計不確定度大時，卡爾曼增益接近1，說明估計值不太可信，因此，我們對估計值給予了較低的權(quán)重，對測量值給予了較大的權(quán)重。如果測量不確定度等于估計不確定度，則卡爾曼增益等于0.5，說明兩者可信度一樣，最終結(jié)果取兩者平均。

卡爾曼增益表示測量值對估計值的影響。

卡爾曼增益方程是第三個卡爾曼濾波器方程。

一維下的估計不確定度

更新方程

下式定義了估計不確定度更新方程：

該方程更新了當(dāng)前狀態(tài)的估計不確定度，它又被稱為協(xié)方差更新方程，為什么是協(xié)方差？我們將在后續(xù)章節(jié)中看到這一點。從方程中可以看出，因為（1?Kn）≤1，隨著濾波器迭代次數(shù)增多，估計的不確定度不斷變小，當(dāng)測量不確定度大時，卡爾曼增益小，因此，估計不確定度的收斂將會很慢，然而，當(dāng)測量不確定度很小時，卡爾曼增益很高，因此，估計的不確定度將迅速收斂為零。

協(xié)方差更新方程是第四個卡爾曼濾波方程。

一維下的估計不確定度

推導(dǎo)方程

與狀態(tài)推導(dǎo)方程一樣，估計不確定度的推導(dǎo)方程也取決于動態(tài)系統(tǒng)的類型。在我們的第二個例子中，一維雷達(dá)的情形，預(yù)測的目標(biāo)位置是：

預(yù)測位置等于當(dāng)前估計位置加上當(dāng)前估計速度乘以時間，預(yù)測速度等于當(dāng)前速度估計值（假設(shè)恒定速度模型）。估計不確定度推導(dǎo)方程為：

預(yù)測位置的估計不確定度等于當(dāng)前位置估計不確定度加上當(dāng)前速度估計不確定度乘以時間平方，預(yù)測速度估計不確定度等于當(dāng)前速度估計不確定度（假設(shè)恒定速度模型）。在我們的第一個例子（金條重量測量）中，系統(tǒng)是恒定的，因此，估計不確定度推導(dǎo)方程為：

估計不確定度推導(dǎo)方程也被稱為協(xié)方差推導(dǎo)方程，這是第五個卡爾曼濾波方程。

匯總到一起：

將所有前面提到的碎片組合成一個算法，與α、β、γ濾波器一樣，卡爾曼濾波器也使用“測量、更新、預(yù)測”算法。下圖提供了算法的低級示意圖描述：

濾波器的輸入：

• 初始化

初始化只執(zhí)行一次，它提供兩個參數(shù)： 初始系統(tǒng)狀態(tài)

初始狀態(tài)不確定度（p1,0）初始化參數(shù)可以由另一個系統(tǒng)、另一個過程（例如，雷達(dá)中的搜索過程）或基于對經(jīng)驗、理論知識的猜測提供，即使初始化參數(shù)不精確，卡爾曼濾波器也能夠收斂到接近真實值。

• 測量

每個濾波器周期都要進(jìn)行測量，并提供兩個參數(shù)：除了測量值之外，卡爾曼濾波器還需要測量不確定度參數(shù)，通常，該參數(shù)由設(shè)備供應(yīng)商提供，或可通過設(shè)備校準(zhǔn)得出，雷達(dá)測量的不確定度取決于幾個參數(shù)，如SNR（信噪比）、波束寬度、帶寬、目標(biāo)時間、時鐘穩(wěn)定性等，并且雷達(dá)每次測量都有不同的信噪比、波束寬度和目標(biāo)時間，因此，雷達(dá)每次都需要重新計算測量的不確定度，并將其報告給跟蹤器。

濾波器輸出為：

系統(tǒng)狀態(tài)估計（

）估計不確定性（pn,n）除了系統(tǒng)狀態(tài)估計，卡爾曼濾波器還提供了估計不確定度！這個在前面已經(jīng)提到了，估計不確定度由以下公式給出：

并且pn,n隨著濾波器迭代增加不斷變小，因為（1?Kn）≤1.因此，可以根據(jù)任務(wù)要求的精度來決定測量多少次，如果我們正在測量建筑物高度，并且我們要求誤差在3厘米（σ）內(nèi)，我們應(yīng)該持續(xù)進(jìn)行測量，直到估計不確定度（σ2）小于9。下表總結(jié)了五個卡爾曼濾波器方程：

注意1: 狀態(tài)推導(dǎo)方程和協(xié)方差推導(dǎo)方程取決于系統(tǒng)模型。注意2: 上表展示了針對特定情況定制的卡爾曼濾波器方程的特殊形式，方程的一般形式將在后面的矩陣表示法中給出，現(xiàn)在，我們的目標(biāo)是理解卡爾曼濾波器的概念。

下圖提供了對于卡爾曼濾波器框圖的詳細(xì)描述：

步驟0：初始化如上所述，初始化只執(zhí)行一次，它提供兩個參數(shù)：初始系統(tǒng)狀態(tài)（

）初始狀態(tài)不確定性（p1,0）初始化之后是預(yù)測。步驟1：測量 測量過程提供兩個參數(shù)：系統(tǒng)狀態(tài)測量值（zn）測量不確定度（rn）步驟2：狀態(tài)更新狀態(tài)更新過程負(fù)責(zé)對系統(tǒng)當(dāng)前狀態(tài)的估計狀態(tài)更新過程輸入包括：測量值（zn）測量不確定度（rn）前一個系統(tǒng)狀態(tài)估計（

)估計不確定度（pn,n?1 )基于輸入，狀態(tài)更新過程計算卡爾曼增益并提供兩個輸出：當(dāng)前系統(tǒng)狀態(tài)估計（

）當(dāng)前狀態(tài)估計不確定性（pn,n）這些參數(shù)是卡爾曼濾波器輸出步驟3：預(yù)測預(yù)測過程基于動態(tài)系統(tǒng)模型，根據(jù)當(dāng)前系統(tǒng)狀態(tài)和當(dāng)前系統(tǒng)狀態(tài)估計不確定度推導(dǎo)得到下一個系統(tǒng)狀態(tài)，在第一次濾波器迭代時，初始化輸出被視為先前狀態(tài)估計和不確定度，預(yù)測輸出變成后續(xù)濾波器迭代中前一個狀態(tài)估計和不確定度。

卡爾曼增益直觀理解

在計算新的估計時，卡爾曼增益定義了測量的權(quán)重和先前估計的權(quán)重。

高卡爾曼增益：

相對于估計不確定度，低測量不確定性將導(dǎo)致高卡爾曼增益（接近1），因此，新的估計值將接近測量值，下圖說明了飛機跟蹤應(yīng)用中高卡爾曼增益對估計的影響。

低卡爾曼增益：

相對于估計不確定度，高測量不確定度將導(dǎo)致低卡爾曼增益（接近0），因此，新的估計值將接近先前的估計值，下圖說明了低卡爾曼增益對飛機跟蹤應(yīng)用中估計值的影響。

現(xiàn)在我們了解了卡爾曼濾波算法，并準(zhǔn)備好了第一個數(shù)值示例。