UKF（无迹卡尔曼滤波器）和 KF（卡尔曼滤波器）？

  • 卡尔曼滤波器（KF） 适用于线性系统；
  • 无迹卡尔曼滤波器（UKF） 是它的非线性扩展，精度更高，且比EKF更稳健。

KF（Kalman Filter）适用于什么？

  • 系统必须是线性的

  • 系统模型可以写成：

    $$
    x_k = F x_{k-1} + w_k \
    z_k = H x_k + v_k
    $$

  • 状态转移（F）和观测（H）都是线性矩阵

  • 噪声是高斯的

KF 的优势：计算简单、推理快速
KF 的局限：无法处理非线性，比如角度、旋转、曲线运动等

UKF（Unscented Kalman Filter）是什么？

UKF 设计来处理非线性系统，不再用简单矩阵来描述状态和观测，而是使用函数形式：

$$
x_k = f(x_{k-1}) + w_k \
z_k = h(x_k) + v_k
$$

核心思想：Unscented Transform（无迹变换）
用一组“sigma点”来近似高斯分布，通过这些点来传递非线性变换的影响，比线性化方式更精确。

KF vs UKF：具体对比

项目 KF（卡尔曼滤波） UKF（无迹卡尔曼滤波）
是否能处理非线性 不能 可以
状态转移模型 线性 F 非线性 f(x)
观测模型 线性 H 非线性 h(x)
处理非线性方式 不支持 用 sigma 点传播分布
精度 高（在线性系统） 更高（在非线性系统）
稳定性 易于理解和实现 更强健，但复杂些
计算量 低 略高一些
应用场景 匀速直线、线性加速度 飞行姿态估计、目标跟踪（非直线）

一个直观的例子

假设要估计目标的位置和朝向（角度）：

  • KF 会认为朝向线性变化，但实际上角度是一个非线性变量（会 wrap-around，例如 359° → 0°）
  • UKF 会用 sigma 点来“描绘”一小群可能的位置+角度，然后对这些点做真实的非线性变换，结果比直接线性化更靠谱！

UKF 的适用场景

  • 目标非线性运动（比如目标在弯道或做曲线移动）
  • 系统模型或测量模型是非线性函数
  • IMU+GPS 融合、SLAM、雷达目标跟踪、姿态估计等

很好，你已经对卡尔曼滤波（KF）有一定了解，这会非常有助于理解无迹卡尔曼滤波（UKF）。你提到“不太懂 UKF 中是不是用了很多协方差矩阵”，我们就围绕UKF 中的协方差矩阵来解释它和 KF 的关系与不同。

UKF 中用到了哪些协方差矩阵？

实际上，UKF 使用的协方差矩阵和 KF 是一样的，仍然是这三个：

矩阵 名称 含义
P 状态协方差矩阵 当前对导弹位置/速度估计的可信度（不确定性）
Q 过程噪声协方差矩阵 描述导弹运动过程中模型误差（比如风力、系统扰动）
R 观测噪声协方差矩阵 描述雷达测量的不确定性（比如雷达噪声、精度限制）

这些矩阵的作用和卡尔曼滤波中的一致，只不过 UKF 的更新方式是通过“采样多个可能点”（Sigma 点）计算协方差，而不是用一阶线性估计（雅可比矩阵）去近似。

它们分别在 UKF 哪些步骤中用到？

步骤 用到的协方差矩阵 说明
1. 状态预测 Q 每个 Sigma 点预测之后，要加上过程噪声协方差 Q
2. 状态协方差预测 Q + 预测协方差 生成新的预测协方差矩阵 P
3. 观测预测协方差 R 将预测观测值的散布计算协方差，并加上测量噪声协方差 R
4. 更新卡尔曼增益 P, R 用于计算 Kalman 增益，融合观测
5. 状态更新 P 根据观测误差和增益调整状态与 P

一个直观的例子理解三者作用（导弹观测）

假设我们预测导弹下一步在 (x=100, y=50)，然后：

  1. Q（过程噪声）
    表示我们对导弹运动模型的不确定，比如可能有风，预测可能偏了：

    • Q 越大 → 说明你不太信模型 → 卡尔曼增益会更偏向观测值。
    • Q 越小 → 说明你非常信模型 → 更偏向预测轨迹。

  2. R（观测噪声）
    雷达测出 (102, 52)，但你知道雷达有 ±5 米误差，所以 R=25。

    • R 越大 → 说明你不信雷达 → 更信模型预测。
    • R 越小 → 说明雷达很准 → 卡尔曼增益更依赖观测值。

  3. P（状态协方差）
    你自己对预测值的置信度，也会影响 Kalman 增益 K 的计算：

    • P 很大 → 说明你对自己预测很不自信 → 更信观测。
    • P 很小 → 你信预测 → 更忽略观测。

非常好问题！我们来完整梳理 UKF 的工作流程，包括：

  1. 每一步的计算公式；
  2. P、Q、R 等协方差矩阵的作用；
  3. 如何初始化这些值（有经验法也有实践法）。

UKF 的具体计算步骤

设状态维度为 n，观测维度为 m。

0. 初始化（初值设定）

需要提供：

  • 初始状态估计向量 x₀，比如导弹初始位置和速度
    x₀ = [x, y, vx, vy]^T

  • 初始状态协方差 P₀：你对这个估计的信心，通常用对角矩阵，如：

    1
    P₀ = diag([10, 10, 5, 5])

  • 系统过程噪声协方差 Q：运动模型的误差，比如风、引擎干扰

    1
    Q = diag([0.1, 0.1, 0.05, 0.05])

  • 观测噪声协方差 R：传感器的误差，比如雷达误差 ±2 米 → 方差为 4

    1
    R = diag([4, 4])

1. 生成 Sigma 点

给定当前状态 x_k 和协方差 P_k，生成 2n + 1 个 Sigma 点。

1
2
3
X[0] = x_k
X[i]   = x_k + (sqrt((n+λ) * P))_i,   i = 1...n
X[i+n] = x_k - (sqrt((n+λ) * P))_i,   i = 1...n

λ 是一个超参数：

1
λ = α²(n + κ) - n

一般经验设置：

  • α = 1e-3（影响 Sigma 点间距）
  • κ = 0
  • β = 2（高斯分布的最优值）

2. 预测步骤

(a) 对每个 Sigma 点用系统动力学函数 f 传播

1
X⁻[i] = f(X[i])

(b) 用加权平均计算预测状态均值：

1
x⁻ = Σ Wm[i] * X⁻[i]

© 计算预测协方差：

1
P⁻ = Σ Wc[i] * (X⁻[i] - x⁻)(X⁻[i] - x⁻)^T + Q

3. 更新步骤（测量观测更新）

(a) 通过观测模型 h(·) 将预测点投影到观测空间：

1
Z[i] = h(X⁻[i])

(b) 计算预测观测均值：

1
z⁻ = Σ Wm[i] * Z[i]

© 计算观测协方差矩阵 S：

1
S = Σ Wc[i] * (Z[i] - z⁻)(Z[i] - z⁻)^T + R

(d) 计算状态与观测之间的协方差矩阵 P_xz：

1
P_xz = Σ Wc[i] * (X⁻[i] - x⁻)(Z[i] - z⁻)^T

(e) 计算 Kalman 增益：

1
K = P_xz * S⁻¹

(f) 更新状态估计和协方差：

1
2
x_k = x⁻ + K * (z_k - z⁻)
P_k = P⁻ - K * S * K^T

初始状态协方差矩阵 P₀

1. P₀ 的含义是什么？

  • P₀ 是状态估计的初始“不确定性”矩阵。

  • 它告诉滤波器“我对初始状态估计有多大的信心”。

  • P₀ 是一个方阵，大小是状态向量维度的平方。

  • 典型情况下，P₀ 是对角矩阵，表示各个状态变量之间的估计误差不相关，对角线上元素代表各状态分量的方差（不确定度大小）。

举例：状态向量是四维 [位置_x, 位置_y, 速度_x, 速度_y]，那么

$$
P_0 = \begin{bmatrix}
\sigma_{x}^2 & 0 & 0 & 0 \
0 & \sigma_{y}^2 & 0 & 0 \
0 & 0 & \sigma_{v_x}^2 & 0 \
0 & 0 & 0 & \sigma_{v_y}^2
\end{bmatrix}
$$

这里的 $\sigma_x, \sigma_y, \sigma_{v_x}, \sigma_{v_y}$ 是对应变量的标准差，平方即是方差。

2. 为什么一般用对角矩阵？

  • 因为初始状态估计的不同维度变量误差通常被假设相互独立（没有协方差）。

  • 如果你有更详细的先验知识，知道某些变量误差相关，可以把相应位置设置非零协方差。

4. 不同 P₀ 会对滤波有什么影响？

  • P₀ 太小 → 滤波器对初始估计过于自信，遇到真实状态大幅偏离时反应迟钝，收敛慢。

  • P₀ 太大 → 表示对初始估计不确定，滤波器会更快信任观测数据，收敛较快，但可能有较大抖动。