卡尔曼滤波解释及示例

卡尔曼滤波的本质是用数学方法平衡预测与观测的可信度 ，通过不断迭代逼近真实状态。其高效性和鲁棒性，通常在导航定位中，需要融合GPS、加速度计、陀螺仪、激光雷达或摄像头数据，来提高位置精度。简单讲，卡尔曼滤波就是通过预测-更新循环 ，动态权衡模型预测与传感器测量，在噪声环境中实现最优估计，其数学本质是贝叶斯滤波在高斯噪声下的解析解。

1. 原理概述

卡尔曼滤波的核心是递归地结合预测与测量 ，在存在噪声的系统中实现对状态的最优估计。其核心步骤是：

1. 预测 ：根据系统模型（如运动方程）预测当前状态和误差协方差。
2. 更新 ：利用传感器测量值修正预测结果，得到更精确的状态估计。

核心优势：

• 实时性 ：仅需当前时刻的数据和前一时刻的状态，无需存储历史数据。
• 最优性 ：在噪声为高斯分布时，卡尔曼滤波是最小均方误差（MMSE）意义下的最优估计器

2. 公式推导

2.1 状态预测

状态预测方程：$x_k^a=F_k{x}_{k-1}+B_k{u}_k$ ，其中$x_k^a$是预测状态，$F_k$ 是状态转移矩阵，$B_k{u}_k$ 是控制输入（如加速度）

协方差预测方程 ：$P_k^a=F_k{P}_{k-1}{F}_k^T+Q_k$，其中$P_k^a$是预测协方差，$Q_k$是过程噪声协方差（模型不确定性）

2.2 状态更新

卡尔曼增益计算：$K_k=P_k^a{H}_k^T(H_k{P}_k^a{H}_k^T+R_k{)}^{-1}$， 其中$K_k$权衡预测与测量的信任度，$H_k$是观测矩阵，$R_k$是测量噪声协方差。

状态更新方程 ：$x_k=x_k^a+K_k(z_k-H_k{x}_k^a)$，其中$z_k$是实际测量值，修正预测结果。

协方差更新方程：$P_k=(I-K_k{H}_k)P_k^a$, 更新后的协方差反映估计的不确定性。

3. 示例说明：小车运动位置估计为例

• 场景 ：小车以2m/s运动，t-1时刻位置为6m，t时刻雷达测得位置为9m。
• 问题 ：雷达测量存在噪声（如高斯噪声），如何估计真实位置？
• 步骤：

1. 预测 ：
   假设无控制输入$u_k=0$，状态转移矩阵 F=1（位置预测仅依赖前一时刻），预测位置为：$x_k^a=6m+2m/s\ast 1s=8m$
   协方差$P_k^a$反映预测的不确定性（假设初始协方差较小）
2. 更新：
   测量值 $z_k=9m$，计算卡尔曼增益 $K_k$， 假设测量噪声 R 较大（雷达不准），则 $K_k$较小，最终估计值会介于预测值（8m）和测量值（9m）之间，例如8.5m。更新后的协方差 $P_k$会更小，反映更高的置信度。

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类似