最近在研究傳感器融合，看到一個很好的開源項目，適合小白學習，為以后做傳感器融合、SLAM、自動駕駛和室內(nèi)定位等方向打下基礎。
算法概述
題目：基于改進擴展卡爾曼濾波（Error State-EKF）的LiDAR/GNSS/IMU的傳感器融合軌跡估計
關鍵詞：改進擴展卡爾曼濾波（Error State Extended Kalman Filter，ES-EFK）、傳感器融合、軌跡估計、激光雷達（LiDAR）、衛(wèi)星導航（GNSS）、慣性測量元件（IMU）
算法的overview如下圖所示：
整體而言，就是使用LiDAR、GNSS和IMU的數(shù)據(jù)根據(jù)ES-EKF進行融合迭代估計軌跡。在本項目中，IMU的采樣頻率較高，而GNSS和LiDAR的采樣頻率較低。該算法可以分為兩個部分：預測（Prediction）和改正（Correction）。下面讓我們一起看看這個算法。
Prediction
Prediction階段是基于小車IMU測量的運動模型進行預測軌跡，然后再結合GNSS或LiDAR的數(shù)據(jù)用EKF融合改正預測的軌跡。
下面的小車的參數(shù)，包括位置pk、速度vk和姿態(tài)qk。
運動方程以IMU的數(shù)據(jù)作為輸入，包括四元數(shù)轉旋轉矩陣Cns，比力加速度fk（由加速度計測量），角速度Wk（由陀螺儀測量）。這里需要注意的是，處理IMU數(shù)據(jù)的時候一般都用四元數(shù)，避免用歐拉角帶來的死鎖問題。
Correction
Correction階段就是用GNSS或者LiDAR觀測到的數(shù)據(jù)對估計的位置進行改正。
GNSS的觀測方程：
LiDAR的觀測方程：
ES-EKF算法
Error-state Extended Kalman Filter（ES-EKF）是一種改進的擴展卡爾曼濾波算法，基本思想就是將State分為兩部分Nominal State和Error State。它用于狀態(tài)估計問題，特別是對于非線性系統(tǒng)的狀態(tài)估計問題，例如在機器人定位、導航和控制方面的應用。
x代表真值， x_hat代表Nominal State，占比較大， delta_x代表Error State，占比較小。
線性化如下：
ES-EKF試圖通過引入誤差狀態(tài)來改進EKF的性能。它的核心思想是，通過對狀態(tài)誤差進行線性化而不是對狀態(tài)本身進行線性化，可以更好地處理非線性性質(zhì)，并提高濾波器的穩(wěn)健性和準確性。相比起一般的EKF算法，優(yōu)勢在于：（1）Error State的線性化比Nominal State更好；（2）對于三維空間的數(shù)據(jù)處理較好，使用旋轉的情況，因此很適合本項目的3D LiDAR數(shù)據(jù)。
詳細推導過程可以參考該文章：ES-EKF算法推導
融合算法實現(xiàn)軌跡估計
1.使用IMU數(shù)據(jù)更新運動模型
2.不確定度計算和傳播
3. 使用GNSS或LiDAR改正
3.1 計算卡爾曼增益
3.2 更新Error State
3.3 改正狀態(tài)預測
3.4 計算改正方差
實驗結果
該項目中，已經(jīng)提供了預處理好的IMU、GNSS和LiDAR數(shù)據(jù)，參考軌跡和預測軌跡的對比實驗結果如下：
位置和姿態(tài)矩陣誤差：（藍色實線是誤差，紅色虛線是不確定度）
以上就是這個小項目的主要算法介紹和實驗結果，非常適合小白學習。最后，附上原作者的項目鏈接文章來源：http://www.zghlxwxcb.cn/news/detail-856980.html
我個人改進后的項目鏈接文章來源地址http://www.zghlxwxcb.cn/news/detail-856980.html
到了這里，關于基于ES-EKF的LiDAR/GNSS/IMU傳感器融合軌跡估計（附項目源碼）的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！
