雷达原理-ukf 滤波器

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

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

KF（Kalman Filter）适用于什么？

• 系统必须是线性的

• 系统模型可以写成：

  $$\begin{gathered} x_k=F{x}_{k-1}+w_k \\ {z}_k=H{x}_k+v_k \end{gathered}$$

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

• 噪声是高斯的

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

UKF（Unscented Kalman Filter）是什么？

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

$$\begin{gathered} x_k=f(x_{k-1})+w_k \\ {z}_k=h(x_k)+v_k \end{gathered}$$

核心思想：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₀：你对这个估计的信心，通常用对角矩阵，如：

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

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

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

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

  R = diag([4, 4])

1. 生成 Sigma 点

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

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

λ 是一个超参数：

λ = α²(n + κ) - n

一般经验设置：

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

---

2. 预测步骤

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

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

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

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

© 计算预测协方差：

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

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

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

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

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

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

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

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

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

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

(e) 计算 Kalman 增益：

K = P_xz * S⁻¹

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

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=\left[\begin{array}{cccc}{\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{array}\right]$$

这里的 ${\sigma }_x,{\sigma }_y,{\sigma }_{v_x},{\sigma }_{v_y}$ 是对应变量的标准差，平方即是方差。

---

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

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

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

---

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

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

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