二维平面上，三个雷达对一个目标跟踪，输出观测平均与UKF（无迹卡尔曼滤波）两种算法的结果对比，包括轨迹图、误差曲线、误差统计特性等。
文章目录
程序简介
程序实现基于无迹卡尔曼滤波（Unscented Kalman Filter, UKF） 的多雷达二维目标跟踪系统，适用于匀速运动模型（Constant Velocity Model）。系统利用三个空间分布已知的雷达节点对目标进行距离（Range）和方位角（Bearing） 联合测量，并通过非线性滤波实现对目标位置与速度的高精度估计。
主要功能
- 目标轨迹仿真：根据匀速运动方程与过程噪声生成动态目标的真实轨迹；
- 多雷达观测模拟：为三部固定雷达生成含噪声的距离与方位角观测；
- UKF滤波估计：通过Sigma点变换实现非线性状态与观测更新，无需Jacobian矩阵；
- 数据融合：采用序贯观测更新方式融合多雷达信息，提高估计精度与稳定性；
- 结果可视化与统计分析：输出轨迹对比、误差曲线、速度估计及协方差变化等图像，并自动计算RMSE及性能提升百分比。
模型与配置
-
状态向量 ：
x = [ x , , y , , v x , , v y ] T \mathbf{x} = [x, , y, , v_x, , v_y]^T x=[x,,y,,vx,,vy]T其中 ((x, y)) 为二维位置，((v_x, v_y)) 为速度分量。
-
观测模型 （非线性）：
z = [ r θ ] z = \begin{bmatrix} r \ \theta \end{bmatrix} z=[r θ]( x − x r ) 2 + ( y − y r ) 2 a t a n 2 ( y − y r , , x − x r ) \] \\begin{bmatrix} \\sqrt{(x-x_r)\^2 + (y-y_r)\^2} \\ \\mathrm{atan2}(y-y_r,,x-x_r) \\end{bmatrix} \[(x−xr)2+(y−yr)2 atan2(y−yr,,x−xr)
其中 ((x_r, y_r)) 为雷达位置。
-
滤波结构：
- 生成Sigma点；
- 状态预测与过程噪声注入；
- 序贯融合多雷达观测；
- 状态更新与协方差修正。
结果输出
程序将自动绘制以下结果图：
- 二维轨迹对比图（真实轨迹、UKF估计、观测均值）；
- 位置误差与速度误差曲线；
- 协方差迹变化图（反映不确定性收敛趋势）；
- 统计对比条形图，展示UKF滤波与直接观测的RMSE对比及性能提升比例。
此外，命令行中输出详细的性能统计，包括：
- 位置与速度RMSE；
- 各轴误差；
- 平均/最大误差；
- 滤波前后性能改善百分比。
运行结果
轨迹对比图像：
速度与位置误差图像：
非滤波方法无法得到速度，所以速度误差仅有UKF的
误差统计特性：
MATLAB源代码
完整代码如下：
matlab
% 3雷达二维目标跟踪滤波系统 - EKF实现，匀速运动模型
% 输入：雷达观测数据（距离、方位角），输出：目标状态估计（位置、速度）
% 作者: matlabfilter
% 2025-11-09/Ver1
clc; clear; close all;
rng(0);
%% === 系统参数配置 ===
config = struct();
% 【可修改】基本参数
config.T = 0.5; % 采样周期 (s) - 可修改
config.total_time = 60; % 总仿真时间 (s) - 可修改
config.num_steps = config.total_time / config.T;
% 【可修改】雷达位置配置 [x, y] (m) - 固定位置，需要时可修改
radar_positions = [
0, 0; % 雷达1位置
2000, 0; % 雷达2位置
0,2000]; % 雷达3位置
% 【可修改】测量噪声标准差 - 根据实际雷达精度调整
config.sigma_range = 20; % 距离测量噪声标准差 (m)
config.sigma_bearing = 0.05; % 方位角测量噪声标准差 (rad) ≈ 2.86°
% 【可修改】过程噪声标准差 - 影响滤波器的跟踪能力
config.sigma_acc = 2.0; % 加速度过程噪声标准差 (m/s²)
% 【可修改】仿真参数
config.detection_prob = 0.95; % 检测概率 - 可调整丢失观测的概率
config.max_range = 15000; % 雷达最大探测距离 (m)
% 【新增】UKF参数
config.alpha = 1e-3; % Sigma点分布参数 (通常1e-4到1)
config.beta = 2; % 高斯分布最优值为2
config.kappa = 0; % 次级缩放参数 (通常为0或3-n)
%% === 初始化UKF滤波器 ===完整代码：
https://download.csdn.net/download/callmeup/92270414
或：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者