卡爾曼濾波在大學課程《現代控制理論》當中有涉及詳細講解。卡爾曼濾波使用條件有：1、線性系統； 2、系統中噪聲(不確定性)服從高斯分布。下文中的方差、誤差、偏差、協方差都指不確定性的意思。誤差 = 偏差；方差 = 偏差 ²。在無人駕駛導航定位當中，需要多傳感器對汽車位姿進行檢測 ；在多傳感器融合方面，使用卡爾曼濾波理論較多。
1、5個公式
2、汽車例子
1 clear all; 2 close all; 3 clc; 4 Z = (1:100); %觀測值 5 noise = randn(1,100);%1行100列高斯白噪聲 6 Z = Z + noise; 7 8 X = [0;0];%狀態值 9 P = [1 0; 0 1];%狀態協方差矩陣 10 F = [1 1; 0 1];%狀態轉移矩陣 11 Q = [0.0001, 0; 0 0.0001];%狀態轉移協方差矩陣 12 H = [1 0];%觀測矩陣 13 R = 1;%觀測噪聲方差 14 15 figure; 16 %hold on; 17 speed = []; 18 distance = []; 19 for i =1:100 20 %% 預測 21 X_ = F*X; 22 P_ = F*P*F' + Q; 23 %% 更新 24 K = P_*H'/(H*P_*H' + R); 25 X = X_ + K*(Z(i) - H*X_); 26 P = (eye(2) - K*H)*P_; 27 28 speed(i) = X(2); 29 distance(i) = X(1); 30 %%plot(X(1), X(2)); 31 end 32 plot(distance, speed);
3、溫度估計例子
在線性高斯系統中，卡爾曼濾波器構成了該系統中的最大后驗概率估計。而且，由於高斯分布經過線性變換后仍服從高斯分布，所以整個過程中我們沒有進行任何的近似。可以說，卡爾曼濾波器構成了線性系統的最優無偏估計。
SLAM 中的運動方程和觀測方程通常是非線性函數，尤其是視覺 SLAM 中的相機模型，需要使用相機內參模型以及李代數表示的位姿，更不可能是一個線性系統。一個高斯分布，經過非線性變換后，往往不再是高斯分布，所以在非線性系統中，我們必須取一定的近似，將一個非高斯的分布近似成一個高斯分布。我們希望把卡爾曼濾波器的結果拓展到非線性系統中來，稱為擴展卡爾曼濾波器（Ex-tended Kalman Filter，EKF）。通常的做法是，在某個點附近考慮運動方程以及觀測方程的一階泰勒展開，只保留一階項，即線性的部分，然后按照線性系統進行推導。
EKF的局限性：
1. 首先，濾波器方法在一定程度上假設了馬爾可夫性，也就是 k 時刻的狀態只與 k −1時刻相關，而與 k − 1 之前的狀態和觀測都無關（或者和前幾個有限時間的狀態相關）。這有點像是在視覺里程計中，只考慮相鄰兩幀關系一樣。如果當前幀確實與很久之前的數據有關（例如回環），那么濾波器就會難以處理這種情況。而非線性優化方法則傾向於使用所有的歷史數據。它不光考慮鄰近時刻的特征點與軌跡關系，更會把考慮很久之前的狀態也考慮進來，稱為全體時間上的 SLAM（Full-SLAM）。在這種意義下，非線性優化方法使用了更多信息，當然也需要更多的計算。
2.與第六章介紹的優化方法相比，EKF 濾波器僅在 k−1時刻相機位姿處做了一次線性化，然后就直接根據這次線性化結果，把后驗概率給算了出來。這相當於在說，我們認為該點處的線性化近似，在后驗概率處仍然是有效的。而實際上，當我們離開工作點較遠的時候，一階泰勒展開並不一定能夠近似整個函數，這取決於運動模型和觀測模型的非線性情況。如果它們有強烈的非線性，那線性近似就只在很小范圍內成立，不能認為在很遠的地方仍能用線性來近似。這就是 EKF 的非線性誤差，是它的主要問題所在。在優化問題中，盡管我們也做一階（最速下降）或二階（G-N 或 L-M）的近似，但每迭代一次，狀態估計發生改變之后，我們會重新對新的估計點做泰勒展開，而不像EKF 那樣只在固定點上做一次泰勒展開。這就導致優化方法適用范圍更廣，則在狀態變化較大時亦能適用。
3.從程序實現上來說，EKF 需要存儲狀態量的均值和方差，並對它們進行維護和更新。如果把路標也放進狀態的話，由於視覺 SLAM 中路標數量很大，這個存儲量是相當可觀的，且與狀態量呈平方增長（因為要存儲協方差矩陣）。因此，EKF SLAM 普遍認為不可適用於大型場景。
參考：https://blog.csdn.net/u010720661/article/details/63253509