卡尔曼滤波器（Kalman Filter） 是一种用于估计动态系统状态的最优估计算法，广泛应用于导航、机器人、控制系统、信号处理等领域。它的核心思想是通过融合预测（模型）和观测（测量），在存在不确定性的情况下，对系统的状态进行实时、递归的最优估计。

核心思想

1. 预测（Predict）
   根据系统的动态模型（例如物理运动方程）预测当前状态，并考虑过程噪声（模型误差）。

2. 更新（Update）
   利用实际测量值修正预测值，同时考虑测量噪声（传感器误差）。通过计算卡尔曼增益（Kalman Gain），在预测和测量之间找到一个最优权重。

关键特点

• 递归性：只需当前时刻的预测和测量，无需存储历史数据，计算高效。
• 最优性：在高斯噪声假设下，提供最小均方误差意义下的最优估计。
• 适用性：适用于线性系统，非线性系统需扩展（如扩展卡尔曼滤波器，EKF）。

基本公式（线性系统）

1. 状态预测：
   [ \hat{x}k^- = A \hat{x}{k-1} + B u_k ]

   • ( \hat{x}_k^- )：预测状态
   • ( A )：状态转移矩阵
   • ( B u_k )：控制输入的影响

2. 协方差预测：
   [ Pk^- = A P{k-1} A^T + Q ]

   • ( P_k^- )：预测状态的不确定性（协方差）
   • ( Q )：过程噪声协方差

3. 卡尔曼增益计算：
   [ K_k = P_k^- H^T (H P_k^- H^T + R)^{-1} ]

   • ( K_k )：卡尔曼增益（权衡预测与测量的权重）
   • ( H )：观测矩阵
   • ( R )：测量噪声协方差

4. 状态更新：
   [ \hat{x}_k = \hat{x}_k^- + K_k (z_k - H \hat{x}_k^-) ]

   • ( z_k )：实际测量值
   • ( z_k - H \hat{x}_k^- )：测量残差（观测与预测的差异）

5. 协方差更新：
   [ P_k = (I - K_k H) P_k^- ]

直观理解

• 卡尔曼增益 ( K_k )：

  • 若测量噪声小（( R ) 小），则 ( K_k ) 大，更信任测量值。
  • 若模型预测噪声小（( Q ) 小），则 ( K_k ) 小，更信任预测值。

• 应用举例：

  • GPS定位：预测车辆位置（基于速度模型），再通过GPS测量值修正。
  • 无人机姿态估计：融合陀螺仪（高频但漂移）和加速度计（低频但稳定）的数据。

扩展形式

• 扩展卡尔曼滤波器（EKF）：通过泰勒展开处理非线性系统。
• 无迹卡尔曼滤波器（UKF）：通过采样点（Sigma点）逼近非线性分布。
• 粒子滤波器：适用于非高斯、强非线性系统。

总结

卡尔曼滤波器通过动态平衡模型预测与传感器测量，在噪声环境中实现高精度状态估计，是许多实时系统的核心算法。理解其核心在于掌握预测-更新循环和不确定性传播的逻辑。