卡尔曼滤波在大学课程《现代控制理论》当中有涉及详细讲解。卡尔曼滤波使用条件有：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