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

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

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

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

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

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

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

r

表示測量不確定度。

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

p

表示估計(jì)的不確定度。

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

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

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

一維下的卡爾曼增益方程

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

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

0≤Kn≤1

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

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

卡爾曼增益表示測量值對(duì)估計(jì)值的影響。

卡爾曼增益方程是第三個(gè)卡爾曼濾波器方程。

一維下的估計(jì)不確定度

更新方程

下式定義了估計(jì)不確定度更新方程：

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

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

一維下的估計(jì)不確定度

推導(dǎo)方程

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

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

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

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

匯總到一起：

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

濾波器的輸入：

• 初始化

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

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

• 測量

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

濾波器輸出為：

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

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

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

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

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

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

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

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

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

卡爾曼增益直觀理解

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

高卡爾曼增益：

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

低卡爾曼增益：

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

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