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

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

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

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

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

• 狀態更新方程。
• 狀態推導方程

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

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

r

表示測量不確定度。

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

p

表示估計的不確定度。

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

• 藍色圓圈表示測量值。
• 真實值由紅色虛線表示。
• 綠線描述了測量的概率密度函數。
• 深綠色區域是測量的標準偏差（σ），即測量值位于該區域內的概率為68.26%

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

一維下的卡爾曼增益方程

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

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

0≤Kn≤1

讓我們重寫狀態更新方程：

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

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

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

一維下的估計不確定度

更新方程

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

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

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

一維下的估計不確定度

推導方程

與狀態推導方程一樣，估計不確定度的推導方程也取決于動態系統的類型。在我們的第二個例子中，一維雷達的情形，預測的目標位置是：

預測位置等于當前估計位置加上當前估計速度乘以時間，預測速度等于當前速度估計值（假設恒定速度模型）。估計不確定度推導方程為：

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

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

匯總到一起：

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

濾波器的輸入：

• 初始化

初始化只執行一次，它提供兩個參數： 初始系統狀態

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

• 測量

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

濾波器輸出為：

系統狀態估計（

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

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

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

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

步驟0：初始化如上所述，初始化只執行一次，它提供兩個參數：初始系統狀態（

）初始狀態不確定性（p1,0）初始化之后是預測。步驟1：測量 測量過程提供兩個參數：系統狀態測量值（zn）測量不確定度（rn）步驟2：狀態更新狀態更新過程負責對系統當前狀態的估計狀態更新過程輸入包括：測量值（zn）測量不確定度（rn）前一個系統狀態估計（

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

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

卡爾曼增益直觀理解

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

高卡爾曼增益：

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

低卡爾曼增益：

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

現在我們了解了卡爾曼濾波算法，并準備好了第一個數值示例。