多TOA目标定位：EKF、UKF、PF与EKPF滤波算法比较

1. 项目背景与核心问题

在雷达信号处理领域，多TOA（Time of Arrival）观测移动目标定位是一个经典问题。我最近在做一个仿真项目，比较了四种不同的滤波算法在这个场景下的表现：扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、粒子滤波(PF)以及它们的混合变种EKPF。

这个问题的实际意义在于：当我们需要通过多个传感器（比如雷达站）测量目标信号到达时间来确定目标位置时，由于测量噪声、目标机动和非线性观测模型等因素，如何选择最合适的滤波算法就变得至关重要。不同的算法在计算复杂度、定位精度和鲁棒性方面各有优劣。

2. 系统建模与问题描述

2.1 运动模型

我采用了标准的匀速运动模型(CV)和协调转弯模型(CT)来模拟目标运动。状态向量x=[px,py,vx,vy]^T包含位置和速度分量。

对于CV模型：
x_k = F_cv * x_{k-1} + w_k
其中F_cv是状态转移矩阵，w_k是过程噪声。

对于机动目标，CT模型更合适：
x_k = F_ct * x_{k-1} + w_k
这里F_ct包含了转弯率参数。

2.2 观测模型

TOA测量可以表示为：
τ_i = ||s_i - p||/c + v_i
其中s_i是第i个传感器位置，p是目标位置，c是光速，v_i是测量噪声。

这个观测方程显然是非线性的，这正是我们需要非线性滤波方法的原因。

3. 滤波算法实现细节

3.1 EKF实现

EKF通过一阶泰勒展开来线性化非线性模型。在我的实现中：

1. 预测步骤：
   x_{k|k-1} = f(x_{k-1|k-1})
   P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k
   其中F_k是f的雅可比矩阵

2. 更新步骤：
   K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}
   x_{k|k} = x_{k|k-1} + K_k (z_k - h(x_{k|k-1}))
   P_{k|k} = (I - K_k H_k) P_

注意：EKF对强非线性系统效果不佳，因为一阶近似可能不准确

3.2 UKF实现

UKF使用sigma点来捕捉非线性变换的统计特性：

1. 选择2n+1个sigma点：
   χ_0 = x
   χ_i = x ± √((n+λ)P), i=1,...,2n

2. 通过非线性变换传播：
   γ_i = f(χ_i)

3. 计算预测统计量：
   x^- = Σ W_i γ_i
   P^- = Σ W_i (γ_i - x^-)(γ_i - x^-)^T + Q

UKF不需要计算雅可比矩阵，对中度非线性系统表现更好。

3.3 PF实现

粒子滤波使用蒙特卡洛方法：

1. 初始化N个粒子{x_0^i}~p(x0), i=1,...,N

2. 预测：x_k^i ~ p(x_k|x_{k-1}^i)

3. 更新权重：
   w_k^i = w_{k-1}^i p(z_k|x_k^i)
   归一化权重

4. 重采样：根据权重重新生成粒子集

PF可以处理强非线性和非高斯噪声，但计算量较大。

3.4 EKPF实现

EKPF是EKF和PF的混合方法：

1. 用EKF生成建议分布：
   q(x_k^i|x_{k-1}^i,z_k) = N(x_k^i; x_{k|k}^i, P_{k|k}^i)

2. 重要性采样：
   x_k^i ~ q(x_k^i|x_{k-1}^i,z_k)
   w_k^i = w_{k-1}^i * (p(z_k|x_k^i)p(x_k^i|x_{k-1}^i))/q(x_k^i|x_{k-1}^i,z_k)

EKPF结合了EKF的高效和PF的灵活性。

4. 仿真设置与比较指标

4.1 仿真参数

• 传感器布局：4个传感器呈正方形分布
• 目标轨迹：包含直线和转弯段
• 测量噪声：高斯白噪声，标准差0.1μs
• 过程噪声：取决于运动模型
• 粒子数：PF和EKPF使用1000个粒子
• 仿真时长：100秒

4.2 性能指标

1. 位置RMSE：
   √(1/N Σ ||p_true - p_est||^2)

2. 计算时间：单次滤波耗时

3. 一致性检验：NEES（归一化估计误差平方）

5. 结果分析与讨论

5.1 定位精度比较

在直线运动阶段：

• EKF和UKF表现相当，RMSE约15m
• PF和EKPF略优，RMSE约12m

在转弯机动阶段：

• EKF性能下降明显，RMSE增至25m
• UKF保持较好，RMSE约18m
• PF和EKPF表现最优，RMSE约15m

5.2 计算效率比较

• EKF最快，单次滤波约0.1ms
• UKF次之，约0.3ms
• EKPF约5ms
• PF最慢，约8ms

5.3 鲁棒性分析

在强非线性情况下（急转弯）：

• EKF可能发散
• UKF保持稳定但误差增大
• PF和EKPF最鲁棒

提示：实际应用中需要在精度和计算量之间权衡

6. 实际应用建议

根据我的仿真经验：

1. 对于计算资源受限且非线性不强的场景，EKF是合理选择

2. 对于中度非线性系统，UKF提供了更好的精度而计算量增加不多

3. 对于强非线性、多模态分布的情况，PF虽然计算量大但是必要的

4. EKPF是一个很好的折中方案，特别适合需要处理偶尔机动的场景

实现时的几个关键点：

• 运动模型的选择至关重要，错误的模型会导致所有滤波器失效
• 噪声统计量的准确估计对性能影响很大
• 对于PF，粒子数的选择需要平衡精度和计算量
• 重采样策略会影响粒子多样性

7. 扩展与改进方向

这个仿真可以进一步扩展：

1. 考虑更复杂的运动模型，如"当前统计"模型

2. 加入多普勒测量形成TDOA/FDOA联合定位

3. 测试不同传感器几何布局的影响

4. 尝试自适应滤波算法自动调整参数

5. 考虑分布式架构下的滤波实现

在实际工程中，我通常会先进行这样的仿真分析，然后根据具体应用场景和资源约束选择最合适的算法。有时候也会采用分层策略，比如在直线段用EKF/UKF，检测到机动时切换到PF。