傳感器融合 卡爾曼：1.使用激光雷達數據的基礎卡爾曼濾波器：

展開全文
卡爾曼濾波器是傳感器融合工程師用于自動駕駛汽車的工具。想象一下，你有一個雷達傳感器，告訴你另一輛車距離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++編寫代碼:
代碼示例：
來源：牛喀網

傳感器融合 卡爾曼：NXP傳感器融合筆記08(卡爾曼濾波4)

這一篇文章回歸正題，開始介紹NXP傳感器Sensor Fusion融合方案(以下簡稱NXP方案)中的s算法部分，對應最后一個官方視頻教程和AN5023。如果沒有前期的鋪墊以及卡爾曼基礎理論，看這篇文章肯定會看暈掉。。。做好心理準了么？！
基本就是翻譯  Optimal State Estimation (2006). 這本書的1.1.3 Matrix calculus 章節(PS： 這本書是有中文版的，不過翻譯實在太差了，我還是習慣看英文版，真的)  另外，這個筆記需要一些線性代數的知識。至少是本科學過線性代數，而且最近復習一遍的水平
連續時間線性系統離散化
對于一個連續時間的線性系統：
w為噪聲， G為噪聲驅動矩陣
其中
是白噪聲的方差強度矩陣 為是狄拉克沖激函數。
他的離散化形式定義為：
下面直接給出結論(中間過程可以去看嚴老師的視頻和書 或者 Simion 的書)
總結一下：
狀態轉移矩陣  離散化為  噪聲驅動矩陣離散化為  離散化后噪聲均值為0(如果原來連續時間噪聲均值為0的話)， 方差為  舉個栗子
對于簡單的牛頓力學方程：

當我們學了微積分之后，可以把速度看成距離的微分，加速度看成速度的微分：

對于這種系統，我們可以將設狀態向量為

這是一個一階微分方程，令  ，則：
最終得到：

常見的簡單隨機過程離散化
其中：
為反相關時間常數 為激勵高斯白噪聲實際物理信號的噪聲單位
這一節解釋一下 啥叫隨機游走，噪聲密度，以及他們的單位到底是怎么推導出來的：
U表示任意物理量
在物理上，要考慮單位, 連續時間噪聲的單位 應該是 U/s(U代表任意物理量，比如距離單位m ，速度單位m/s 等等) 那么噪聲的方差就應該是 (U/s)^(2)就是  。 第二點，要知道沖擊響應函數  單位是  , 所以功率譜密度的單位就是  。物理意義就是每Hz上分配的功率大小，就是功率譜的概念。
把  開根號就得到 激勵噪聲密度：
激勵噪聲密度 和 隨機游走系數 是等價的單位相同，只是習慣于描述不同的物理量，激勵噪聲密度描述w噪聲本身，而隨機游走系數是想描述x(物理量本身)
慣導中常用誤差：

卡爾曼濾波復習
簡單重溫下標準的卡爾曼濾波：NXP使用的符號可能和前面不一樣，比如量測矩陣H，在NXP這里叫做C，其他大部分都一樣：
標準線性卡爾曼復習，A為過程轉移矩陣，C為量測矩陣（其他文章一般叫做H）
幾點需要注意：
減號代表 priori，中文叫做先驗(感覺中文防翻譯的并不好)。 其實就是卡爾曼公式中在新的量測數據到達之前的各個狀態量加號代表posteriori, 中文叫做后驗。其實就是卡爾曼公式中在新的量測數據到達之后的，進行更新之后的各個狀態量
對于NXP的sensor fusion 方案，有如下幾個特點
總結以下：
NXP的方案是間接(誤差)卡爾曼濾波NXP的方案過程轉移矩陣A為0！這會導致卡爾曼的5個公式大變!,真正的開源代碼和AN5023中有一點點不一樣。符號表
最好把這個符號表打出來，供后面分析使用.
1. 加速度計和地磁的噪聲方差加速度噪聲方差矩陣
具體推導過程可以參考AN筆記，這里直接寫下結論，就是加速度計和地磁的噪聲方差認為等于三倍的與正常值的模(當地標準重力大小，當地標準地磁場大小)的差。
加速度所有噪聲方差
其中:
:加速度計噪聲的方差:   :線性加速度干擾說造成的方差:  地磁噪聲偏差矩陣地磁噪聲方差
至于這個3是怎么來的，AN5023里有寫，我就不推導了
陀螺和加速度計誤差向量估計
在NXP sensor fusion 中 旋轉狀態指的是 Goblal->Local既：
對于重力有：
對于地磁有：
兩個向量r,s ,計算從u轉到v的四元數的公式為：
計算從向量r轉到向量s的四元數,其中r和s必須是單位化的
這個q的矢量部分可以表示兩個向量之間差的夾角。在后面NXP的方案中會頻繁用到。
重力及地磁誤差四元數
加速度計四元數誤差，其實就是38式子，取其中的矢量部分：
加速度誤差四元數
取其中矢量部分：
加速度誤差四元數的矢量部分
同理可得地磁場的誤差四元數：
地磁場誤差四元數陀螺儀傳感器模型
記陀螺儀讀值為  ，則認為陀螺儀測量模型為：
陀螺儀測量模型
其中：
是真實的角速度 陀螺零偏 是陀螺噪聲(認為是高斯噪聲)
陀螺噪聲表達為：
陀螺噪聲方差
陀螺零偏  又可以表示為上一次的零偏加一個高斯噪聲(隨機游走)：
陀螺零偏模型(是一個隨機游走過程)
其中  也是高斯噪聲，三個軸之前無獨立，表示為：
陀螺隨機游走噪聲表示角速度誤差的模型
注意這里是角速度誤差的模型，不是角速度的模型！
其中
是上一次的角速度誤差估計值 加速度隨機游走 角速度噪聲
編譯時的宏常量
FQVY_6DOF_GBY_KALMAN: 陀螺噪聲，這個值越大，陀螺置信度越低，系統更依賴于觀測值(加速度，地磁)。所有會導致收斂更快，但更容易被加速度，地磁噪聲影響。 QWB_6DOF_GY_KALMAN： 陀螺零偏噪聲： 這個值越大，零偏跟蹤性能越好。但是更受噪聲影響。直接卡爾曼過程模型：(以下以6軸進行講解，先不考慮地磁)
狀態量：
直接卡爾曼狀態量
狀態轉移就是四元數積分：
其中：  (推導見前面四元數筆記)
間接(誤差)卡爾曼過程模型
NXP開源代碼實用的是誤差卡爾曼，而非直接卡爾曼！
狀態量：
間接卡爾曼狀態量，注意，最后的b是陀螺零偏的誤差, 不是陀螺零偏本身！
兩個結論：
長期于高速運動時候(設地磁干擾很小)，只有地磁作為量測，加速度計因為有巨大的線性加速度干擾導致噪聲會非常大，長期處于這種運動中，會造成pitch/ roll飄移
長期處于地磁干擾情況下(低速運動或者靜止)，只有加速度計參與量測更新，Pitch和Roll可以得到量測更新但是Yaw得客觀性為0，所以Yaw會緩慢飄逸。
卡爾曼方程：
過程轉移方程：  ，因為每一次更新后認為誤差為0，所以有：

每次更新后認為誤差為0，所以過程轉移矩陣直接沒了。。。
卡爾曼狀態量更新方程直接變成：
這個感覺實在是差強人意啊，上一步的狀態量直接就沒了。。這還能時卡爾曼？。。。
再來看系統方差矩陣，卡爾曼原來的先驗系統方差為：  ，因為現在A為0，所以。。：
系統協方差矩陣直接等于過程方差
卡爾曼增益K的標準公式為：  (C就是H，量測矩陣)，因為現在  ，所以卡爾曼增益公式變成：
卡爾曼增益公式
由此可見，NXP這套方案主要取決于兩個方差矩陣：
過程方差，陀螺儀的噪聲。 量測方差，加速度和地磁的噪聲。
當  很大時候(高動態，高地磁干擾)，卡爾曼系數趨近于：  ，所以更多相信陀螺儀：  。反之，當  過大的時候，系統更相信測量值，卡爾曼系數接近于1：狀態量更基本取決于量測：
也就是說，只要確定了 和  和  ,整個系統的每個方程就確定下來了: 下面就來搞定 和  和  ！！！
量測矩陣   量測方差  和過程方差  的確定
的確定
6軸融合中， 量測輸入為  ，這是重力誤差四元數的矢量部分(陀螺儀算出來的先驗重力向量和加速度測出的重力向量的誤差四元數矢量部分)
卡爾曼量測更新，注意因為過程矩陣A已經沒了，所以量測更新變成了現在這個模樣。。
重力量測方程：
記  則有：
6軸誤差卡爾曼量測方程H(在NXP官方筆記里叫做C)9軸誤差卡爾曼量測方程H(在NXP官方筆記里叫做C)
的確定：
對于9軸的量測方差  ：
9軸量測方差矩陣的確定， Qvk是一個6x6矩陣
因為量測輸入  既和陀螺的噪聲，陀螺的零偏有關系，也和加速度的噪聲，加速度的零偏有關系，所以有：
量測噪聲Qv
對于6軸 注意  是一個3x3矩陣，并且對角線元素都一樣：
6軸量測方差矩陣
過程方差  的確定
9軸過程噪聲方差的定義
6軸過程噪聲方差的定義
下面來一個一個看里面的內容：
先研究  ：它是陀螺零篇的估計誤差，可以表示為：
其中  表示為：
再研究  ：它是地磁誤差四元數和自己的協方差:
其中  為：
再研究  ：它是重力矯正四元數矢量部分的誤差，可以表示為：
其中  表示為：
再研究  ：它是重力矯正四元數和陀螺零篇誤差之間的協方差：
其中  表示為：
再研究  ：它是地磁矯正四元數和陀螺零偏誤差之間的協方差：
其中  為
至此，過程方差全部確定。
一些總結：
總框圖一些重點總結一些重點總結NXP方案的量測輸入時先驗重力誤差四元數(上一次姿態算出來的重力估計值和本次加速度計讀回來的重力值得誤差四元數)和 先驗地磁場誤差四元數。 量測噪聲本身和重力和地磁場的模值有關系。如果和當地重力 或者 地磁場大小差的太大，表明目前干擾很大計算過程方差  非常復雜，詳見AN5023

傳感器融合 卡爾曼：卡爾曼濾波器是如何運用于多傳感器融合的？

我目前認為的卡爾曼濾波器是這樣的：
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。

傳感器融合 卡爾曼：擴展卡爾曼濾波EKF與多傳感器融合

參考：
Extended Kalman Filter（擴展卡爾曼濾波）是卡爾曼濾波的非線性版本。在狀態轉移方程確定的情況下，EKF已經成為了非線性系統狀態估計的事實標準。本文將簡要介紹EKF，并介紹其在無人駕駛多傳感器融合上的應用。
KF與EKF
本文假定讀者已熟悉KF，若不熟悉請參考卡爾曼濾波簡介。
KF與EKF的區別如下：
預測未來：x′=Fx+u用x′=f(x,u)代替；其余F用Fj代替。
修正當下：將狀態映射到測量的Hx′用h(x′)代替；其余H用Hj代替。
其中，非線性函數f(x,u)，h(x′)用非線性得到了更精準的狀態預測值、映射后的測量值；線性變換Fj，，Hj通過線性變換使得變換后的x，z仍滿足高斯分布的假設。
Fj，Hj計算方式如下：

為什么要用EKF
KF的假設之一就是高斯分布的x預測后仍服從高斯分布，高斯分布的x變換到測量空間后仍服從高斯分布。可是，假如F、H是非線性變換，那么上述條件則不成立。
將非線性系統線性化
既然非線性系統不行，那么很自然的解決思路就是將非線性系統線性化。
對于一維系統，采用泰勒一階展開即可得到：

對于多維系統，仍舊采用泰勒一階展開即可得到：

其中，Df(a)是Jacobian矩陣。
多傳感器融合
lidar與radar
本文將以汽車跟蹤為例，目標是知道汽車時刻的狀態
。已知的傳感器有lidar、radar。
lidar：笛卡爾坐標系。可檢測到位置，沒有速度信息。其測量值
。
radar：極坐標系。可檢測到距離，角度，速度信息，但是精度較低。其測量值
，圖示如下。
傳感器融合步驟
步驟圖如上所示，包括：
收到第一個測量值，對狀態xx進行初始化。
預測未來
修正當下
初始化
初始化，指在收到第一個測量值后，對狀態x進行初始化。初始化如下，同時加上對時間的更新。
對于radar來說，

對于radar來說，

預測未來
預測主要涉及的公式是：

需要求解的有三個變量：F、P、Q。
F表明了系統的狀態如何改變，這里僅考慮線性系統，F易得：

P表明了系統狀態的不確定性程度，用x的協方差表示，這里自己指定為：

Q表明了x′=Fx未能刻畫的其他外界干擾。本例子使用線性模型，因此加速度變成了干擾項。x′=Fx中未衡量的額外項目v為：

v服從高斯分布N(0,Q)。

修正當下
lidar
lidar使用了KF。修正當下這里牽涉到的公式主要是：

需要求解的有兩個變量：H、R。
H表示了狀態空間到測量空間的映射。

R表示了測量值的不確定度，一般由傳感器的廠家提供，這里lidar參考如下：

radar
radar使用了EKF。修正當下這里牽涉到的公式主要是：

區別與上面lidar的主要有：
狀態空間到測量空間的非線性映射f(x)
非線性映射線性化后的Jacob矩陣
radar的
狀態空間到測量空間的非線性映射f(x)如下

非線性映射線性化后的Jacob矩陣Hj

R表示了測量值的不確定度，一般由傳感器的廠家提供，這里radar參考如下：

傳感器融合實例
多傳感器融合的示例如下，需要注意的有：
lidar和radar的預測部分是完全相同的
lidar和radar的參數更新部分是不同的，不同的原因是不同傳感器收到的測量值是不同的
當收到lidar或radar的測量值，依次執行預測、更新步驟
當同時收到lidar和radar的測量值，依次執行預測、更新1、更新2步驟
多傳感器融合的效果如下圖所示，紅點和藍點分別表示radar和lidar的測量位置，綠點代表了EKF經過多傳感器融合后獲取到的測量位置，取得了較低的RMSE。