追狗，從入門到精通2.0

作者：望墨溢（西北工業(yè)大學 航海學院）

文章來源于科學大院公眾號（ID：kexuedayuan）

——

上一次教大家追狗，不知道大家追上沒有？新來的小伙伴可以 → 《追狗，從入門到精通1.0》 學習一下。聽說經過半年的發(fā)展，狗是越來越難追。不過，今天大院er就給大家升級一下攻略，保準你成為追狗大師~

（圖片來源：作者繪制）

單人追狗：Kalman濾波器

首先，我們來復習一下一個人是怎么追狗的。如果你提著菜刀要去收拾一條剛拆完家的二哈，可以借助Kalman濾波器[1]，它的原理是：

（1）上一時刻的狗：已知上一時刻狗跑到了狗窩處，誤差方差為1cm2的誤差（只要測量就存在誤差）；

（2）預測當前時刻的狗：按照狗的運動方式（運動模型，Motion Model），它當前時刻應該跑到冰箱處，這一過程有方差為5 cm2的誤差（狗也不那么肯定自己完全跑直線）。由于上一時刻狗窩的1 cm2方差，因此預測的方差應該是1+5=6 cm2；

（圖片來源：作者繪制）

（3）當前時刻的量測：若你（量測模型，Measurement Model）看到它在電視處，但這個量測是有方差為10 cm2的誤差（眼睛總有誤差）；

（4）狀態(tài)更新：這兩個信息（預測和量測）都不完全可信，但可以知道，預測相對于量測更的可信度可信為10/（10+6）=10/16，因此我們可以用預測位置乘以可信度，用量測位置乘以6/16，兩個結果相加，表示融合預測和量測信號后的估計（Estimate）；

（5）誤差方差：這一估計的誤差方差是多少呢？由于是用量測修正預測，那么應該是預測誤差-量測的相對誤差，即6*(1-6/16)=3.75 cm2。

（圖片來源：作者繪制）

估計誤差的方差3.75 cm2，既小于量測的10 cm2，也小于預測的6 cm2，如此一來，便實現(xiàn)了準確的追狗（目標跟蹤，Target Tracking）。

Kalman濾波可以總結為“預測-更新”兩個步驟，來得到濾波結果和方差

（圖片來源：作者繪制）

為了得到二哈更加準確的位置，我們既可以優(yōu)化硬件，也可以優(yōu)化濾波算法，但還有一種方案，就是讓多人來追狗，將多人的信息融合成一個更為可靠的信息，這就是用于目標跟蹤的傳感器信息融合（Sensor Information Fusion）。

融合跟蹤按照處理的是量測還是濾波結果，可分為集中式融合（Centralized Fusion）和分布式融合（Distributed Fusion）兩類。

多人追狗：集中式融合跟蹤

算法一：并行融合（Parallel Fusion）[2]

你叫了大強二強來一起追狗，已知你們三人的量測模型、量測和量測噪聲方差（例如大強看狗得到量測1）

（圖片來源：作者繪制）

那么，可將這些信息進行擴維重組（Augment）（類似汽車人合體），相當于有個“合體強”看到了“合體量測”，方差為“合體方差”。

（圖片來源：作者繪制）

然后將擴維后的“合體信息”輸入至Kalman濾波器，即可融合多人信息，更為準確地追狗。

（圖片來源：作者繪制）

算法二：序貫融合（Sequential Fusion）

再回顧一下Kalman濾波器的原理：先預測，再用量測對預測進行更新。若進行一次預測后，先用第一個量測對其更新，然后將更新結果再次視為預測，再用第二個量測對其更新，……，直至融合所有量測，這就是序貫融合算法。

相比于并行融合算法一次性融合所有量測，序貫融合算法是按照某種順序依次融合各傳感器量測。

（圖片來源：作者繪制）

可以證明，序貫融合有著和并行融合相同的性能，均為全局最優(yōu)（Global Optimal）。但序貫融合有一個顯著的優(yōu)勢：可以融合不同步的信息，因此它在工程上應用更為廣泛。集中式異步融合（Asynchronous Fusion），便是以序貫融合為基本原理。

多人追狗：分布式融合跟蹤

算法三：凸組合融合

（Convex Combination Fusion）

集中式融合處理的是量測，而分布式融合處理的是傳感器的濾波結果。假設大強、二強和你都用Kalman濾波器，估計狗的位置和方差。而誰的方差越大，說明誰的估計越不可靠，因此，可用方差的倒數(shù)作為權系數(shù)，對估計進行融合。

假設大強認為狗子在X1處，方差為P1；二強認為狗子在X2處，方差為P2；你認為狗子在X3處，方差為P3，那么根據(jù)凸組合融合算法，二哈的最終位置確定在X=X1/P1+X2/P2+X3/P3（待歸一化）。

（圖片來源：作者繪制）

可以證明，凸組合融合結果的方差比任意一個局域傳感器濾波結果的方差都小，即提高了追狗的性能。

（圖片來源：作者繪制）

凸組合融合算法認為，不同估計間互不相關，而這一點在現(xiàn)實中難以滿足，因此又有人提出Bar Shalom-Campo融合算法。在凸組合融合的思路上，權系數(shù)計算減掉了互協(xié)方差（Cross Covariance），去除了相關性。

算法四：協(xié)方差交叉融合

（Covariance Intersection Fusion）[3]

有時，Bar Shalom-Campo融合算法所需的互協(xié)方差難以計算，因此，又有人提出了協(xié)方差交叉融合算法。以大強和二強為例，若有一系數(shù)w，使得w*P1+(1-w)*P2最小，那么就可以認為可靠的融合結果是w*待歸一化的X1+(1-w)* 待歸一化的X2。

（圖片來源：作者繪制）

在尋找最優(yōu)融合系數(shù)時，常用黃金分割法（Golden Section Algorithm）進行實現(xiàn)。

分布式融合算法往往滿足交換律（Commutative Law），即（大強融二強）融你、（大強融你）融二強、大強融（你融二強）的結果是一樣的。因此在工程實踐中，分布式融合算法有著獨特的優(yōu)勢。

（圖片來源：作者繪制）

追狗，永無止境

融合跟蹤在工程應用中依舊面臨很多問題，下面這兩個問題最為重要：

（1）信息不同步。由于傳感器距離目標的距離不同，探測信號到達目標所需的時間也不同，往往不同傳感器觀測到的是不同時間不同位置的目標，這就對融合提出了新的挑戰(zhàn)。

（圖片來源：作者繪制）

（2）異類信息融合。在現(xiàn)實的傳感器網絡中，往往會出現(xiàn)異類傳感器（Heterogeneous Sensor）。例如，雷達傳感器既能測角度也能測距離，而紅外傳感器只能測角度。如何融合這種量測內容不同的信息，也是難題之一。

（圖片來源：作者繪制）

在理想情況下，融合跟蹤技術已趨于成熟，那么這一領域的人還研究什么呢？當然是不理想情況下的應用。理想的融合跟蹤算法根本無法應用到實際，不過這幾年“協(xié)同”概念的興起，必然要求融合跟蹤算法的進一步發(fā)展。

追狗，永無止境。

參考文獻:

[1] R E Kalman. A new approach to linear filtering and prediction problem, Trans. ASME. J. Basic Engineering, 1960, 82: 34-45

[2] Roecker J A, McGillem C D. Comparison of two-sensor tracking methods based on state vector fusion and measurement fusion. IEEE Transactions on Aerospace and Electronic Systems, 1988, 24(4): 447-449

[3] S. J. Julier, J. K. Uhlmann. Using covariance intersection for SLAM[J]. Robotics and Autonomous Systems, 2007, 55(1): 3-20.