初探卡爾曼濾波的廣泛應用

卡爾曼濾波是基于線性系統的基礎上，測量方差已知的前提下，在包含一系列噪聲的觀測數據中進行系統狀態的最優估計，得到誤差最小的估計值。

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

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

以感知中目標跟蹤為例，卡爾曼濾波可以用來預測下一時刻行人運動的最優位置，不僅可以濾除檢測帶來的虛警，還可以彌補偶發的漏檢，使目標的運動過程更加平滑。如下圖所示：從T0到T7時刻，檢測器對行人A的觀測結果大部分是良好的，但是在T3時刻出現了虛警B，此時卡爾曼濾波器會根據T3的前后時刻來判斷行人B是一個新的對象還是噪聲；在T4時刻檢測器再次失效，行人A消失了，此時卡爾曼濾波器會根據T3時刻行人A的位置來估計T4時刻A可能運動到某處作為結果。

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

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

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

卡爾曼濾波器采用遞歸的方式來解決線性問題，只需上一個時刻的估計值和當前的測量值來進行狀態估計，并使估計均方誤差最小。作為初步理解，我們只需要看下面兩個方程：

預測方程如下所示：

其中A是狀態轉移矩陣，用來表達如何將上一時刻的狀態量通過某種關系轉換成當前狀態；B是控制輸入矩陣，但是在實際應用中這一項通常為0；w表示的過程噪聲，是期望為0，方程為Q的高斯白噪聲。

觀測方程如下所示：

很多初學者會對觀測方程產生疑問，為什么它可以由狀態值和觀測誤差來表示？觀測量不是指傳感器直接輸出嗎？

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

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

• 需要滿足線性系統
• 需要符合高斯分布

進階卡爾曼濾波的公式推演

卡爾曼濾波過程有五大核心公式，手擼版本如下：公式(1)(2)屬于預測過程，公式(3)(4)(5)屬于狀態更新。我們一條條來解釋這幾個公式：

上式是先驗估計的求解過程：由上一時刻的最優估計值結合控制輸入和環境噪聲來預測當前的狀態估計值。在很多時候該公式會被簡化成當前估計值等于上一時刻最優估計+噪聲，在目標跟蹤任務中BU項是可以被忽略的，A表示的是狀態轉移矩陣或者說是運動模型，如何準確的構建狀態矩陣需要用戶結合自身場景來確定。所以像EKF，UKF等并非從算法上對KF做了什么優化，而是搭建更合適的運動模型來模擬實際場景。

上式是先驗估計的協方差求解過程：由上一時刻的先驗估計協方差P和過程噪聲Q決定（推導的過程可以對公式（1）的當前估計值求取協方差即可得到）。這里我們會發現先驗估計協方差P中是包含過程噪聲Q的，所以當P越小會導致卡爾曼增益K就越小（這里需要結合卡爾曼增益K的公式一起看）。

上式是更新卡爾曼增益的過程：它其實是一個中間變量，我們在說先驗估計協方差P的時候提到，P中是包括過程噪聲Q的，那么結合該式可以發現卡爾曼增益K的調節本質上就是調節Q和R兩個噪聲值。

• K越小越相信模型的預測估計；
• K越大越相信傳感器中的觀測；
• 所以K的值和傳感器精度以及環境誤差有關；

上式是最優估計的修正過程：這里Z是我們的觀測值，X相當于我們的預測值，直接拿觀測值和預測值做差，用K來決定是相信觀測值Z多一些還是相信預測值X多一些。

上式是更新后驗估計的協方差過程：主要用于下一次迭代的輸入，屬于一個中間變量，一旦P0這個初始值確定后，該值會慢慢趨于收斂（通過調Q）。算法調優時可以不用特別關注該項。

深入卡爾曼濾波的參數調優

卡爾曼濾波是一個需要手動調參的算法，在上面的介紹中我們提到最優估計值是先驗估計和觀測值之間的權衡，而這個權重是通過卡爾曼增益K進行調節的。通過K的推導公式可以發現它的大小取決于超參數Q和R。

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

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

• 過程噪聲：外界環境引入的誤差；
• 觀測噪聲：傳感器自身的誤差；

它們均符合正態分布，Q就是過程噪聲的方差，Q值越小表示對預測值的信任度越高，但是過小的Q也會引起系統發散；Q值越大表示對測量值的信任度就會變高。

R是觀測噪聲的方差，R值越小表示系統的初始增益大，收斂快更快，但是在穩態情況下引入過多的噪聲容易出現震蕩不收斂的現象；R值越大表示對測量值的信任度降低，響應也會隨著變慢。

另一個超參數就是P的初始值，它決定了濾波器初始的工作狀態，更準確的說就是濾波器初始的收斂速度。調大P0能夠迭代出較大的初始增益，相應的使濾波器更快的響應輸入信號的變化。

所以卡爾曼濾波調參是在P0，Q，R之間追求系統和濾波之前的收斂平衡。對于初學者而言，通常不太關心P0，只需不為0即可；而Q和R需要一點點嘗試，適當的增加/減小參數，反復迭代才能逐步收斂于一個穩定值。

實踐卡爾曼濾波的目標跟蹤

多目標跟蹤有很多方法，可以使用當前幀和之前幀中的信息做當前時刻的目標跟蹤；也可以對每一幀的預測使用所有幀中的信息尋找全局最優。

這里我們基于卡爾曼濾波算法將運動模型看似線性勻速運動來估計幀間位移，并結合匈牙利算法進行預測的外接框和檢測的外接框做數據匹配，最終選擇合適的目標外接框作為最優跟蹤BBox。應用于圖像空間需要以下幾步：

• IoU作為前后幀間目標關聯的衡量標準；
• 卡爾曼濾波器預測目標的當前位置；
• 匈牙利算法進行檢測框和預測框數據關聯；

估計模型

這里我們采用卡爾曼濾波對目標的軌跡進行預測，并且使用置信度較高的跟蹤結果進行預測的修正。

數據關聯

這里我們采用帶權重的匈牙利算法，使用IoU構建的權重作為成本矩陣，當然這里的權重還可以以不同維度的特征做加權。

實驗例子

下面是一個基于C++實現的檢測+跟蹤的例子，直接原生的算法未做任何優化，所以并非工程可用，但也更能暴露算法本身的缺陷，從而加以針對性的優化策略。從視頻中我們可以看到基于卡爾曼濾波+數據關聯的跟蹤算法對獨立目標具有穩定的輸出，但是當目標較遠時或者存在遮擋的情況下，id就會發生漂移。我們可以從以下三個方面來分析：

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