MEMS慣性傳感器之卡爾曼濾波

01 說(shuō)在前面的話

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

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

02應(yīng)用舉例

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

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

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

03卡爾曼濾波器算法

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

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

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

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

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

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

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

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

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

03.1 預(yù)測(cè)階段

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

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

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

03.2 更新階段

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

因?yàn)橛^測(cè)過(guò)程中存在一個(gè)觀測(cè)誤差的協(xié)方差矩陣，我們可以給出一個(gè)觀測(cè)余量的協(xié)方差：

接下來(lái)給出一個(gè)卡爾曼增益(Kalman Gain)：

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

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

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

03.3 算法流程抽象

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

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

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

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

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

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

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

審核編輯：湯梓紅