初探卡爾曼濾波的廣泛應(yīng)用

卡爾曼濾波是基于線性系統(tǒng)的基礎(chǔ)上，測(cè)量方差已知的前提下，在包含一系列噪聲的觀測(cè)數(shù)據(jù)中進(jìn)行系統(tǒng)狀態(tài)的最優(yōu)估計(jì)，得到誤差最小的估計(jì)值。

眾所周知該方法經(jīng)久不衰幾十年，涉及領(lǐng)域包括機(jī)器人導(dǎo)航、運(yùn)動(dòng)控制、傳感器融合、圖像處理等。僅在無人駕駛中，我們就多處看到它的身影，比如：

• 感知模塊
• 融合模塊
• 定位模塊
• 控制模塊

以感知中目標(biāo)跟蹤為例，卡爾曼濾波可以用來預(yù)測(cè)下一時(shí)刻行人運(yùn)動(dòng)的最優(yōu)位置，不僅可以濾除檢測(cè)帶來的虛警，還可以彌補(bǔ)偶發(fā)的漏檢，使目標(biāo)的運(yùn)動(dòng)過程更加平滑。如下圖所示：從T0到T7時(shí)刻，檢測(cè)器對(duì)行人A的觀測(cè)結(jié)果大部分是良好的，但是在T3時(shí)刻出現(xiàn)了虛警B，此時(shí)卡爾曼濾波器會(huì)根據(jù)T3的前后時(shí)刻來判斷行人B是一個(gè)新的對(duì)象還是噪聲；在T4時(shí)刻檢測(cè)器再次失效，行人A消失了，此時(shí)卡爾曼濾波器會(huì)根據(jù)T3時(shí)刻行人A的位置來估計(jì)T4時(shí)刻A可能運(yùn)動(dòng)到某處作為結(jié)果。

既然線性系統(tǒng)是卡爾曼濾波的前提，那么為何它還能如此廣泛應(yīng)用？

以車輛運(yùn)動(dòng)模型為例，雖然在整個(gè)運(yùn)動(dòng)過程中很難擬合出其運(yùn)動(dòng)規(guī)律，但是如果把每次運(yùn)動(dòng)的時(shí)間間隔都縮的很小，那么就可以近似的將車輛的運(yùn)動(dòng)看成勻速、勻加速、均減速等。

入門卡爾曼濾波的基本原理

卡爾曼濾波器采用遞歸的方式來解決線性問題，只需上一個(gè)時(shí)刻的估計(jì)值和當(dāng)前的測(cè)量值來進(jìn)行狀態(tài)估計(jì)，并使估計(jì)均方誤差最小。作為初步理解，我們只需要看下面兩個(gè)方程：

預(yù)測(cè)方程如下所示：

其中A是狀態(tài)轉(zhuǎn)移矩陣，用來表達(dá)如何將上一時(shí)刻的狀態(tài)量通過某種關(guān)系轉(zhuǎn)換成當(dāng)前狀態(tài)；B是控制輸入矩陣，但是在實(shí)際應(yīng)用中這一項(xiàng)通常為0；w表示的過程噪聲，是期望為0，方程為Q的高斯白噪聲。

觀測(cè)方程如下所示：

很多初學(xué)者會(huì)對(duì)觀測(cè)方程產(chǎn)生疑問，為什么它可以由狀態(tài)值和觀測(cè)誤差來表示？觀測(cè)量不是指?jìng)鞲衅髦苯虞敵鰡幔?/p>

其實(shí)它是對(duì)傳感器測(cè)量的模擬仿真，用真值加上誤差來表征傳感器的測(cè)量值，而H可以看成是狀態(tài)量到觀測(cè)量的一種變換關(guān)系。我們將狀態(tài)變換過程和模擬的觀測(cè)過程合起來就是下面的流程圖：

因此使用卡爾曼濾波器解決實(shí)際問題，就需要滿足兩大基本假設(shè)：

• 需要滿足線性系統(tǒng)
• 需要符合高斯分布

進(jìn)階卡爾曼濾波的公式推演

卡爾曼濾波過程有五大核心公式，手?jǐn)]版本如下：公式(1)(2)屬于預(yù)測(cè)過程，公式(3)(4)(5)屬于狀態(tài)更新。我們一條條來解釋這幾個(gè)公式：

上式是先驗(yàn)估計(jì)的求解過程：由上一時(shí)刻的最優(yōu)估計(jì)值結(jié)合控制輸入和環(huán)境噪聲來預(yù)測(cè)當(dāng)前的狀態(tài)估計(jì)值。在很多時(shí)候該公式會(huì)被簡(jiǎn)化成當(dāng)前估計(jì)值等于上一時(shí)刻最優(yōu)估計(jì)+噪聲，在目標(biāo)跟蹤任務(wù)中BU項(xiàng)是可以被忽略的，A表示的是狀態(tài)轉(zhuǎn)移矩陣或者說是運(yùn)動(dòng)模型，如何準(zhǔn)確的構(gòu)建狀態(tài)矩陣需要用戶結(jié)合自身場(chǎng)景來確定。所以像EKF，UKF等并非從算法上對(duì)KF做了什么優(yōu)化，而是搭建更合適的運(yùn)動(dòng)模型來模擬實(shí)際場(chǎng)景。

上式是先驗(yàn)估計(jì)的協(xié)方差求解過程：由上一時(shí)刻的先驗(yàn)估計(jì)協(xié)方差P和過程噪聲Q決定（推導(dǎo)的過程可以對(duì)公式（1）的當(dāng)前估計(jì)值求取協(xié)方差即可得到）。這里我們會(huì)發(fā)現(xiàn)先驗(yàn)估計(jì)協(xié)方差P中是包含過程噪聲Q的，所以當(dāng)P越小會(huì)導(dǎo)致卡爾曼增益K就越小（這里需要結(jié)合卡爾曼增益K的公式一起看）。

上式是更新卡爾曼增益的過程：它其實(shí)是一個(gè)中間變量，我們?cè)谡f先驗(yàn)估計(jì)協(xié)方差P的時(shí)候提到，P中是包括過程噪聲Q的，那么結(jié)合該式可以發(fā)現(xiàn)卡爾曼增益K的調(diào)節(jié)本質(zhì)上就是調(diào)節(jié)Q和R兩個(gè)噪聲值。

• K越小越相信模型的預(yù)測(cè)估計(jì)；
• K越大越相信傳感器中的觀測(cè)；
• 所以K的值和傳感器精度以及環(huán)境誤差有關(guān)；

上式是最優(yōu)估計(jì)的修正過程：這里Z是我們的觀測(cè)值，X相當(dāng)于我們的預(yù)測(cè)值，直接拿觀測(cè)值和預(yù)測(cè)值做差，用K來決定是相信觀測(cè)值Z多一些還是相信預(yù)測(cè)值X多一些。

上式是更新后驗(yàn)估計(jì)的協(xié)方差過程：主要用于下一次迭代的輸入，屬于一個(gè)中間變量，一旦P0這個(gè)初始值確定后，該值會(huì)慢慢趨于收斂（通過調(diào)Q）。算法調(diào)優(yōu)時(shí)可以不用特別關(guān)注該項(xiàng)。

深入卡爾曼濾波的參數(shù)調(diào)優(yōu)

卡爾曼濾波是一個(gè)需要手動(dòng)調(diào)參的算法，在上面的介紹中我們提到最優(yōu)估計(jì)值是先驗(yàn)估計(jì)和觀測(cè)值之間的權(quán)衡，而這個(gè)權(quán)重是通過卡爾曼增益K進(jìn)行調(diào)節(jié)的。通過K的推導(dǎo)公式可以發(fā)現(xiàn)它的大小取決于超參數(shù)Q和R。

回顧一下上一節(jié)公式中的Q和R是什么？

在卡爾曼濾波的過程中有兩種噪聲，過程噪聲和觀測(cè)噪聲：

• 過程噪聲：外界環(huán)境引入的誤差；
• 觀測(cè)噪聲：傳感器自身的誤差；

它們均符合正態(tài)分布，Q就是過程噪聲的方差，Q值越小表示對(duì)預(yù)測(cè)值的信任度越高，但是過小的Q也會(huì)引起系統(tǒng)發(fā)散；Q值越大表示對(duì)測(cè)量值的信任度就會(huì)變高。

R是觀測(cè)噪聲的方差，R值越小表示系統(tǒng)的初始增益大，收斂快更快，但是在穩(wěn)態(tài)情況下引入過多的噪聲容易出現(xiàn)震蕩不收斂的現(xiàn)象；R值越大表示對(duì)測(cè)量值的信任度降低，響應(yīng)也會(huì)隨著變慢。

另一個(gè)超參數(shù)就是P的初始值，它決定了濾波器初始的工作狀態(tài)，更準(zhǔn)確的說就是濾波器初始的收斂速度。調(diào)大P0能夠迭代出較大的初始增益，相應(yīng)的使濾波器更快的響應(yīng)輸入信號(hào)的變化。

所以卡爾曼濾波調(diào)參是在P0，Q，R之間追求系統(tǒng)和濾波之前的收斂平衡。對(duì)于初學(xué)者而言，通常不太關(guān)心P0，只需不為0即可；而Q和R需要一點(diǎn)點(diǎn)嘗試，適當(dāng)?shù)脑黾?減小參數(shù)，反復(fù)迭代才能逐步收斂于一個(gè)穩(wěn)定值。

實(shí)踐卡爾曼濾波的目標(biāo)跟蹤

多目標(biāo)跟蹤有很多方法，可以使用當(dāng)前幀和之前幀中的信息做當(dāng)前時(shí)刻的目標(biāo)跟蹤；也可以對(duì)每一幀的預(yù)測(cè)使用所有幀中的信息尋找全局最優(yōu)。

這里我們基于卡爾曼濾波算法將運(yùn)動(dòng)模型看似線性勻速運(yùn)動(dòng)來估計(jì)幀間位移，并結(jié)合匈牙利算法進(jìn)行預(yù)測(cè)的外接框和檢測(cè)的外接框做數(shù)據(jù)匹配，最終選擇合適的目標(biāo)外接框作為最優(yōu)跟蹤BBox。應(yīng)用于圖像空間需要以下幾步：

• IoU作為前后幀間目標(biāo)關(guān)聯(lián)的衡量標(biāo)準(zhǔn)；
• 卡爾曼濾波器預(yù)測(cè)目標(biāo)的當(dāng)前位置；
• 匈牙利算法進(jìn)行檢測(cè)框和預(yù)測(cè)框數(shù)據(jù)關(guān)聯(lián)；

估計(jì)模型

這里我們采用卡爾曼濾波對(duì)目標(biāo)的軌跡進(jìn)行預(yù)測(cè)，并且使用置信度較高的跟蹤結(jié)果進(jìn)行預(yù)測(cè)的修正。

數(shù)據(jù)關(guān)聯(lián)

這里我們采用帶權(quán)重的匈牙利算法，使用IoU構(gòu)建的權(quán)重作為成本矩陣，當(dāng)然這里的權(quán)重還可以以不同維度的特征做加權(quán)。

實(shí)驗(yàn)例子

下面是一個(gè)基于C++實(shí)現(xiàn)的檢測(cè)+跟蹤的例子，直接原生的算法未做任何優(yōu)化，所以并非工程可用，但也更能暴露算法本身的缺陷，從而加以針對(duì)性的優(yōu)化策略。從視頻中我們可以看到基于卡爾曼濾波+數(shù)據(jù)關(guān)聯(lián)的跟蹤算法對(duì)獨(dú)立目標(biāo)具有穩(wěn)定的輸出，但是當(dāng)目標(biāo)較遠(yuǎn)時(shí)或者存在遮擋的情況下，id就會(huì)發(fā)生漂移。我們可以從以下三個(gè)方面來分析：

• 檢測(cè)角度：基于檢測(cè)的跟蹤算法，必然檢測(cè)的穩(wěn)定性是關(guān)鍵，與其把重心放在跟蹤算法的優(yōu)化上，不如先把目標(biāo)檢測(cè)弄穩(wěn)定，好在當(dāng)下圖像級(jí)別的目標(biāo)檢測(cè)已經(jīng)達(dá)到了很高的性能，具備了模型小，推理快，精度高的優(yōu)勢(shì)。
• 數(shù)據(jù)關(guān)聯(lián)：視頻中的車輛跟蹤失效，很大一部分原因出在當(dāng)前幀的檢測(cè)框和基于上一幀的預(yù)測(cè)框之間外接框匹配算法上，這里僅使用了IoU來匹配兩個(gè)框的關(guān)聯(lián)性，但是此類形態(tài)上的重合度無法解決目標(biāo)被遮擋后的匹配問題，而且在遠(yuǎn)距離處外接框較小，當(dāng)目標(biāo)個(gè)數(shù)較多時(shí)，很容易造成混亂。至于如何優(yōu)化數(shù)據(jù)關(guān)聯(lián)算法，小伙伴們可以考慮特征級(jí)別或者多維度級(jí)聯(lián)的方式！
• 估計(jì)模型：這里說的就是卡爾曼濾波算法了，我們用的是均速模型表針車輛的運(yùn)動(dòng)模型，雖然極小的時(shí)間間隔中，這種模型帶來的誤差也能接受，但是為了更好的構(gòu)建運(yùn)動(dòng)方程，也可以考慮采用擴(kuò)展卡爾曼或者無跡卡爾曼來擬合車輛的非線性運(yùn)行。