發布日期：2022-10-09 點擊率：58
展開全文
卡爾曼濾波器是傳感器融合工程師用于自動駕駛汽車的工具。想象一下，你有一個雷達傳感器，告訴你另一輛車距離15米，一個激光傳感器說車輛距離20米。你如何協調這些傳感器測量？這就是卡爾曼濾波器的功能。卡爾曼濾波在自動駕駛汽車上的應用十分廣泛，本文講述卡爾曼濾波算法，希望對你有所幫助。
卡爾曼濾波算法在控制領域有極廣泛的應用，在發動機燃油噴射控制中，可以應用擴展的卡爾曼濾波理論研究瞬態工況下發動機循環進氣量的最優估計算法，在雷達中，人們感興趣的是跟蹤目標，但目標的位置、速度、加速度的測量值往往在任何時候都有噪聲。卡爾曼濾波利用目標的動態信息,設法去掉噪聲的影響，得到一個關于目標位置的好的估計。
為了以后更好的工程實踐應用卡爾曼濾波算法，今天小編帶領著大家了解卡爾曼濾波算法的理論，及其在自動駕駛多傳感器融合算法中的應用。ー 1 ー什么是卡爾曼濾波？
你可以在任何含有不確定信息的動態系統中使用卡爾曼濾波，對系統下一步的走向做出有根據的預測，即使伴隨著各種干擾，卡爾曼濾波總是能指出真實發生的情況。
在連續變化的系統中使用卡爾曼濾波是非常理想的，它具有占用內存小的優點（除了前一個狀態量外，不需要保留其它歷史數據），并且速度很快，很適合應用于實時問題和嵌入式系統。
在Google上找到的大多數關于實現卡爾曼濾波的數學公式看起來有點晦澀難懂，這個狀況有點糟糕。實際上，如果以正確的方式看待它，卡爾曼濾波是非常簡單和容易理解的，下面我將用漂亮的圖片和色彩清晰的闡述它，你只需要懂一些基本的概率和矩陣的知識就可以了。
ー 2 ー我們能用卡爾曼濾波做什么？
用玩具舉例：你開發了一個可以在樹林里到處跑的小機器人，這個機器人需要知道它所在的確切位置才能導航。我們可以說機器人有一個狀態： ，表示位置和速度：
注意這個狀態只是關于這個系統基本屬性的一堆數字，它可以是任何其它的東西。在這個例子中是位置和速度，它也可以是一個容器中液體的總量，汽車發動機的溫度，用戶手指在觸摸板上的位置坐標，或者任何你需要跟蹤的信號。這個機器人帶有GPS，精度大約為10米，還算不錯，但是，它需要將自己的位置精確到10米以內。樹林里有很多溝壑和懸崖，如果機器人走錯了一步，就有可能掉下懸崖，所以只有GPS是不夠的。或許我們知道一些機器人如何運動的信息：例如，機器人知道發送給電機的指令，知道自己是否在朝一個方向移動并且沒有人干預，在下一個狀態，機器人很可能朝著相同的方向移動。當然，機器人對自己的運動是一無所知的：它可能受到風吹的影響，輪子方向偏了一點，或者遇到不平的地面而翻倒。所以，輪子轉過的長度并不能精確表示機器人實際行走的距離，預測也不是很完美。GPS 傳感器告訴了我們一些狀態信息，我們的預測告訴了我們機器人會怎樣運動，但都只是間接的，并且伴隨著一些不確定和不準確性。但是，如果使用所有對我們可用的信息，我們能得到一個比任何依據自身估計更好的結果嗎？回答當然是YES，這就是卡爾曼濾波的用處。ー 3 ー卡爾曼濾波是如何看到你的問題的？
下面我們繼續以只有位置和速度這兩個狀態的簡單例子做解釋。我們并不知道實際的位置和速度，它們之間有很多種可能正確的組合，但其中一些的可能性要大于其它部分： 卡爾曼濾波假設兩個變量（位置和速度，在這個例子中）都是隨機的，并且服從高斯分布。每個變量都有一個均值μ，表示隨機分布的中心（最可能的狀態），以及方差，表示不確定性。在上圖中，位置和速度是不相關的，這意味著由其中一個變量的狀態無法推測出另一個變量可能的值。下面的例子更有趣：位置和速度是相關的，觀測特定位置的可能性取決于當前的速度： 這種情況是有可能發生的，例如，我們基于舊的位置來估計新位置。如果速度過高，我們可能已經移動很遠了。如果緩慢移動，則距離不會很遠。跟蹤這種關系是非常重要的，因為它帶給我們更多的信息：其中一個測量值告訴了我們其它變量可能的值，這就是卡爾曼濾波的目的，盡可能地在包含不確定性的測量數據中提取更多信息！ ：這種相關性用協方差矩陣來表示，簡而言之，矩陣中的每個元素， 表示第 i 個和第 j 個狀態變量之間的相關度。（你可能已經猜到協方差矩陣是一個對稱矩陣，這意味著可以任意交換 i 和 j）。協方差矩陣通常用來表示，其中的元素則表示為ー 4 ー使用矩陣來描述問題
我們基于高斯分布來建立狀態變量，所以在時刻 k 需要兩個信息：最佳估計
（即均值，其它地方常用 μ 表示），以及協方差矩陣。
（當然，在這里我們只用到了位置和速度，實際上這個狀態可以包含多個變量，代表任何你想表示的信息）。接下來，我們需要根據當前狀態（k-1 時刻）來預測下一狀態（k 時刻）。記住，我們并不知道對下一狀態的所有預測中哪個是“真實”的，但我們的預測函數并不在乎。它對所有的可能性進行預測，并給出新的高斯分布。我們可以用矩陣來表示這個預測過程：
它將我們原始估計中的每個點都移動到了一個新的預測位置，如果原始估計是正確的話，這個新的預測位置就是系統下一步會移動到的位置。那我們又如何用矩陣來預測下一個時刻的位置和速度呢？下面用一個基本的運動學公式來表示：現在，我們有了一個預測矩陣來表示下一時刻的狀態，但是，我們仍然不知道怎么更新協方差矩陣。此時，我們需要引入另一個公式，如果我們將分布中的每個點都乘以矩陣 A，它的協方差矩陣系會怎樣變化呢？很簡單，下面給出公式：結合方程（4）和（3）得到： ー 5 ー外部控制量我們并沒有捕捉到一切信息，可能存在外部因素會對系統進行控制，帶來一些與系統自身狀態沒有相關性的改變。 以火車的運動狀態模型為例，火車司機可能會操縱油門，讓火車加速。相同地，在我們機器人這個例子中，導航軟件可能會發出一個指令讓輪子轉向或者停止。如果知這些額外的信息，我們可以用一個向量來表示，將它加到我們的預測方程中做修正。假設由于油門的設置或控制命令，我們知道了期望的加速度，根據基本的運動學方程可以得到：
以矩陣的形式表示就是： 稱為控制矩陣，稱為控制向量（對于沒有外部控制的簡單系統來說，這部分可以忽略）。讓我們再思考一下，如果我們的預測并不是100%準確的，該怎么辦呢？ー 6 ー外部干擾如果這些狀態量是基于系統自身的屬性或者已知的外部控制作用來變化的，則不會出現什么問題。但是，如果存在未知的干擾呢？例如，假設我們跟蹤一個四旋翼飛行器，它可能會受到風的干擾，如果我們跟蹤一個輪式機器人，輪子可能會打滑，或者路面上的小坡會讓它減速。這樣的話我們就不能繼續對這些狀態進行跟蹤，如果沒有把這些外部干擾考慮在內，我們的預測就會出現偏差。 在每次預測之后，我們可以添加一些新的不確定性來建立這種與“外界”（即我們沒有跟蹤的干擾）之間的不確定性模型： 原始估計中的每個狀態變量更新到新的狀態后，仍然服從高斯分布。我們可以說的每個狀態變量移動到了一個新的服從高斯分布的區域，協方差為
。換句話說就是，我們將這些沒有被跟蹤的干擾當作是協方差的的噪聲來處理。這產生了具有不同協方差（但是具有相同的均值）的新的高斯分布。我們通過簡單地添加得到的擴展的協方差，下面給出預測步驟的完整表達式：由上式可知，新的最優估計是根據上一個最優估計預測的道德，并加上已知外部控制量的修正。而新的不確定性由上一不確定性預測得到，并加上外部環境的干擾。好了，我們對系統可能的動向有了一個模糊的估計，用和來表示。如果結合傳感器的數據會怎樣呢？
ー 7 ー用測量值來修正估計值我們可能會有多個傳感器來測量系統當前的狀態，哪個傳感器具體測量的是哪個狀態變量并不重要，也許一個是測量位置，一個是測量速度，每個傳感器間接地告訴了我們一些狀態信息。注意，傳感器讀取的數據的單位和尺度有可能與我們要跟蹤的狀態額單位和尺度不一樣，我們用矩陣來表示傳感器的數據。我們可以計算出傳感器讀數的分布，用之前的表示方法如下式所示： 卡爾曼濾波的一大優點就是能處理傳感器噪聲，換句話說，我們的傳感器或多或少都有點不可靠，并且原始估計中的每個狀態可以和一定范圍內的傳感器讀數對應起來。
從測量到的傳感器數據中，我們大致能猜到系統當前處于什么狀態。但是由于存在不確定性，某些狀態可能比我們得到的讀數更接近真實狀態。
我們將這種不確定性（例如：傳感器噪聲）用協方差表示，該分布的均值就是我們讀取到的傳感器數據，稱之為：傳感器噪 現在我們有了兩個高斯分布，一個是在預測值附近，一個是在傳感器讀數附近。我們必須在預測值（粉紅色）和傳感器測量值（綠色）之間找到最優解。 那么，我們最有可能的狀態是什么呢？對于任何可能的度數有兩種情況：（1）傳感器的測量值；（2）由前一狀態得到的預測值。如果我們想知道這兩種情況都可能發生的概率，將這兩個高斯分布相乘就可以了。剩下的就是重疊部分了，這個重疊部分的均值就是兩個估計最可能的值，也就是給定的所有信息中的最優估計。瞧！這個重疊的區域看起來像另一個高斯分布。如你所見，把兩個具有不同均值和方差的高斯分布相乘，你會得到一個新的具有獨立均值和方差的高斯分布！下面用公式講解。ー 8 ー融合高斯分布先以一維高斯分布來分析比較簡單點，具有方差 和 μ 的高斯曲線可以用下式表示：
如果把兩個服從高斯分布的函數相乘會得到什么呢？
將式（9）代入到式（10）中（注意重新歸一化，使總概率為1）可以得到： 將式（11）中的兩個式子相同的部分用 k 表示：
下面進一步將式（12）和（13）寫成矩陣的形式，如果 Σ 表示高斯分布的協方差， 表示每個維度的均值，則：
矩陣稱為卡爾曼增益，下面將會用到。放松！我們快要完成了！
將所有公式結合起來我們有兩個高斯分布，預測部分，和測量部分，將它們放到式（15）中算出它們之間的重疊部分：
由式（14）可得卡爾曼增益為： 將式（16）和式（17）的兩邊同時左乘矩陣的逆（注意里面包含了 ）將其約掉，再將式（16）的第二個等式兩邊同時右乘矩陣 的逆得到以下等式： 上式給出了完整的更新步驟方程。就是新的最優估計，我們可以將它和放到下一個預測和更新方程中不斷迭代。 以上所有公式中，你只需要用到式（7）、（18）、（19）。（如果忘了的話，你可以根據式（4）和（15）重新推導一下）
我們可以用這些公式對任何線性系統建立精確的模型，對于非線性系統來說，我們使用擴展卡爾曼濾波，區別在于EKF多了一個把預測和測量部分進行線性化的過程。ー 9 ー自動駕駛中的傳感器融合算法
追蹤靜止和移動的目標是自動駕駛技術領域最為需要的核心技術之一。來源于多種傳感器的信號，包括攝像頭，雷達，以及激光雷達（基于脈沖激光的測距設備）等傳感器組合的組合體來估計位置，速度，軌跡以及目標的種類，例如其他車輛和行人。
你可能會問——為什么我們需要這么多的傳感器？
這是因為每種傳感器提供了追蹤物體所需要的不同精度和類型的信息，尤其是在不同天氣條件下。比如，以激光雷達為基礎的傳感器能很好地解決位置的問題，但是在糟糕的天氣條件下其精度和性能都會有很大程度的下降。另一方面，雷達的空間解決方案也相對便宜很多，與此同時，在惡劣的天氣條件下也能提供更準確的數據。
同樣的，不像激光雷達傳感器，雷達可以提供目標的速度和方位。雷達數據也是計算密集型的，因為一束激光發射非常多包含每個獨立的激光點的范圍的數據，它使得你必須理解你的算法。
組合來自不同傳感器信息的技術稱之為傳感器融合技術。之所以較早的討論這個，是因為應用在傳感器融合之上的算法必須處理短暫的，充滿雜訊的輸入，生成可靠的運動狀態估計的概率。
下面我們將展示在位置的追蹤和估計中最通用的算法，卡爾曼濾波器的變種——‘擴展卡爾曼濾波器’。
1.使用激光雷達數據的基礎卡爾曼濾波器：
卡爾曼濾波器的歷史已經超過半個世紀，但是對于輸入數據的噪聲信息和狀態估計的平滑來說仍然是最有效的傳感器融合算法之一。它假定位置參數是符合高斯分布的，即完全可以被均值和協方差參數化:X～N(μ, σ2)
當傳感器的信息流開始的時候,卡爾曼濾波器使用一系列的狀態信息來預測和衡量更新步驟去更新被追蹤目標的信心值(概率)。預測和更細心的步驟如下圖所示：
狀態預測：
我們會使用一個簡化的線性狀態空間模型(鏈接：去闡述濾波器的工作方式. 一個系統在t時刻的線性狀態可以從t-1時刻根據以下等式被估計：
衡量更新：
卡爾曼濾波器的下一部分則是去使用實測參數z去更新預測狀態'x',通過縮放因子(通常稱之為卡爾曼增益)成比例的計算估計值和測量值之間的誤差。
你可以在以下鏈接中找到衡量更新等式的求導：
舉個例子
理論部分結束！讓我們嘗試用一些代碼去表示一些卡爾曼濾波器的基礎過程。
對此，我們模擬一個目標，它的狀態被四維向量x=[px py vx vy]所描述。
在這個例子中測量傳感器是返回位置數據但是沒有速度信息的激光傳感器。為了觀測到速度信息我們需要使用雷達傳感器數據。在接下來的章節中，當我們討論擴展卡爾曼濾波的時候我們將會涉及到這些。
讓我們以一些假設開始：
偽代碼：
基礎版本的卡爾曼濾波器代碼步驟列在了下面。你可以找到一個基礎的例子：
最后的迭代步驟通過測量和應用預測以及濾波器的更新步驟如下所示：
plot_position_variance(x,P,edgecolor='r') #plot initial position and covariance in red
for z in measurements:
x,P =predict(x, P)
x,P = update(x, P,z)
plot_position_variance(x,P,edgecolor='b') #plot updates in blue
print(x)
print(P)
卡爾曼濾波器迭代：濾波器在迭代之后向真實值收斂
上方的圖闡述了濾波器在每次迭代中狀態向量的px,py維度和位置的協方差發生了哪些變化。紅圈表示初始過程不確定性。隨著預測和測量更新，我們開始有了較小的誤差（半徑），狀態估計開始越來越精確。
如同你看到的，最終估計的狀態向量x[11.99,2.05]非常接近最終的觀測值，誤差最小值也縮小到了0.05。
2.擴展卡爾曼濾波器——使用雷達數據
雷達數據帶來了一些更困難的挑戰。雷達返回數據值的是基于極坐標系，其由三個部分構成：
- ρ /Range(從原點到此的距離)
- ? / bearing ( ρ 和 x的夾角),
- ρ˙：接近率/距離變化率
由于沒有H矩陣將狀態向量映射到雷達的測量空間，我們需要一個函數h（x）來將狀態空間銀蛇到測量空間以測量更新步驟。這個方法是映射極坐標到笛卡爾坐標的方法的衍生方法，定義如下：
這個映射闡述了一個非線性函數，它將使得卡爾曼濾波器的過程和測量符合高斯分布的假設無效。擴展卡爾曼濾波器使用局部線性模型來逼近非線性模型，然后使用卡爾曼濾波應用到逼近值上。局部線性逼近是通過計算當前狀態估計的一階泰勒展開得出的。一階的逼近也叫雅克比矩陣。我們不會在這里過多介紹雅克比矩陣的推到過程。相關內容在網絡中可以找到優秀的介紹，但是如果你想要直接使用這些東西，你可以在以下的github引用中找到代碼實例：Github link
現實參考
你可以在github存儲庫中找到關于Kalman過濾器的c++編寫代碼:
代碼示例：
來源：牛喀網
一文理清卡爾曼濾波，從傳感器數據融合開始談起
5/19,'19 FesianXu
前言
最近在一個項目中接觸到了卡爾曼濾波，并且對此進行了學習，發現其是一個很有意思的信息融合的算法，可以結合多種傳感器的信息（存在噪聲），得到更為理想的估計，因此在此進行筆記和心得紀錄。本人不是從事控制相關專業工作，可能在短暫的自學過程中對此存在誤解，若有謬誤，望聯系指出，謝謝。（文章主要參考了[1]）
聯系方式：
e-mail: FesianXu@163.com
QQ:
github:
@@[toc]定位問題[1]狀態預測方程考慮施加外力的情況，添加控制項導致預測或者觀察不確定性的因素描述不確定性預測和觀測~同父異母的兄弟結合起來吧~狀態更新
從傳感器的測量談起
在正式討論卡爾曼濾波前，我們先討論對物理量的測量。我們會發現是和卡爾曼濾波緊密相關的。 我們知道，如果需要對自然界的某個物理量，比如溫度，氣壓，速度等進行測量，我們需要用各種傳感器進行測量。但是，因為器件的工藝不可能達到完美，或者其他不能被人為預測到或者控制到的因素和噪聲等存在，傳感器對物理量的預測不可能是完全準確的。因此，我們與其把傳感器的測量結果當成是一個確定值，不如把它看成是一個隨機變量，其均值和方差分別為，既是，這兩個統計參數描述了測量的輸出值（也就是我們直接觀察到的值）和對這個測量的可信程度。同時，我們要注意到，這里的不一定是時間平穩的，也就是說可能隨著時間的變化而變化。 （暫且假設傳感器的測量均值是和真實值無偏的。）
如下圖所示，如果直接觀察傳感器數據，那么其可能是會存在很大的抖動，而不是平滑的，原因可能是觀察噪聲的影響。
Fig 1. 傳感器的數據直接觀測結果。
我們這個時候就想到，如果一次觀察是抖動的，有著的不確定的，那么如果用同一個傳感器對這個物理量觀察次，然后對次數據進行求和，以減少不確定性的影響，豈不妙哉？ 這樣的確是可以的，這個就是信號處理當中的滑動窗口均值濾波(mean filter)。但這個簡單操作有幾個缺點：
我們前面談到了不確定度是可能時變的，簡單相加不能最好地消除不確定性。時間上滑動窗口進行多次測量的求和，會導致延遲。
對此，我們進行一個小改進，就是用多個相同的傳感器去同時測量一個物理量，然后求和或者根據可靠程度去求加權平均和，我們假設多個傳感器的采樣值滿足分布，其中表示傳感器序號:
我們發現，其因為假設是無偏測量傳感器因此均值相同，但是每個傳感器的不確定性不一定相同。
這個時候簡單的求和就容易造成結果的偏移，我們不妨根據方差的大小，進行加權平均求和，在此之前，我們需要幾個假設：
不同傳感器的測量都是一個隨機變量，其均值相同。不同傳感器的測量之間是無關的，也就是說知道了不能對知道其他策略提供任何信息，但是也不會影響到觀測的均值，即是。
接下來，我們用這兩個假設，進行簡單的傳感器間的數據融合以提高測量效果。Let's move on！
簡單版本，多傳感器數據融合
為了簡單起見，假設我們用兩個相同的傳感器進行測量，那么最后數據融合結果應該是:
那么，融合后的估計的不確定度可以通過的方差進行衡量，公式如:
為了最小化 ，我們用求導并且置為0的方法[3]，不難推導出當時，式子(2.2)有最小值，此時，式子(2.1)可化為:
這里討論的只是兩個傳感器的情況，可以簡單地推導到多個傳感器的情況和當觀測值是一個向量時候的情況，以及為了計算有效性，采用迭代計算的方法，具體可以參考文獻[3]。其中，為了以后討論的方便，這里給出當觀測值是一個向量，并且只有兩個傳感器時的公式(2.4)：
可以發現，此時不再假設每個傳感器的測量均值都是一樣的了，其中的稱之為卡爾曼增益(Kalman Gain)，嘛，這里不過只是個名字，暫且不管。
卡爾曼濾波，開始征程
接下來我們開始正式討論卡爾曼濾波(Kalman Filter)。我們之前討論的傳感器之間其實都是**無關(uncorrelated)**的，但是，其實經常我們知道了某個測量量，是可以確定或者為確定另一個測量量提供信息量的，比如我們現在需要測量車輛的位置和速度，那么知道了速度，通常可以為下一步知道位置提供一定的信息。在這種前提下，我們便能夠通過更為合理的數據融合手段，得到更為精確的估計結果。
定位問題[1]
考慮一個例子，我們的機器人需要定位，通常使用的是GPS進行定位，得到車輛的狀態量之一的位置： 。其次，我們可以通過測量輪子的轉過的圈數，對機器人的運行速度進行測量，得到狀態量另一個，速度：。但是，我們要牢記，我們的觀測不是完全準確的，比如GPS存在誤差，而測量輪子轉過的圈數也不能完美的描述速度，因為可能因為地面不平，輪胎打滑等原因導致誤差。不過我們記住我們這個例子中的兩個狀態量：
在這個情況下，我們對兩個狀態量的觀測其實是一個兩元的概率變量，我們的每個觀測都落在分布之中，而我們的任務就是從這個不確定性高的分布中，得到個不確定性更小的分布，從而得到更為精確的估計。圖如：
Fig 2. 當兩個狀態量的觀測存在相關性的時候，其觀測可能的落在的分布圖。
Fig 3. 當兩個狀態量的觀測不存在相關性的時候，其觀測可能的落在的分布圖，其為一個水平于橫軸的類似矩陣的區域。
這個時候，觀測的不確定性體現在方差上，而觀測值可以用均值向量描述，如Fig 4所示：
Fig 4. 觀測中的 值和不確定性的表述。
狀態預測方程
這個時候，如果我們的觀測是準確的，那么會出現什么情況呢？我們根據牛頓力學，可以對位置-速度的過程進行建模，我們會有：
用矩陣形式表達就是：
其中，表示的是狀態向量，為。
我們稱為預測矩陣(Predict Matrix)，通過此，我們可以用當前時刻的狀態量去預測下一個時刻的狀態量，即使我們并不知道真實的值應該是多少，但是這并不影響我們對狀態的預測。
Fig 5. 通過建模，對下個時刻狀態的預測。
當在用進行描述這個預測時，其實可以看成是點對之間的線性變換，那么如Fig 6所示：
Fig 6. 在線性變換下，點對的坐標變化情況。
那么此時，預測的狀態量有了，還需要預測時刻的不確定性，也就是方差，在多變量情況下是協方差矩陣，用表示:
于是我們有，狀態量預測和協方差預測：
考慮施加外力的情況，添加控制項
在控制理論問題中，我們怎么能忘記添加控制項呢？畢竟我們都希望整個系統是可控制的，而不是任其隨意發展的。
讓我們繼續擴展我們上面的那個例子。考慮到機器人本身有油門，可以進行一定的加速行駛，也可以按照一定的加速度制動，讓我們假設這個加速度為，那么根據牛頓力學，我們的狀態預測方程(3.1)就更新為:
同樣還是用矩陣形式表達，有:
其中，被稱之為控制矩陣(Control Matrix)， 被稱之為控制向量(Control Vector) 。如果一個系統實在是沒有控制項，那么可以忽視這個控制項。
然而，因為一系列的誤差存在，我們的預測不可能是完全準確的。那么我們的誤差或者是不確定性主要在哪里存在呢?
導致預測或者觀察不確定性的因素
有以下四種因素可能導致我們的狀態預測或者觀察存在不確定性[2]，我們對此進行簡單描述：
參數的不確定性：參數不確定性指的是在對預測進行建模時，比如，這個模型通過參數進行建模，然而對這個參數的觀察不可能是百分百精確的，這個參數的誤差就會導致整個模型的誤差，因此狀態預測這個時候更新為:
控制器的不確定性：實際生活中，我們的控制器同樣不可能完美，這個誤差可以建模為:
模型的不確定性：實際中，我們通過簡單的線性建模不一定能很好地表達模型預測，因此要引入一個殘差項，表示模型的不完美，建模為：
觀測不確定性：就像我們之前談到的，我們的觀測也是不完美的。描述不確定性
正如上一節我們談到的，有一些影響狀態的因素，比如風，地面情況，輪胎打滑或者其他各種小情況我們是沒法完全考慮到的，也就沒法建模出來，這個時候，狀態預測結果就存在不確定性，如Fig 7所示：
Fig 7. 預測的不確定性。
我們為了對未能跟蹤到的變量進行統一建模，我們假設狀態從到下一個時刻狀態，其下一個狀態落在一個協方差為的高斯分布中，也就是說，我們把所有未能跟蹤到的影響因素都用這個高斯分布描述了。
這個影響導致了我們式子(3.4)中的協方差發生了變化，但是其均值不變，公式如:
以上的內容只是對預測結果進行了討論，但是實際上我們除了預測，還會存在傳感器的測量，雖然這個測量是不準確的，但是也能提供一定的信息量。
根據觀測結果對估計進行調整預測和觀測~同父異母的兄弟
實際系統中，我們可能有多個傳感器給予我們關于系統狀態的信息，我們這里不在乎其測量的量到底是什么，我們只要知道，每個傳感器都間接地告訴了我們狀態量。注意到我們預測狀態量的尺度和單位和觀測結果的尺度和單位可能是不一樣的，這個時候就需要用線性變換把他們變成一樣尺度和單位的。你可能猜到了，我們還是引入一個矩陣描述這個線性變換。
Fig 8. 預測和觀察的尺度和單位不一致性。
Fig 9. 通過線性變換將預測的尺度和單位轉換到和觀測一致。
那么，公式有:
老問題，因為傳感器存在噪聲，我們的觀測結果至少在某種程度上是不可靠的，在原來估計的情況下的結果可能對應了一個范圍的傳感器觀測值。
Fig 10. 同一個預測可能對應多個觀測。
為了描述這個不確定性（也就是傳感器噪聲），我們引入了，這個分布的均值和我們的觀測值相同，但是存在有不確定性，用協方差進行描述。
到現在為止，我們有了兩個高斯分布：
一個是通過線性變換 將預測結果轉化為理論觀察值的分布 。另一個是實際的觀察值的分布，其均值為 。
Fig 11. 兩個描述同一個量的不同分布的交集。
不難發現，因為這兩個分布都是描述同一個量，其交集處，如Fig 12所示，應該是最好的估計結果，我們看到這個交集處，其協方差明顯比觀測和預測的都要小得多，結果也就更為精確。
為了得到這個交集分布的表達形式，我們需要將兩個高斯分布進行相乘即可，我們知道高斯分布的乘積也是高斯分布[4]。
Fig 12. 最佳估計其實也是符合高斯分布。
因此問題也就變成怎么求兩個高斯分布的乘積的高斯分布的參數，如Fig 13所示：
Fig 13. 求藍色分布的參數，高斯分布的乘積也是高斯分布。
根據[1,4]的推導，我們知道相乘后的結果為:
通過引入，可以進行一些化簡有：
如果用矩陣形式表達，則有：
其中分別是預測和觀測的協方差矩陣。其中如(2.4)，我們把稱為卡爾曼增益，我們發現(3.11)和(2.4)形式上是一致的。
結合起來吧~狀態更新
我們現在有兩個分布了：
預測分布:
觀測分布:
將(3.12) (3.13)代入(3.11)，我們有:
注意到(3.14)的可以進行約減，最后得到：
其中，卡爾曼增益變為:
這樣，我們就得到了最后的結合了觀測和模型預測的最佳估計了，并且知道了，可以為下一步的迭代更新提供先驗了。我們發現，其實觀測為預測提供了先驗知識。這個過程可以一直迭代更新。整個卡爾曼濾波的流程如Fig 14所示。
Fig 14. 卡爾曼濾波的整個流程。
Reference
[1].
[2].
[3]. Pei Y, Biswas S, Fussell D S, et al. An elementary introduction to kalman filtering[J]. arXiv preprint arXiv:1710., 2017.
[4].
我目前認為的卡爾曼濾波器是這樣的：
1.設置狀態轉移矩陣
2.[預測]通過 t-1 時的狀態預測 t 時的狀態
3.[更新]用傳感器的測量值與 2 中的預測值估計出最優值，并更新卡爾曼增益等常數
4.重復 2～3 步驟
（如上正是opencv庫中自帶的卡爾曼濾波器例子的步驟，也是初學者能最直觀理解到的，它只涉及到一個測量值）
那么請問，在某些涉及多傳感器融合的問題中，例如 陀螺儀（累積誤差）和電子羅盤（干擾）的數據融合，此時有多個測量值存在，卡爾曼濾波器又是怎樣工作的呢？
貼一個數據融合的結果，用陀螺儀和GPS數據進行數據融合，濾波估計車輛前輪轉角即行業內所說的慣導角位移。LZ精準農業領域，器件型號不表，用霍爾傳感器(精度0.1度)標定，精度在0.17度，用于農機自動駕駛，真他娘的好用。。。。。。
補一個IMU姿態測量系統數據融合的結果。陀螺和加表數據融合，濾波估計系統俯仰角和橫滾角。器件型號不表。
這個是實驗室桌面實驗，手動翻轉IMU
下面是室外實驗，用GPS雙天線的俯仰角進行精度標定，GPS主副天線基線2米，俯仰角精度0.1度。IMU剛性鏈接在GPS工裝上，兩手托舉行走。
更一個濾波融合的例子。
霍爾傳感器安裝在車輛輪子處測量車輛前輪的角度，但是霍爾傳感器測量的是0到360度的絕對角度，安裝在車輪轉向節處時，在車輛前輪打正的情況下角度不是0而是一個隨機的固定的偏置，采用濾波估計的方法估計其安裝偏置，精度可以達到0.1度：
再更一個IMU或者GPS在載體上安裝角估計的應用。IMU或者雙天線GPS安裝在載體上，由于安裝誤差，IMU的前向或GPS主副天線的航向與載體的前向有一個固定的安裝誤差角，采用濾波估計的方法進行估計，精度可以達到0.2度：
在更一個組合導航的例子。
單天線GNSS+IMU做組合導航，提供位置速度姿態，IMU型號是ADI某較低成本器件，位置速度精度和RTK狀態下GNSS結果保持一致，航向精度0.12度(RMS)。
產品是農機自動駕駛儀，用來輔助農機的自動駕駛進行作業，目前主要應用在插秧機、拖拉機、割草機等四輪小車。
組合航向精度和雙天線GNSS航向精度基本一致，且靜止情況下航向不漂。
農機橫向控制精度<2.5cm RMS。
acceleration
鏀剁鏃ユ湡
:2008
2
10
2
15
0
寮?/p>
璦€
闈犲崟涓€鐨勪俊鎭簮宸卞緢闅句繚璇佽幏鍙栫幆澧冧俊鎭殑蹇€?
鎬у拰鍑嗙‘鎬?/p>
,
浠ュ強緇欑郴緇熷鍛ㄥ洿鐜鐨勭悊瑙ｅ強緋葷粺鐨勫喅
絳栧甫鏉ョ殑褰卞搷
銆?/p>
鍙﹀
,
鍗曚竴浼犳劅鍣ㄨ幏寰楃殑浠呮槸鐜鐗瑰緛
鐨勫眬閮?/p>
銆?/p>
鐗囬潰鐨勪俊鎭?/p>
,
淇℃伅閲忓崄鍒嗘湁闄?/p>
,
鑰屼笖姣忎釜浼犳劅鍣?/p>
閲囬泦鍒扮殑淇℃伅榪樺彈鍒拌嚜韜搧璐?/p>
銆?/p>
鎬ц兘鍣０鐨勫獎鍝?/p>
,
淇℃伅
寰€寰€鏄笉瀹屾暣鐨?/p>
,
甯︽湁杈冨ぇ鐨勪笉紜畾鎬?/p>
,
鐢氳嚦鍑虹幇閿欒
銆?/p>
閫氬父鍦ㄤ紶緇熸柟寮忎腑
,
鍚勪紶鎰熷櫒閲囬泦鐨勪俊鎭槸鍗曠嫭
銆?/p>
瀛ょ珛
鍦拌繘琛屽姞宸ュ鐞嗙殑
銆?/p>
榪欎笉浠呬細瀵艱嚧澶勭悊宸ヤ綔閲忓鍔?/p>
,
鑰?/p>
涓斿壊瑁備簡鍚勪紶鎰熷櫒淇℃伅鐨勮仈緋?/p>
,
涓㈠け浜嗕俊鎭殑鏈夋満緇勫悎
钑存兜鐨勪俊鎭壒寰?/p>
,
涔熼€犳垚淇℃伅璧勬簮鐨勬氮璐?/p>
[
1
2
5
]
銆傚湪榪愬姩
鎺у埗緋葷粺涓?/p>
,
寰€寰€灝嗛€熷害浼犳劅鍣ㄦ祴閲忓埌鐨勯€熷害涓庡姞閫熷害
璁℃祴閲忓埌鐨勫姞閫熷害榪涜鍗曠嫭澶勭悊
,
娌℃湁灝嗕袱鑰呯殑淇℃伅榪?/p>
琛屾暟鎹瀺鍚?/p>
銆?/p>
鐢辯墿鐞嗗畾寰嬪彲鐭?/p>
,
鍔犻€熷害涓庨€熷害鎴愬鏁板叧
緋?/p>
,
鎵€浠ヤ袱鑰呯殑鏁版嵁瀛樺湪鐫€鍐呭湪鐨勮仈緋?/p>
,
瀹屽叏鍙互鏍規嵁
淇℃伅铻嶅悎鐞嗚瀵逛袱鑰呮暟鎹繘琛岀患鍚堝鐞?/p>
,
浠庤€屽緱鍒版洿鍔?/p>
鍑嗙‘鐨勭粨鏋?/p>
銆?/p>
鍗″皵鏇兼護娉㈠櫒鏄父鐢ㄧ殑涓€縐嶆暟鎹瀺鍚堟妧
鏈?/p>
銆?/p>
瀹冨埄鐢ㄨ凱浠ｉ€掓帹璁＄畻鐨勬柟寮?/p>
,
瀵瑰瓨璐┖闂磋姹傚緢
灝?/p>
,
閫傚悎浜庡瓨璐┖闂村拰璁＄畻閫熷害鍙楅檺鐨勫満鍚?/p>
[
6
,7
]
銆傚湪
姝?/p>
,
鍒嗘瀽浜嗛€熷害浼犳劅鍣ㄥ拰鍔犻€熷害璁″悇鑷殑浼樼己鐐?/p>
,
緇欏嚭
涓€縐嶅簲鐢ㄥ崱灝旀浖婊ゆ嘗鍣ㄥ師鐞嗗涓よ€呰繘琛屾暟鎹瀺鍚堢殑杈?
濂芥柟娉?/p>
銆?/p>
1
浼犳劅鍣ㄧ畝浠?/p>
1.
1
鍏夌數緙栫爜鍣?
鍏夌數緙栫爜鍣ㄩ€氬父鐢ㄤ簬瑙掑害
銆?/p>
浣嶇Щ鎴栬漿閫熺殑嫻嬮噺
,
閫?/p>
榪囧鍏夎剦鍐茬殑涓暟榪涜璁℃暟
,
鍐嶇粡榪囪綆?/p>
,
寰楀埌嫻嬮噺鍊?/p>
銆?/p>
鍋囪鍦ㄥ懆闀夸負
L
鐨勫渾鐩樹笂鏈?/p>
M
涓繃鍏夊瓟
銆?/p>
鍦ㄧ鏁ｇ郴緇?/p>
涓?/p>
,
鍛ㄦ湡鏃墮棿
T
鍐?/p>
,
瀵硅剦鍐茶繘琛岃鏁?/p>
,
鍏跺€間負
N
,
鍒?/p>
絎?/p>
k
嬈℃祴閲忕殑綰塊€熷害鍙〃杈句負
:
v
(
k
)
=
L
M
下一篇： PLC、DCS、FCS三大控
上一篇： 電氣控制線路圖控制原
