卡爾曼濾波的本質是用數學方法平衡預測與觀測的可信度 ，通過不斷迭代逼近真實狀態。其高效性和魯棒性，通常在導航定位中，需要融合GPS、加速度計、陀螺儀、激光雷達或攝像頭數據，來提高位置精度。簡單講，卡爾曼濾波就是通過預測-更新循環 ，動態權衡模型預測與傳感器測量，在噪聲環境中實現最優估計，其數學本質是貝葉斯濾波在高斯噪聲下的解析解。
1. 原理概述
卡爾曼濾波的核心是遞歸地結合預測與測量 ，在存在噪聲的系統中實現對狀態的最優估計。其核心步驟是：
- 預測 ：根據系統模型（如運動方程）預測當前狀態和誤差協方差。
- 更新 ：利用傳感器測量值修正預測結果，得到更精確的狀態估計。
核心優勢：
- 實時性 ：僅需當前時刻的數據和前一時刻的狀態，無需存儲歷史數據。
- 最優性 ：在噪聲為高斯分布時，卡爾曼濾波是最小均方誤差（MMSE）意義下的最優估計器
2. 公式推導
2.1 狀態預測
狀態預測方程： x k a = F k x k ? 1 + B k u k x_k^a = F_kx_{k-1}+B_ku_k xka?=Fk?xk?1?+Bk?uk? ，其中 x k a x_k^a xka?是預測狀態， F k F_k Fk? 是狀態轉移矩陣， B k u k B_ku_k Bk?uk? 是控制輸入（如加速度）
協方差預測方程 ： P k a = F k P k ? 1 F k T + Q k P_k^a=F_kP_{k-1}F_k^T + Q_k Pka?=Fk?Pk?1?FkT?+Qk?，其中 P k a P_k^a Pka?是預測協方差， Q k Q_k Qk?是過程噪聲協方差（模型不確定性）
2.2 狀態更新
卡爾曼增益計算： K k = P k a H k T ( H k P k a H k T + R k ) ? 1 K_k=P_k^aH_k^T(H_kP_k^aH_k^T + R_k)^{-1} Kk?=Pka?HkT?(Hk?Pka?HkT?+Rk?)?1， 其中 K k K_k Kk?權衡預測與測量的信任度， H k H_k Hk?是觀測矩陣， R k R_k Rk?是測量噪聲協方差。
狀態更新方程 ： x k = x k a + K k ( z k ? H k x k a ) x_k=x_k^a + K_k(z_k - H_kx_k^a) xk?=xka?+Kk?(zk??Hk?xka?)，其中 z k z_k zk?是實際測量值，修正預測結果。
協方差更新方程： P k = ( I ? K k H k ) P k a P_k = (I-K_kH_k)P_k^a Pk?=(I?Kk?Hk?)Pka?, 更新后的協方差反映估計的不確定性。
3. 示例說明：小車運動位置估計為例
- 場景 ：小車以2m/s運動，t-1時刻位置為6m，t時刻雷達測得位置為9m。
- 問題 ：雷達測量存在噪聲（如高斯噪聲），如何估計真實位置？
- 步驟：
- 預測 ：
假設無控制輸入 u k = 0 u_k=0 uk?=0，狀態轉移矩陣 F=1（位置預測僅依賴前一時刻），預測位置為： x k a = 6 m + 2 m / s ? 1 s = 8 m x_k^a = 6m + 2m/s * 1s = 8m xka?=6m+2m/s?1s=8m
協方差 P k a P_k^a Pka?反映預測的不確定性（假設初始協方差較小） - 更新：
測量值 z k = 9 m z_k =9m zk?=9m，計算卡爾曼增益 K k K_k Kk?， 假設測量噪聲 R 較大（雷達不準），則 K k K_k Kk?較小，最終估計值會介于預測值（8m）和測量值（9m）之間，例如8.5m。更新后的協方差 P k P_k Pk?會更小，反映更高的置信度。
4. 其它擴展對比
關鍵關系總結：
- EKF 是最早的非線性濾波方法，但受限于線性化誤差。
- UKF 通過無跡變換避免線性化，精度更高，適用于強非線性系統。
- IEKF 和 AKF 是EKF的改進，分別通過迭代和自適應機制提升魯棒性。
- EnKF 和 PF 針對高維或非高斯問題，與UKF同屬非線性濾波但方法不同。
- CKF 是UKF的優化變種，采用容積采樣提升效率
| 算法 | 核心原理 | 優缺點 | 應用場景 | 與KF對比關系 |
|---|---|---|---|---|
| 卡爾曼濾波 (KF) | 基于線性系統模型，通過狀態方程和觀測方程遞推估計最優狀態。 | 優點：計算高效，理論最優； 缺點：無法直接處理非線性問題。 | 僅適用于線性系統。 | 基礎算法，其他擴展均針對其無法處理非線性系統的局限性進行改進。 |
| 擴展卡爾曼濾波 (EKF) | 通過泰勒展開對非線性系統在當前估計點處線性化，應用標準卡爾曼濾波框架。 | 優點：計算效率高； 缺點：線性化誤差大，對強非線性系統不穩定。 | 機器人定位、傳感器融合（如SLAM中的非線性運動模型）。 | 基礎非線性濾波方法，EKF通過線性化擴展到非線性系統，但精度受限于泰勒展開的截斷誤差 |
| 無跡卡爾曼濾波 (UKF) | 通過無跡變換（UT）選擇采樣點（Sigma點），直接近似狀態分布的均值和協方差，避免顯式線性化。 | 優點：精度高于EKF，無需計算雅可比矩陣； 缺點：計算量略高，對高維系統效率下降。 | 非線性系統狀態估計（如目標跟蹤、姿態估計）。 | UKF避免線性化誤差，直接處理非線性，但計算復雜度增加 |
| 自適應卡爾曼濾波 (AKF) | 動態調整噪聲協方差矩陣（Q/R）或狀態模型，以適應系統統計特性變化。 | 優點：魯棒性強； 缺點：參數調整復雜，可能引入額外計算負擔。 | 時變噪聲環境（如傳感器漂移、突變干擾）。 | 可與EKF/UKF結合，增強對不確定噪聲的適應性。 |
| 迭代擴展卡爾曼濾波 (IEKF) | 在EKF基礎上多次迭代更新狀態估計，逐步修正線性化誤差。 | 優點：收斂性優于EKF； 缺點：計算量顯著增加。 | 高精度定位（如衛星導航、工業控制）。 | EKF的改進版，通過迭代減少線性化截斷誤差。 |
| 集合卡爾曼濾波 (EnKF) | 用隨機采樣集合表示狀態分布，通過集合統計量近似協方差，適用于高維非線性系統。 | 優點：適合高維系統； 缺點：需大量樣本，可能低估協方差。 | 氣象預測、海洋模型等大規模動態系統。 | 與UKF類似但采樣方式不同，屬于蒙特卡洛方法。 |
| 粒子濾波 (PF) | 基于序貫重要性采樣，用大量粒子表示狀態分布，適用于非高斯、非線性系統。 | 優點：無需假設高斯分布； 缺點：計算成本極高，粒子退化問題。 | SLAM、目標跟蹤（非高斯噪聲場景）。 | PF完全擺脫線性/高斯假設，但計算資源需求遠高于KF |
| 容積卡爾曼濾波 (CKF) | 利用球面-徑向容積規則選擇采樣點，計算非線性積分，平衡精度與效率。 | 優點：精度接近UKF，計算效率更高； 缺點：對極端非線性可能不穩定。 | 航空航天、多傳感器融合。 | CKF通過更高效的采樣策略提升非線性處理能力，但適用場景與UKF類似 |
更新公告)