01 說在前面的話

關(guān)于卡爾曼這部分內(nèi)容，一直思考怎么用通俗易懂的方式來按照自己的理解梳理。但是無奈，總是繞不開用數(shù)學(xué)的方法表達那 5 個關(guān)鍵的公式。不論是在B站，還是各個博客都用很多的講解。直到理解了嚴恭敏老師在講卡爾曼濾波時的一個溫度估算舉例，才斗膽引用下面文章，來填寫卡爾曼濾波這個坑。

一個簡單的應(yīng)用是估計物體的位置和速度。簡要描述如下：假設(shè)我們可以獲取一個物體的包含噪聲的一系列位置觀測數(shù)據(jù)，我們可以獲得此物體的精確速度和位置連續(xù)更新信息。例如，對于雷達來說，我們關(guān)心的是跟蹤目標(biāo)，而目標(biāo)的位置、速度、加速度的觀測值是時刻含有誤差的，卡爾曼濾波器利用目標(biāo)的動態(tài)信息，去掉噪聲影響，獲取目標(biāo)此刻好的位置估計(即濾波過程)，將來的位置估計(即預(yù)測過程)，也可以是過去位置估計的(即插值或平滑過程)。

02應(yīng)用舉例

假設(shè)我們要研究的對象是一個房間的溫度。根據(jù)你的經(jīng)驗判斷，這個房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度(假設(shè)我們用一分鐘來做時間單位)。 假設(shè)你對你的經(jīng)驗不是絕對相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲(White Gaussian Noise，理想情況下我們以高斯噪聲來進行假設(shè)估計)，也就是這些偏差跟前后時間是沒有關(guān)系的而且符合高斯分布(Gaussian Distribution)。 另外，我們在房間里放一個溫度計，但是這個溫度計也不準確的，觀測值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。好了，現(xiàn)在對于某一分鐘我們有兩個有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗的預(yù)測值(系統(tǒng)的預(yù)測值)和溫度計的值(觀測值)。下面我們要用這兩個值結(jié)合它們各自的噪聲來估算出房間的實際溫度值。 假如我們要估算k時刻的實際溫度值。首先你要根據(jù)k?1時刻的溫度值，來預(yù)測k時刻的溫度。因為你相信溫度是恒定的，所以你會得到k時刻的溫度預(yù)測值是跟k?1時刻一樣的，假設(shè)是23度，同時該值的高斯噪聲的偏差是5度(5是這樣得到的：如果k?1時刻估算出的最優(yōu)溫度值的偏差是3，你對自己預(yù)測的不確定度是4度，它們平方相加再開方就是5)。 然后，你從溫度計那里得到了k時刻的溫度值，假設(shè)是25度，同時該值的偏差是4度。由于我們用于估算k時刻的實際溫度有兩個溫度值，分別是23度和25度。究竟實際溫度是多少呢?相信自己還是相信溫度計呢?究竟相信誰多一點，我們可以用它們的協(xié)方差(covariance)來判斷。因為 Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時刻的實際溫度值是：23+0.78?(25?23)=24.56度。

可以看出，因為溫度計的協(xié)方差比較小(比較相信溫度計)，所以估算出的最優(yōu)溫度值偏向溫度計的值。現(xiàn)在我們已經(jīng)得到k時刻的最優(yōu)溫度值了，下一步就是要進入k+1時刻，進行新的最優(yōu)估算。在進入k+1時刻之前，我們還要算出k時刻那個最優(yōu)值(24.56 度)的偏差。算法如下：

這里的5就是上面的k時刻你預(yù)測的那個23度溫度值的偏差，得出的2.35就是進入k+1時刻以后k時刻估算出的最優(yōu)溫度值的偏差(對應(yīng)于上面的3)。就是這樣，卡爾曼濾波器就不斷的把協(xié)方差遞歸，從而估算出最優(yōu)的溫度值。它運行的很快，而且它只保留了上一時刻的協(xié)方差。上面的Kg，就是卡爾曼增益(Kalman Gain)，可以隨不同的時刻而改變自己的值。

03卡爾曼濾波器算法

卡爾曼濾波基于時域描述的線性動態(tài)系統(tǒng)，它的模型是馬爾科夫鏈(Markov Chain)，而馬爾科夫鏈建立在一個被高斯噪聲干擾的線性算子之上。 系統(tǒng)的狀態(tài)可以用一個元素為實數(shù)的向量表示。隨著離散時間的增加，這個線性算子就會作用到當(dāng)前狀態(tài)之上，產(chǎn)生一個新的狀態(tài)，并且會帶入一定的噪聲，同時一些已知的控制信息也會加入。同時，另外一個受噪聲干擾的線性算子將產(chǎn)生這些隱含狀態(tài)的可見輸出。卡爾曼濾波器可以被看作為類隱馬爾科夫模型，它們的顯著不同點在于：

隱狀態(tài)變量的取值空間是一個連續(xù)的空間，而不是離散的狀態(tài)空間;

另外，隱馬爾科夫模型可以描述下一個狀態(tài)的一個任意分布，這也與應(yīng)用于卡爾曼濾波器中的高斯噪聲模型相反。

先看一下動態(tài)系統(tǒng)的基本模型。

首先，我們先要引入一個離散控制過程的系統(tǒng)。該系統(tǒng)的過程模型可用一個線性隨機微分方程(Linear Stochastic Difference Equation)來描述：

再加上系統(tǒng)觀測模型：

卡爾曼濾波是一種遞歸的估計，即只要獲知上一時刻狀態(tài)的估計值以及當(dāng)前狀態(tài)的觀測值就可以計算出當(dāng)前狀態(tài)的估計值，因此不需要記錄觀測或者估計的歷史信息。卡爾曼濾波器與大多數(shù)濾波器不同之處在于：它是一種純粹的時域濾波器，它不需要像低通濾波器等頻域濾波器那樣，需要在頻域設(shè)計再轉(zhuǎn)換到時域?qū)崿F(xiàn)。卡爾曼濾波器的操作包括兩個階段：預(yù)測與更新：

在預(yù)測階段，濾波器使用上一狀態(tài)的估計，做出對當(dāng)前狀態(tài)的估計。

在更新階段，濾波器利用對當(dāng)前狀態(tài)的觀測值優(yōu)化在預(yù)測階段獲得的預(yù)測值，以獲得一個更精確的新估計值。

03.1 預(yù)測階段

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

下面我們來用它們結(jié)合它們的協(xié)方差來估算系統(tǒng)的最優(yōu)化輸出(類似上一節(jié)那個溫度的例子)。首先，我們要利用系統(tǒng)的過程模型，來預(yù)測下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在狀態(tài)(稱為預(yù)測的狀態(tài)估計方程)：

到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對應(yīng)于x(k∣k?1)的協(xié)方差(covariance)還沒更新。我們用P表示協(xié)方差，它實際上描述了預(yù)測值的準確程度(稱為預(yù)測的協(xié)方差矩陣估計方程)：

03.2 更新階段

在進行更新之前，我們先計算三個值： 首先是觀測余量(measurement residual)：

因為觀測過程中存在一個觀測誤差的協(xié)方差矩陣，我們可以給出一個觀測余量的協(xié)方差：

接下來給出一個卡爾曼增益(Kalman Gain)：

現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測結(jié)果，然后我們再收集現(xiàn)在狀態(tài)的觀測值。結(jié)合預(yù)測值和觀測值，我們可以得到現(xiàn)在狀態(tài)k的最優(yōu)化估算值x(k|k)(稱為更新的狀態(tài)估計方程)：

到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值x(k∣k)。但是為了要使得卡爾曼濾波器不斷的運行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下x(k∣k)的協(xié)方差(稱為更新的協(xié)方差矩陣估計方程)：

其中 I 為單位矩陣，對于單模型單觀測，I=1。當(dāng)系統(tǒng)進入k+1狀態(tài)時，P(k∣k)就是預(yù)測方程中的P(k?1∣k?1)。這樣，算法就可以自回歸的運算下去。

03.3 算法流程抽象

1)先決定當(dāng)前系統(tǒng)的初始狀態(tài)，并根據(jù)預(yù)測方程(過程模型)得到一個下一個時刻預(yù)測的狀態(tài);

2)根據(jù)預(yù)測方程中過程的誤差，得到當(dāng)前預(yù)測的協(xié)方差估計;

3)進入更新階段，我們根據(jù)目前系統(tǒng)的觀測值和上一個時刻預(yù)測的狀態(tài)，從轉(zhuǎn)換方程(觀測模型)入手，得到一個測量余量;

4)根據(jù)轉(zhuǎn)換方程和上個時刻預(yù)測的協(xié)方差估計，也可以得到一個測量余量的協(xié)方差估計;

5)根據(jù) a)測量余量的協(xié)方差Sk∣k; b)轉(zhuǎn)換方程H和 c)上個時刻的預(yù)測協(xié)方差估計Pk∣k?1，我們得到卡爾曼增益Kk∣k;

6)根據(jù)卡爾曼增益和測量余量，我們從預(yù)測的狀態(tài)中更新優(yōu)化當(dāng)前的狀態(tài)的值，而這個值可以用來預(yù)測下一個時刻的狀態(tài);

7)同樣，我們根據(jù) a)卡爾曼增益Kk∣k 和 b)上個時刻的預(yù)測協(xié)方差估計Pk∣k?1，我們把當(dāng)前更新階段的協(xié)方差Pk∣k估計也得到，幫助下一時刻的卡爾曼增益計算。

審核編輯：湯梓紅