重磅干貨,，第一時間送達(dá) 
卡爾曼濾波器是一種復(fù)雜的算法,，在大多數(shù)情況下，我們在沒有完全理解其方程的情況下使用它,。 當(dāng)我開始使用卡爾曼濾波器時，我也是這樣做的,。我閱讀了一堆試圖直觀地解釋算法的教程,，但在這樣做時，忽略了關(guān)于協(xié)方差矩陣作用的關(guān)鍵部分,。 我正在開發(fā)的產(chǎn)品是基于檢測的對象跟蹤器,。在其中，卡爾曼濾波器用于預(yù)測和更新給定視頻流的對象的位置和速度,，以及對每一幀的檢測,。在某些時候，我們還想通過在給定運(yùn)動角度的情況下調(diào)整對象的位置來考慮相機(jī)的傾斜和平移,。當(dāng)然,，物體的速度應(yīng)該不受這種相機(jī)運(yùn)動的影響。 但是,，當(dāng)開始實(shí)施這種調(diào)整時,，我意識到我并不真正了解速度是如何估計的。我假設(shè)在這個過程中的某個地方,，在給定先前和當(dāng)前位置的情況下會明確計算速度（在我們的例子中,，速度永遠(yuǎn)不會直接測量）。我還假設(shè)以與位置相同的方式處理此速度。令我驚訝的是,，這兩個假設(shè)都被證明是錯誤的,。 這使我深入研究了卡爾曼濾波器的方程，試圖盡可能直觀地弄清楚到底發(fā)生了什么,，但沒有過度簡化,。本文是我在這個過程中學(xué)到的東西的總結(jié)。本文中我們需要小伙伴熟悉概率論中的基本概念,，例如概率分布函數(shù),、高斯和貝葉斯定理。此外,，我們將使用矩陣乘法和加法進(jìn)行一些計算,。因此，如果小伙伴也熟悉這一點(diǎn),，那就更好了,。 卡爾曼濾波器是一種算法，旨在隨著時間的推移估計測量變量的值,，給定這些變量的連續(xù)測量值以及這些測量值的不確定性,。卡爾曼濾波器還考慮了估計變量之間的給定關(guān)系,。 讓我們以在視頻中跟蹤乒乓球的過程為例,，給定球的 2D 邊界框檢測。檢測可能在給定 bbox 的坐標(biāo)中或由誤報/負(fù)數(shù)引起的坐標(biāo)中包含一些不準(zhǔn)確之處,。 為了更順暢地跟蹤球,，我們將要跟蹤測量的中心坐標(biāo) ( x , y )、bbox 的寬度和高度,。假設(shè)移動球的速度可以在短時間內(nèi)近似為常數(shù),，我們還希望跟蹤 x 和 y 速度（v_x，v_y）,，希望得到這些預(yù)測： 通常,，當(dāng)試圖解釋卡爾曼濾波器時，人們會使用一個例子來跟蹤一個物體,，使用它的位置和速度的測量值（例如 GPS 和速度計）,。在我們的示例中，我們沒有直接測量速度,。相反,，卡爾曼濾波器使用測量的位置和我們注入其方程的先驗(yàn)知識來推斷這個速度，這些知識與我們期望的運(yùn)動學(xué)行為有關(guān)（在這種情況下是恒定的速度）,。 為簡單起見,，從現(xiàn)在開始我們將只討論x和v_x 的跟蹤（一維跟蹤示例）,。在了解 1D 中發(fā)生了什么之后，邏輯可以很容易地擴(kuò)展到 2 維和 3 維,，因?yàn)樵谖覀兊氖纠咐?，每個維度都完全獨(dú)立于其他維度。 假設(shè) 使用卡爾曼濾波器時有兩個重要的假設(shè)： 傳感器有噪聲,，其輸出和噪聲可以通過高斯概率分布函數(shù) (PDF)準(zhǔn)確建模,。 我們對實(shí)際狀態(tài)的初始先驗(yàn)知識也是一個高斯 PDF。
在我們的示例中,，傳感器輸出由一維高斯表示,，其中平均值等于實(shí)際傳感器輸出，方差是傳感器的不確定性（主要是根據(jù)經(jīng)驗(yàn)測量）,。
先驗(yàn)高斯是二維的（一維表示位置,，一維表示速度）?？梢允褂?6 個數(shù)字完全描述 2D 高斯：2 表示其中心坐標(biāo)向量（也稱為均值）,，另外 4 表示 2x2 協(xié)方差矩陣。 
協(xié)方差矩陣的對角線對應(yīng)于每個維度的方差,，2個非對角線元素是位置和速度之間的實(shí)際協(xié)方差,。由于協(xié)方差的對稱性，非對角線元素總是相同的,，即 x 和 y 之間的相關(guān)性與 y 和 x 之間的相關(guān)性相同（稍后我們將詳細(xì)介紹該元素）,。
卡爾曼濾波器將嘗試使用輸入檢測更新均值和協(xié)方差。常用術(shù)語是將此分布稱為系統(tǒng)狀態(tài),。在我們的示例中,，我們將使用以下初始狀態(tài)：平均位置μ_x取自第一個檢測到的 bbox 的 x 位置，我們假設(shè)一些初始速度μ_v,。我們還假設(shè)最初速度和位置之間沒有協(xié)方差,。我們可以用圖形繪制這個初始狀態(tài),，如下所示：
為了發(fā)揮它的魔力,，卡爾曼濾波器有兩個步驟：預(yù)測，其中它嘗試在給定先前狀態(tài)和此后經(jīng)過的時間的情況下預(yù)測當(dāng)前狀態(tài),。更新,，它嘗試將預(yù)測狀態(tài)與傳入的測量值結(jié)合起來。讓我們使用我們的示例來看看這是如何發(fā)生的,。為了在給定時間的情況下預(yù)測下一個狀態(tài),，卡爾曼濾波器使用狀態(tài)轉(zhuǎn)換矩陣 ( F )，（也稱為物理模型）將前一個狀態(tài)與當(dāng)前狀態(tài)相關(guān)聯(lián),。在我們的例子中,，由于我們假設(shè)在很短的時間間隔內(nèi)（大約幾幀）的速度恒定,，狀態(tài)轉(zhuǎn)移矩陣是：
這樣，如果我們將它與我們的狀態(tài)均值向量相乘,，我們就會得到預(yù)測均值：
預(yù)測新的平均值是不夠的,。我們預(yù)計，如果時間過去了,，那么協(xié)方差矩陣（表示狀態(tài)的不確定性）也會發(fā)生變化,。為了預(yù)測新的協(xié)方差，我們將使用隨機(jī)變量的線性變換公式,，并向其添加噪聲對角矩陣,。這種噪聲稱為“過程噪聲”，它包含了自我們上次更新狀態(tài)以來所經(jīng)過的時間所產(chǎn)生的不確定性,，并且在我們的物理模型中沒有考慮到,。所以在我們的例子中，它可以表示非恒定速度和相機(jī)運(yùn)動,。它通常用Q表示,。所以協(xié)方差預(yù)測方程如下：
如果我們?yōu)槲覀兊某跏紶顟B(tài)求解這個方程：
請注意，除了過程噪聲增加的額外不確定性外,，位置方差也增加了一個因子：
這個因素源于我們在將F（物理模型）定義為非對角矩陣時引入的位置和速度之間的依賴性,。可以這樣理解：假設(shè)預(yù)測位置是狀態(tài)速度的函數(shù),，這個預(yù)測位置的不確定性是我們之前狀態(tài)的不確定性加上速度的不確定性乘以它們耦合的因子（平方,，單位）。這里要注意的另一件事是新引入的協(xié)方差因子：
這意味著我們的高斯維度不再是獨(dú)立的,。這更容易以圖形方式解釋,，所以讓我們這樣做。
那么這里發(fā)生了什么,？為什么我們的州突然轉(zhuǎn)向了一邊？這是因?yàn)槲恢煤退俣炔皇莾蓚€獨(dú)立的變量,。狀態(tài)的速度直接影響預(yù)測的位置,。直觀地說，這意味著更高的速度應(yīng)該對應(yīng)于更遠(yuǎn)的位置,，反之亦然,。在數(shù)學(xué)上，我們的狀態(tài)轉(zhuǎn)移矩陣 (F) 不是對角線這一事實(shí)表明了這一點(diǎn),。非對角元素 Δt 將位置和速度耦合在一起,。由于這種耦合，每個更新步驟都會在協(xié)方差矩陣中引入非對角元素,，這些元素在圖形上對應(yīng)于我們分布的移動,，如上面的 GIF 所示,。這一步在數(shù)學(xué)上有點(diǎn)復(fù)雜，但直觀上也很簡單,。在這一步中,，卡爾曼濾波器將接收兩個可能的狀態(tài)（定義為高斯 PDF）作為輸入，并輸出一個結(jié)合了兩者信息的新狀態(tài),。在深入研究實(shí)際描述此過程的方程式之前,，要點(diǎn)如下：以統(tǒng)計上最優(yōu)的方式組合來自兩個（或多個）PDF 的信息，相當(dāng)于將它們相乘,。由于高斯分布的特殊性質(zhì),，這種乘積是另一個具有均值和協(xié)方差的高斯分布，可以通過（相對）簡單的方程獲得,。現(xiàn)在了解更多詳情,。更新過程可以被認(rèn)為是貝葉斯推理。如果我們將我們正在跟蹤的對象的實(shí)際位置（我們實(shí)際上并不知道）定義為x,，將實(shí)際測量的位置定義為m,，我們將得到以下更新方程：
卡爾曼濾波器中使用的貝葉斯定理讓我們在卡爾曼濾波器的上下文中理解這些術(shù)語：- 后驗(yàn)：給定測量值的實(shí)際位置的概率分布。這就是我們想要使用更新步驟推斷的內(nèi)容,。
- 可能性：給定實(shí)際位置的測量概率分布,。假設(shè)這是圍繞實(shí)際位置的高斯分布（如“假設(shè)”部分中所述）。
- Prior：在測量之前（也就是預(yù)測狀態(tài)）,，實(shí)際位置的概率分布到我們目前所知的最佳狀態(tài),。這也被假定為最初的高斯分布。
- 歸一化：測量概率分布,，對所有可能的位置求和,。在這種情況下，這部分沒有計算,，因?yàn)槲覀冎魂P(guān)心更新均值和協(xié)方差,，而不關(guān)心高斯的幅度。
從這里我們可以推斷出更新后的狀態(tài)將是預(yù)測狀態(tài)和測量狀態(tài)的乘積,。因?yàn)槲覀冊谶@里處理的是高斯分布,，所以這種分布的乘積將是另一個高斯分布，其均值和協(xié)方差具有封閉方程,。如果我們對位置和速度都有測量（我們沒有）,，那么更新步驟會像這樣：
預(yù)測狀態(tài)取自上述過程,。測量狀態(tài),，類似于我們的初始狀態(tài)，具有軸對齊的橢圓形狀,。也就是說,，測量的位置和測量的速度來自兩個獨(dú)立的傳感器,，每個傳感器的不確定性由高斯方差捕獲。但是,，因?yàn)樵谖覀兊囊曨l跟蹤示例中,，我們只有位置測量，沒有速度測量,，所以測量狀態(tài)的概率只會在位置維度上衰減,，但在速度維度上，它會均勻分布,，所以看起來有點(diǎn)像像這樣：
在這里,，小伙伴可以看到一個測量位置的平均值高于預(yù)測位置。更新后的狀態(tài)的位置當(dāng)然會介于兩者之間,。但請注意,，雖然我們只測量了位置，但更新狀態(tài)的平均速度也相對于預(yù)測狀態(tài)有所增加,。如果我們回想一下預(yù)測狀態(tài)的位置和速度是相關(guān)的,，就可以直觀地理解這種速度變化，這意味著更遠(yuǎn)的位置與更高的速度相關(guān)聯(lián),。鑒于這一特性,，為什么測量某個位置會導(dǎo)致我們對平均速度的信念發(fā)生變化就變得很清楚了。考慮兩個高度相關(guān)的概率分布：例如,，云量概率分布和降雨量概率分布,。我們知道，大量的云與更多的降雨高度相關(guān)?，F(xiàn)在假設(shè)我們不得不猜測昨天巴黎下了多少雨,。在沒有任何先驗(yàn)知識的情況下，小伙伴可能會猜到 3 毫米的降雨量（小伙伴猜測的平均值）?，F(xiàn)在,，有人告訴你昨天很陰天。使用這些先驗(yàn)知識,，小伙伴肯定會將小伙伴對降雨的猜測更新到更高的數(shù)量,。這正是卡爾曼濾波器中發(fā)生的情況。給定遠(yuǎn)位置測量值,，我們還將速度更新為更高的值,。在本博客中，我們僅展示它們并嘗試?yán)斫馑鼈儭?/span>
我們使用R來 表示測量的狀態(tài)協(xié)方差矩陣,，因?yàn)檫@是卡爾曼濾波器教科書中常用的表示方式,。在這個例子中，由于我們的測量是一維的,，測量的狀態(tài)可以用兩個標(biāo)量表示：
因?yàn)槲覀兊臏y量和狀態(tài)具有不同的維度,，要將兩者結(jié)合起來,，我們需要一個線性變換，將我們從狀態(tài)空間帶到測量空間,。我們用矩陣H表示這種變換,。在我們的例子中，我們的測量只有一維（位置）,，而狀態(tài)有兩個（位置和速度）,，所以我們可以使用：
這樣，如果我們將預(yù)測均值與H相乘,，我們將僅得到均值的測量部分：
現(xiàn)在來看看實(shí)際的方程式,。我們將首先給出更新狀態(tài)的均值和協(xié)方差的完整方程，然后將它們分解為更有意義的組件,，以便我們更好地了解正在發(fā)生的事情,。完整更新的平均方程（對于我們的 1D 視頻跟蹤示例）是：
這是一個相當(dāng)復(fù)雜的方程，所以讓我們分解一下,。首先,，我們將傳感器“創(chuàng)新”定義為：
這只是測量位置和平均預(yù)測位置之間的差異。它被稱為創(chuàng)新,，因?yàn)樗梢员徽J(rèn)為是我們從傳感器獲得的新（創(chuàng)新）信息,。
卡爾曼增益是我們將新傳感器信息納入更新狀態(tài)的因素,。
讓我們進(jìn)一步分解為位置更新和速度更新,。對于更新后的位置，我們得到：
這個等式很有意義,。更新的平均值將是預(yù)測平均值和測量平均值的加權(quán)平均值,，權(quán)重因子與它們中的每一個的相對不確定性成正比。因此,，假設(shè)我們有一個零方差的完美傳感器,。這將導(dǎo)致預(yù)測的測量值等于傳感器的測量值（應(yīng)該如此——它是完美的。）,。另一方面,，如果傳感器與預(yù)測狀態(tài)的方差相比非常嘈雜（高不確定性），那么它將是相反的,，完全忽略測量并且只使用預(yù)測位置,。完全如小伙伴所料。
這種一般形式?jīng)]有多大意義（至少不是我能想到的）,，所以讓我們嘗試將結(jié)果插入我們之前推導(dǎo)出的單個協(xié)方差預(yù)測步驟,，看看是否有幫助。如果我們假設(shè)零過程噪聲（Q=0，這是不現(xiàn)實(shí)的,，但有利于更簡單和更直觀的方程），我們得到預(yù)測的協(xié)方差為：
請記住,，“ t ”上標(biāo)只是表示“上一步的”,；插入我們得到的速度更新：
讓我們考慮一下這個等式的不同部分以及它們的含義。最后一部分：
是速度創(chuàng)新,。就像位置創(chuàng)新一樣,，這是預(yù)測的狀態(tài)速度與我們實(shí)際到達(dá)測量位置所需的速度之間的差異。現(xiàn)在,，我們希望將多少創(chuàng)新融入到更新的狀態(tài)中,？嗯，它取決于我們先前狀態(tài)下速度的不確定性相對于測量速度的不確定性,。但是等等,，我們沒有測量速度！它是從位置測量中推斷出來的,。那么我們?nèi)绾味x這個推斷速度的不確定性呢,？速度是使用先前位置和測量位置推斷出來的，因此它的不確定性將是其兩個分量的不確定性之和是有道理的,。因此,，我們可以將推斷的速度不確定性定義為：
我們還可以定義一個廣義的預(yù)測速度不確定性，它也解釋了自上次更新以來經(jīng)過的時間：
這個等式更直觀,。它的意思是更新的速度將等于預(yù)測的速度加上速度創(chuàng)新的一部分,。當(dāng)推斷的速度不確定性非常大（相對于預(yù)測速度）時，該分?jǐn)?shù)將接近零,，反之則接近 1,。
我們可以插入上面獲得的卡爾曼增益和以下事實(shí)：
這個常數(shù)只是相對于位置總不確定度的測量不確定度。如果我們將其代入等式（在我將省略你的更多代數(shù)之后）,，我們得到：
讓我們分解一下：更新后的位置不確定性是預(yù)測位置不確定性和測量位置不確定性的調(diào)和平均值,。調(diào)和平均值是一種通過接近兩個元素中較小的元素來組合兩個元素的平均值。因此,，如果：
更新后的位置速度協(xié)方差也很直觀。它是預(yù)測協(xié)方差的縮小版本,，比例因子是相對測量不確定性,。因此，再次,，對于完美的測量,，這些元素將接近于零，而對于嘈雜的測量，它們將保持不變,。現(xiàn)在是更新的速度方差,。乍一看，這似乎沒有多大意義,，因此,，就像我們之前所做的那樣，讓我們插入我們之前推導(dǎo)出的單個協(xié)方差預(yù)測步驟的結(jié)果,。
我們可以再次使用我們上面定義的推斷速度不確定性和廣義速度預(yù)測不確定性來獲得：
我們再次得到更新的方差是推斷速度不確定性和廣義速度預(yù)測不確定性的調(diào)和平均值,。所以基本上總是接近兩者中較小的一個。
讓我們總結(jié)一下我們可以從這個等式中學(xué)到什么,。位置方差和速度方差都可以表示為兩個不確定性的調(diào)和平均值（每個不確定性都與它相關(guān)聯(lián)）。位置速度協(xié)方差也隨著我們相對于先驗(yàn)的測量確定性而縮小,。如果小伙伴返回顯示更新過程的圖形,，這也是非常直觀的原因。
|
