卡尔曼滤波解释及示例

卡尔曼滤波的本质是用数学方法平衡预测与观测的可信度 ，通过不断迭代逼近真实状态。其高效性和鲁棒性，通常在导航定位中，需要融合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的优化变种，采用容积采样提升效率