卡尔曼滤波详解

卡尔曼滤波（Kalman Filter）是一种高效的递归滤波器，用于从包含噪声的观测数据中估计动态系统的状态。它广泛应用于导航、控制系统、信号处理、机器人、金融等领域。以下是对卡尔曼滤波的详细解析：

1. 核心思想

卡尔曼滤波通过预测-更新两个步骤迭代进行，结合系统模型（动态方程）和观测数据，最优地估计系统状态：

• 预测：基于系统模型预测当前状态及其不确定性。
• 更新：利用观测数据修正预测值，得到更精确的状态估计。

2. 基本假设

• 线性系统：状态转移和观测模型均为线性（可通过扩展卡尔曼滤波/EKF处理非线性）。
• 高斯噪声：过程噪声（系统噪声）和观测噪声均服从零均值高斯分布。
• 马尔可夫性：当前状态仅依赖前一状态，与更早状态无关。

3. 数学模型

(1) 状态方程（预测）

 ​

•  ​：n维状态向量（待估计量）。
•  ：状态转移矩阵（描述系统动态）。
•  ​：控制输入（可选）。
•  ​：控制输入矩阵。
• ：过程噪声，协方差矩阵为 ​。

(2) 观测方程（更新）

​

• ：m维观测向量。
• ：观测矩阵（连接状态与观测）。
• ：观测噪声，协方差矩阵为 。

4. 算法步骤

预测阶段

1. 状态预测：
   ​
2. 误差协方差预测：
   

更新阶段

1. 计算卡尔曼增益（权衡预测与观测的权重）：
   
2. 状态更新：
   
3. 协方差更新：
   

5. 关键概念

• 卡尔曼增益：决定相信预测模型还是观测数据：
  • 若观测噪声小  ，更信任观测。
  • 若预测噪声小 ，更信任预测。
• 协方差矩阵P：表示状态估计的不确定性，迭代中自动调整。

6. 扩展与变种

• 扩展卡尔曼滤波（EKF）：通过局部线性化处理非线性系统。
• 无迹卡尔曼滤波（UKF）：使用采样点近似非线性分布。
• 粒子滤波：适用于非高斯、强非线性系统。

7. 示例：一维运动模型

假设小车以恒定速度运动，受噪声干扰：

• 状态：位置p和速度v，即  。
• 状态转移：
  ，
• 观测：仅测量位置， ，观测噪声方差 。

8. 实现代码（Python伪代码）

def kalman_filter(x_prev, P_prev, z, F, H, Q, R):
    # 预测
    x_pred = F @ x_prev
    P_pred = F @ P_prev @ F.T + Q
    
    # 更新
    K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
    x_new = x_pred + K @ (z - H @ x_pred)
    P_new = (np.eye(len(x_prev)) - K @ H) @ P_pred
    
    return x_new, P_new

9. 应用场景

• GPS定位：融合卫星观测与运动模型。
• 自动驾驶：跟踪车辆或行人状态。
• 金融：估计隐藏的市场变量。

10. 优缺点

• 优点：计算高效、适用于实时系统、最优线性估计。
• 缺点：对非线性/非高斯系统需改进，依赖准确的噪声统计。

通过理解卡尔曼滤波的预测-更新机制和协方差管理，可以灵活应用于各类动态系统状态估计问题。