卡尔曼滤波的本质是用数学方法平衡预测与观测的可信度 ，通过不断迭代逼近真实状态。其高效性和鲁棒性，通常在导航定位中，需要融合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=Fkxk−1+Bkuk ，其中 x k a x_k^a xka是预测状态， F k F_k Fk 是状态转移矩阵， B k u k B_ku_k Bkuk 是控制输入（如加速度）
协方差预测方程 ： 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=FkPk−1FkT+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=PkaHkT(HkPkaHkT+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−Hkxka)，其中 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−KkHk)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类似 |