使用卡爾曼濾波框架做松耦合融合

關於松耦合融合

在狀態預測過程中，最經典的是卡爾曼濾波，主要作用是可將多個傳感器做融合處理，估計出一個較好的狀態值，處理一般分爲松耦合和緊耦合，
松耦合是指只用傳感器的結果數據作爲觀測值，對預測值進行更新。
例如，使用IMU作爲預測的原始數據進行計算，使用GPS定位的結果作爲觀測值進行誤差修正更新，邏輯簡單實用，而且可以使用多傳感器做融合來提高精度。
而緊耦合時，要使用GPS的僞距數據，以及其他的更原始更全的數據進行計算，比較複雜。

例如，
可以在EKF預測運算時，使用IMU作爲預測數據進行計算，只要有IMU數據產生，就立即進行位姿預測，而使用其他傳感器的數據作爲參考觀測值，例如，GPS數據和LiDAR數據，當有觀測值數據產生時，使用觀測值對預測值進行修正，並且更新IMU和預測數據，從而保證預測結果不會產生太大的累積誤差。
所以，松耦合所使用的傳感器越多越容易糾正估計誤差，而且主流做法都是把IMU作爲預測數據傳感器，把其他傳感器作爲觀測傳感器。

關於卡爾曼濾波

關於卡爾曼濾波的詳細介紹推薦一篇博文 深度解析卡爾曼濾波在IMU中的使用，寫的很詳細，
在此簡要提取主要步驟，以備參考。

兩個模型

狀態方程
$x_k = Fx_{k-1} + Bu_k+w_k$
測量方程
$z_k = Hx_k+v_k$

預測階段

k時刻系統狀態先驗估計
$\hat {x}_{k|k-1} = F \hat x_{k-1|k-1} +Bu_k$
計算誤差協方差
$P_{k|k-1} = FP_{k-1|k-1}F^T +Q_k$

更新/校正階段

計算殘差
$\tilde y_k = z_k - H \hat x_{k|k-1}$
計算殘差協方差
$S_k = HP_{k|k-1}H^{T} +R$
計算增益(置信度)
$K_k = P_{k|k-1}H^TS_k^{-1}$