卡尔曼滤波详解：概念与公式推导-优快云博客

本文链接：https://blog.youkuaiyun.com/JMFS1119/article/details/149444300

核心目标： 在存在不确定性（噪声）的动态系统中，结合系统模型（预测）和传感器测量（观测），得到比单独使用模型或测量更准确、更可靠的系统状态最优估计。它本质上是基于概率（高斯分布）的最优信息融合算法。

想象场景：

你开着一辆车。
你有系统模型：知道油门踩多大车会怎么加速，刹车踩多大车会怎么减速（但模型不完美）。
你有传感器（如GPS）：告诉你当前在哪（但测量有误差）。
卡尔曼滤波就像一个聪明的助手：它结合你对车运动的了解（预测）和GPS读数（测量），给你一个更准确的位置和速度估计，并且告诉你这个估计有多可靠（不确定性）。

一. 核心概念与数学符号

状态 (State - x)： 你想要估计的系统内部变量集合。用向量表示。
- 例如：位置 p 和速度 v -> x = [p; v] (列向量)
- xᵏ 表示在时间步 k 的状态向量。xᵏ⁻¹ 表示上一时刻 (k-1) 的状态。
系统模型 (动态模型)： 描述状态如何随时间演变。
- 状态转移方程： xᵏ = Fᵏ⁻¹ * xᵏ⁻¹ + Bᵏ⁻¹ * uᵏ⁻¹ + wᵏ⁻¹
  - Fᵏ⁻¹ (状态转移矩阵): 描述系统如何从 xᵏ⁻¹ 自然演化到 xᵏ (忽略控制输入和噪声)。例如，状态 [p; v]，时间间隔 Δt，则 Fᵏ⁻¹ = [[1, Δt]; [0, 1]]。含义：新位置 pᵏ = pᵏ⁻¹ + vᵏ⁻¹ * Δt；新速度 vᵏ = vᵏ⁻¹。
  - Bᵏ⁻¹ (控制输入矩阵): 描述外部控制输入 uᵏ⁻¹ 如何影响状态变化。
  - uᵏ⁻¹ (控制向量): 在时刻 k-1 施加到系统上的已知控制量（如油门、舵角）。
  - wᵏ⁻¹ (过程噪声): 代表模型不精确性、未建模干扰。假设它是均值为0、协方差矩阵为 Qᵏ⁻¹ 的高斯白噪声：wᵏ⁻¹ ~ N(0, Qᵏ⁻¹)。Qᵏ⁻¹ 的大小反映模型信任度（Q 越大，模型越不可靠）。
测量模型 (观测模型)： 描述传感器测量值与系统状态的关系。
- 测量方程： zᵏ = Hᵏ * xᵏ + vᵏ
  - zᵏ (测量向量): 在时刻 k 通过传感器实际观测到的值（如GPS位置读数）。
  - Hᵏ (观测矩阵): 将状态空间映射到测量空间。例如，状态是 [p; v]，但只能测量位置 p，则 Hᵏ = [1, 0]（表示：测量值 = 1 * 位置 + 0 * 速度）。
  - vᵏ (测量噪声): 代表传感器误差。假设它是均值为0、协方差矩阵为 Rᵏ 的高斯白噪声：vᵏ ~ N(0, Rᵏ)。Rᵏ 的大小反映传感器信任度（R 越大，传感器越不精确）。
估计误差协方差矩阵 (State Estimation Error Covariance - P)： 表示对状态估计值的不确定性程度。
- Pᵏ：在时刻 k 的状态估计误差协方差矩阵。
- 对角线元素 Pᵏ[i, i] 是状态分量 xᵏ[i] 估计误差的方差（不确定性大小的平方）。
- 非对角线元素 Pᵏ[i, j] 是状态分量 xᵏ[i] 和 xᵏ[j] 估计误差之间的协方差（表示它们误差的相关性）。
- P 越小，表示对估计值越有信心。
- 例如，状态 [p; v]，Pᵏ 可能为：
```
Pᵏ = [ σₚ²     σₚσᵥ ]   # σₚ² 是位置误差方差, σᵥ² 是速度误差方差
     [ σₚσᵥ    σᵥ²  ]   # σₚσᵥ 是位置和速度误差的协方差
```
卡尔曼增益 (Kalman Gain - K)： 它是一个权重矩阵，决定了在更新时，我们是更相信预测 (xᵏ⁻, Pᵏ⁻) 还是更相信新测量 (zᵏ)。Kᵏ 根据预测和测量的相对可靠性（由 Pᵏ⁻ 和 Rᵏ 体现）动态计算。
- 测量很可靠 (Rᵏ 小)： Kᵏ 较大 -> 更新更倾向于测量值。
- 预测很可靠 (Pᵏ⁻ 小)： Kᵏ 较小 -> 更新更倾向于预测值。
高斯分布 (Gaussian Distribution)： 卡尔曼滤波的数学基础。
- 假设：状态估计值、过程噪声 w、测量噪声 v 都服从高斯（正态）分布。
- 关键性质： 两个高斯分布（先验预测分布 N(xᵏ⁻, Pᵏ⁻) 和测量似然分布 N(zᵏ | Hᵏxᵏ, Rᵏ)）的融合结果（后验分布） p(xᵏ | zᵏ) 仍然是一个高斯分布 N(xᵏ, Pᵏ)。
- 卡尔曼滤波公式的目标就是计算这个融合后高斯分布的均值 (xᵏ) 和协方差 (Pᵏ)。

二. 卡尔曼滤波算法步骤 (预测与更新)

卡尔曼滤波是递归算法，每个时间步 k 包含两步：

预测步 (Predict Step) - 基于模型向前推算
- 输入： 上一时刻的最优估计 xᵏ⁻¹ 及其协方差 Pᵏ⁻¹，系统模型 (Fᵏ⁻¹, Bᵏ⁻¹, uᵏ⁻¹, Qᵏ⁻¹)。
- 输出： 当前时刻的预测状态 xᵏ⁻ 和预测协方差 Pᵏ⁻。
- 公式推导与解释：
  - 预测状态 (xᵏ⁻)：
    直接从状态转移方程出发，忽略过程噪声的均值（因为 E[wᵏ⁻¹] = 0）：
    xᵏ⁻ = Fᵏ⁻¹ * xᵏ⁻¹ + Bᵏ⁻¹ * uᵏ⁻¹ (1)
    
    解释： 用状态转移矩阵 Fᵏ⁻¹ 乘以上一时刻的最优估计 xᵏ⁻¹，再加上控制输入 Bᵏ⁻¹ * uᵏ⁻¹ 的影响，得到当前时刻的预测状态 xᵏ⁻。
  - 预测协方差 (Pᵏ⁻)：
    协方差表示不确定性。预测步骤会引入新的不确定性（来自过程噪声 wᵏ⁻¹）。需要计算预测状态 xᵏ⁻ 的协方差。
    根据协方差的定义和线性变换的性质（假设 xᵏ⁻¹ 和 wᵏ⁻¹ 不相关）：
```
Pᵏ⁻ = Cov(xᵏ⁻)
     = Cov(Fᵏ⁻¹ * xᵏ⁻¹ + Bᵏ⁻¹ * uᵏ⁻¹ + wᵏ⁻¹)
     = Cov(Fᵏ⁻¹ * xᵏ⁻¹) + Cov(wᵏ⁻¹)  // 已知控制项 uᵏ⁻¹ 协方差为0(已知的确定输入,非随机变量)
     = Fᵏ⁻¹ * Cov(xᵏ⁻¹) * (Fᵏ⁻¹)ᵀ + Cov(wᵏ⁻¹)//随机变量被矩阵 A 线性变换后，其协方差等于 A×原协方差×Aᵀ
     = Fᵏ⁻¹ * Pᵏ⁻¹ * (Fᵏ⁻¹)ᵀ + Qᵏ⁻¹
```
    解释：
    - Fᵏ⁻¹ * Pᵏ⁻¹ * (Fᵏ⁻¹)ᵀ：将上一时刻估计的不确定性 Pᵏ⁻¹ 通过状态转移矩阵 Fᵏ⁻¹ 传播到当前时刻。上一时刻位置和速度的不确定度以及它们的相关性，经过运动模型推算，会影响当前时刻位置和速度的不确定度及相关性。
    - + Qᵏ⁻¹：加上过程噪声 wᵏ⁻¹ 的协方差 Qᵏ⁻¹，代表模型本身的不精确性带来的额外不确定性。
      Pᵏ⁻ = Fᵏ⁻¹ * Pᵏ⁻¹ * (Fᵏ⁻¹)ᵀ + Qᵏ⁻¹ (2)
- 预测步总结： 这一步利用系统模型 (F, B, u) 和上一时刻的最优估计 (xᵏ⁻¹, Pᵏ⁻¹)，计算出当前时刻的预测状态 xᵏ⁻ 和预测协方差 Pᵏ⁻。预测协方差 Pᵏ⁻ 通常比 Pᵏ⁻¹ 大，表示预测增加了不确定性。
更新步 (Update Step) - 融合预测和新测量
- 输入： 预测值 xᵏ⁻, 预测协方差 Pᵏ⁻, 新测量值 zᵏ, 测量模型 (Hᵏ, Rᵏ)。
- 输出： 当前时刻的最优估计状态 xᵏ 和最优估计协方差 Pᵏ。
- 公式推导与解释 (核心！)：
  目标：融合预测分布（先验） N(xᵏ⁻, Pᵏ⁻) 和测量似然 p(zᵏ | xᵏ) = N(zᵏ; Hᵏxᵏ, Rᵏ)，得到后验分布 p(xᵏ | zᵏ) = N(xᵏ, Pᵏ)。根据贝叶斯定理：
```
后验分布 ∝ 先验分布 * 似然函数
```
  因为先验和似然都是高斯的，后验也是高斯的。卡尔曼滤波的更新公式就是为了计算这个后验高斯的均值和协方差。
  - 新息/残差 (Innovation / Residual - ỹᵏ)：
    计算实际测量值 zᵏ 与 预测状态在测量空间的表现 Hᵏ * xᵏ⁻ (预测的测量值) 之间的差异：
    ỹᵏ = zᵏ - Hᵏ * xᵏ⁻ (3)
    
    解释： 这个差异包含了预测值和测量值不一致的信息，是融合的关键输入。
  - 新息协方差 (Innovation Covariance - Sᵏ)：
    表示预测测量值 (Hᵏ * xᵏ⁻) 和实际测量值 (zᵏ) 之间差异的不确定性。它由两部分组成：
    1. 预测状态的不确定性 Pᵏ⁻ 映射到测量空间：Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ
    2. 测量噪声的不确定性：Rᵏ
```
Sᵏ = Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ + Rᵏ  (4)
```
    解释： Sᵏ 越大，表示预测和测量之间的差异越不可信（可能是预测不准或测量不准或两者都不准）。它是计算卡尔曼增益的关键。
  - 卡尔曼增益 (Kᵏ)：
    这是融合的核心权重矩阵。它决定了用多少新息 ỹᵏ 来修正预测状态 xᵏ⁻。
```
Kᵏ = Pᵏ⁻ * (Hᵏ)ᵀ * (Sᵏ)⁻¹
   = Pᵏ⁻ * (Hᵏ)ᵀ * (Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ + Rᵏ)⁻¹  (5)
```
    推导思路： 卡尔曼滤波可以证明是在线性高斯假设下，使得状态估计误差的均方值 E[(xᵏ - xᵏᵗʳᵘᵉ)ᵀ(xᵏ - xᵏᵗʳᵘᵉ)] 最小的最优估计。通过最小化该均方误差准则，可以推导出 Kᵏ 的表达式。
    直观理解：
    - 分子 Pᵏ⁻ * (Hᵏ)ᵀ：描述了状态空间的不确定性 Pᵏ⁻ 如何通过观测矩阵 Hᵏ 关联到测量空间。
    - 分母 Sᵏ = Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ + Rᵏ：测量空间的总不确定性（预测映射过来的 + 测量噪声）。
    - Kᵏ 的意义： 如果测量很可靠 (Rᵏ 小)，Sᵏ ≈ Rᵏ 很小，(Sᵏ)⁻¹ 很大，导致 Kᵏ 很大 -> 更新更相信测量 (zᵏ)。如果预测在测量空间很可靠 (Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ 小) 或测量很不可靠 (Rᵏ 大)，Kᵏ 会很小 -> 更新更相信预测 (xᵏ⁻)。
  - 最优估计状态 (xᵏ)：
    用卡尔曼增益 Kᵏ 将新息 ỹᵏ 从测量空间加权“拉回”到状态空间，对预测状态进行修正：
```
xᵏ = xᵏ⁻ + Kᵏ * ỹᵏ
   = xᵏ⁻ + Kᵏ * (zᵏ - Hᵏ * xᵏ⁻)  (6)
```
    解释： 这是信息融合的核心操作。在预测状态 xᵏ⁻ 的基础上，加上一个修正项 Kᵏ * ỹᵏ。修正项的大小和方向由新息 ỹᵏ（测量值和预测测量值的差异）决定，强度由卡尔曼增益 Kᵏ 控制。
  - 最优估计协方差 (Pᵏ)：
    融合后的估计应该比单独的预测或测量更精确（不确定性更小）。协方差的更新公式反映了这种不确定性的减少：
```
Pᵏ = (I - Kᵏ * Hᵏ) * Pᵏ⁻  (7)  // 经典形式 (最常用)
```
    推导思路： 同样源于最小均方误差准则或高斯分布融合公式。
    直观理解： (I - Kᵏ * Hᵏ) 是一个“压缩因子”。Kᵏ * Hᵏ 代表了利用测量信息对状态不确定性的修正比例。Pᵏ 等于预测协方差 Pᵏ⁻ 减去 Kᵏ * Hᵏ * Pᵏ⁻ 这部分（相当于通过测量消除掉的那部分不确定性）。因此 Pᵏ < Pᵏ⁻ (在矩阵正定意义上)，体现了融合后估计的精度提高。
    注：有时也使用更稳定（保证对称正定）但计算量稍大的约瑟夫形式 (Joseph form)：Pᵏ = (I - Kᵏ * Hᵏ) * Pᵏ⁻ * (I - Kᵏ * Hᵏ)ᵀ + Kᵏ * Rᵏ * (Kᵏ)ᵀ。
- 更新步总结： 这一步利用新的传感器测量 zᵏ 及其噪声特性 (Hᵏ, Rᵏ)，结合预测步的结果 (xᵏ⁻, Pᵏ⁻)，通过计算新息 ỹᵏ、新息协方差 Sᵏ、卡尔曼增益 Kᵏ，最终得到融合后的最优状态估计 xᵏ 和显著降低的不确定性 Pᵏ。

三. 卡尔曼滤波流程总结

初始化：
- 设定初始状态估计 x⁰ 和初始协方差矩阵 P⁰。
- P⁰ 通常设得较大（如对角线元素取较大的正数），表示初始不确定性很高。

循环 (对于每个时间步 k = 1, 2, 3, ...):

预测步 (Predict):

xᵏ⁻ = Fᵏ⁻¹ * xᵏ⁻¹ + Bᵏ⁻¹ * uᵏ⁻¹   // (公式 1) 预测状态
Pᵏ⁻ = Fᵏ⁻¹ * Pᵏ⁻¹ * (Fᵏ⁻¹)ᵀ + Qᵏ⁻¹  // (公式 2) 预测协方差 (不确定性增大)

更新步 (Update):

ỹᵏ = zᵏ - Hᵏ * xᵏ⁻                 // (公式 3) 计算新息/残差
Sᵏ = Hᵏ * Pᵏ⁻ * (Hᵏ)ᵀ + Rᵏ          // (公式 4) 计算新息协方差
Kᵏ = Pᵏ⁻ * (Hᵏ)ᵀ * inv(Sᵏ)        // (公式 5) 计算卡尔曼增益 (核心!)
xᵏ = xᵏ⁻ + Kᵏ * ỹᵏ                  // (公式 6) 融合预测和测量，更新状态估计
Pᵏ = (I - Kᵏ * Hᵏ) * Pᵏ⁻            // (公式 7) 更新估计协方差 (不确定性减小!)

xᵏ 和 Pᵏ 成为下一时刻 (k+1) 预测步的输入 xᵏ⁻¹ 和 Pᵏ⁻¹。

四. 关键点与深入理解

最优性： 在线性系统和高斯噪声假设下，卡尔曼滤波是最小均方误差 (MMSE) 意义下的最优估计器。没有其他线性算法能提供更小均方误差的估计。
递归性： 它只需要前一时刻的估计 (xᵏ⁻¹, Pᵏ⁻¹) 和当前测量值 zᵏ，不需要存储整个历史数据，计算效率高（O(n³)，n 为状态维数），非常适合实时嵌入式系统。
不确定性管理： 核心优势在于不仅提供状态估计 xᵏ，还通过 Pᵏ 量化这个估计的不确定性。Pᵏ 是理解估计可靠性和进行决策的关键。
卡尔曼增益 (Kᵏ) 是灵魂： 它动态地、最优地平衡了预测 (Pᵏ⁻) 和测量 (Rᵏ) 的可信度。
假设与局限性：
- 线性系统： 标准KF要求系统模型 (F, B) 和测量模型 (H) 是线性的。对于非线性系统，需要使用扩展卡尔曼滤波 (EKF) 或无迹卡尔曼滤波 (UKF) 等变体。
- 高斯噪声： 假设过程噪声 w 和测量噪声 v 是高斯（正态）白噪声。如果噪声非高斯，KF可能不是严格最优，但有时效果仍不错。如果噪声非白（相关），需要更复杂的处理（如状态增广）。
参数调整 (Q, R)： 过程噪声协方差 Q 和测量噪声协方差 R 是设计者需要设定的关键参数，对滤波器性能影响极大：
- Q 太大： 滤波器过于信任测量 -> 估计跟随测量噪声抖动。
- Q 太小： 滤波器过于信任模型预测 -> 对测量反应迟钝，跟不上真实状态变化。
- R 太大： 滤波器过于信任预测 -> 忽略测量。
- R 太小： 滤波器过于信任测量 -> 估计对测量噪声敏感。

五. 代码示例

import numpy as np
import matplotlib.pyplot as plt

# 卡尔曼滤波类实现
class KalmanFilter:
    """
    一维卡尔曼滤波器，用于跟踪物体的位置和速度
    
    参数:
    - initial_state: 初始状态向量 [位置, 速度]
    - initial_covariance: 初始协方差矩阵 (表示初始不确定性)
    - transition_matrix: 状态转移矩阵 F
    - control_matrix: 控制输入矩阵 B (本例未使用)
    - process_noise: 过程噪声协方差矩阵 Q
    - observation_matrix: 观测矩阵 H
    - measurement_noise: 测量噪声协方差 R
    """
    def __init__(self, initial_state, initial_covariance, transition_matrix, 
                 control_matrix, process_noise, observation_matrix, measurement_noise):
        # 初始化滤波器参数
        self.state = initial_state.astype(float)           # 状态向量: [位置, 速度]^T
        self.covariance = initial_covariance.astype(float) # 状态协方差矩阵
        self.F = transition_matrix.astype(float)           # 状态转移矩阵
        self.B = control_matrix.astype(float)              # 控制输入矩阵
        self.Q = process_noise.astype(float)               # 过程噪声协方差
        self.H = observation_matrix.astype(float)          # 观测矩阵
        self.R = measurement_noise.astype(float)           # 测量噪声协方差
        
    def predict(self, control_input=np.zeros(1)):
        """
        预测步骤: 基于系统模型预测下一时刻状态
        """
        # 状态预测: x_k_minus = F * x_k-1 + B * u_k
        self.state = np.dot(self.F, self.state) + np.dot(self.B, control_input)
        
        # 协方差预测: P_k_minus = F * P_k-1 * F^T + Q
        self.covariance = np.dot(np.dot(self.F, self.covariance), self.F.T) + self.Q
        
        # 返回预测结果
        return self.state, self.covariance
    
    def update(self, measurement):
        """
        更新步骤: 使用新的测量值更新状态估计
        """
        # 计算新息(残差): y_tilde = z_k - H * x_k_minus
        innovation = measurement - np.dot(self.H, self.state)
        
        # 计算新息协方差: S = H * P_k_minus * H^T + R
        innovation_cov = np.dot(self.H, np.dot(self.covariance, self.H.T)) + self.R
        
        # 计算卡尔曼增益: K = P_k_minus * H^T * S^{-1}
        kalman_gain = np.dot(np.dot(self.covariance, self.H.T), np.linalg.inv(innovation_cov))
        
        # 更新状态估计: x_k = x_k_minus + K * y_tilde
        self.state = self.state + np.dot(kalman_gain, innovation)
        
        # 更新协方差估计: P_k = (I - K * H) * P_k_minus
        I = np.eye(self.state.shape[0])
        self.covariance = np.dot(I - np.dot(kalman_gain, self.H), self.covariance)
        
        # 返回更新后的状态和协方差
        return self.state, self.covariance

# 主函数: 演示卡尔曼滤波器的使用
def main():
    # 模拟参数设置
    np.random.seed(42)  # 设置随机种子以确保结果可重现
    true_initial_position = 0.0  # 真实初始位置
    true_velocity = 0.5          # 真实速度 (0.5 m/s)
    dt = 0.1                     # 时间步长 (0.1秒)
    num_steps = 100              # 总步数
    
    # 卡尔曼滤波器参数设置
    # 状态转移矩阵 F: 假设匀速运动模型
    # [位置_new]   = [1  Δt] [位置_old]
    # [速度_new]     [0  1] [速度_old]
    F = np.array([[1, dt],
                  [0, 1]])
    
    # 控制输入矩阵 B (本例不使用控制输入)
    B = np.zeros((2, 1))
    
    # 过程噪声协方差矩阵 Q
    # 表示模型的不确定性。这里假设加速度噪声标准差为0.1 m/s²
    # Q = [Δt⁴/4  Δt³/2] * σ_a²
    #     [Δt³/2   Δt²]
    accel_noise = 0.1
    Q = np.array([[ (dt**4)/4, (dt**3)/2 ],
                  [ (dt**3)/2,    dt**2   ]]) * accel_noise**2
    
    # 观测矩阵 H: 我们只能观测位置，不能直接观测速度
    H = np.array([[1, 0]])
    
    # 测量噪声协方差 R: 位置测量的方差 (假设测量标准差为0.5米)
    position_noise = 0.5
    R = np.array([[position_noise**2]])
    
    # 初始状态估计: [位置, 速度]
    initial_state = np.array([[0.0],  # 初始位置估计
                             [0.0]]) # 初始速度估计
    
    # 初始协方差矩阵: 表示初始估计的不确定性
    initial_covariance = np.array([[10.0, 0.0],  # 位置方差大，表示初始位置不确定
                                   [0.0, 10.0]]) # 速度方差大，表示初始速度不确定
    
    # 创建卡尔曼滤波器实例
    kf = KalmanFilter(initial_state=initial_state,
                      initial_covariance=initial_covariance,
                      transition_matrix=F,
                      control_matrix=B,
                      process_noise=Q,
                      observation_matrix=H,
                      measurement_noise=R)
    
    # 数据存储
    true_positions = []      # 真实位置
    measurements = []        # 带噪声的测量值
    estimates = []           # 卡尔曼滤波估计的位置
    velocities = []          # 卡尔曼滤波估计的速度
    
    # 真实状态初始化
    true_position = true_initial_position
    
    # 主循环
    for step in range(num_steps):
        # 1. 更新真实状态 (匀速运动)
        true_position += true_velocity * dt
        
        # 2. 生成带噪声的测量值 (真实位置 + 高斯噪声)
        measurement = true_position + np.random.normal(0, position_noise)
        
        # 3. 卡尔曼滤波预测步骤
        kf.predict()
        
        # 4. 卡尔曼滤波更新步骤 (使用新测量值)
        state_estimate, cov_estimate = kf.update(measurement)
        
        # 5. 存储数据用于绘图
        true_positions.append(true_position)
        measurements.append(measurement)
        estimates.append(state_estimate[0, 0])  # 估计的位置
        velocities.append(state_estimate[1, 0])  # 估计的速度
    
    # 设置Matplotlib支持中文显示
    plt.rcParams["font.family"] = ["SimHei", "WenQuanYi Micro Hei", "Heiti TC"]  # 支持中文的字体
    plt.rcParams["axes.unicode_minus"] = False  # 解决负号显示异常问题

    # 可视化结果
    plt.figure(figsize=(15, 10))
    
    # 位置图
    plt.subplot(2, 1, 1)
    plt.plot(true_positions, 'g-', label='真实位置', linewidth=2)
    plt.plot(measurements, 'r.', label='测量值', markersize=5)
    plt.plot(estimates, 'b-', label='卡尔曼估计', linewidth=1.5)
    plt.title('位置跟踪')
    plt.xlabel('时间步')
    plt.ylabel('位置 (米)')
    plt.legend()
    plt.grid(True)
    
    # 速度图
    plt.subplot(2, 1, 2)
    true_velocities = [true_velocity] * num_steps  # 真实速度恒定
    plt.plot(true_velocities, 'g-', label='真实速度', linewidth=2)
    plt.plot(velocities, 'b-', label='估计速度', linewidth=1.5)
    plt.title('速度估计')
    plt.xlabel('时间步')
    plt.ylabel('速度 (米/秒)')
    plt.legend()
    plt.grid(True)
    
    plt.tight_layout()
    plt.show()

if __name__ == "__main__":
    main()