【代码介绍】二维平面上的雷达跟踪与UKF（无迹卡尔曼滤波），高精度估计目标轨迹，输出真值、估计值、误差特性等

单雷达、极坐标量测（距离 + 方位角）的二维目标跟踪器
程序原创，包运行成功

背景介绍

核心功能

本文所述程序程序模拟在二维平面内做匀加速运动的目标，并利用布置在固定位置的雷达对其进行监控。

• 观测模型：非线性。雷达直接获取的是极坐标数据( r , θ ) (r, \theta)(r,θ)，而状态估计是在笛卡尔坐标系( x , y , v x , v y ) (x, y, v_x, v_y)(x,y,vx​,vy​)下进行的。
• 核心算法：UKF。由于观测方程具有强非线性（包含平方根和反正切函数），传统的线性卡尔曼滤波（KF）无法直接使用，而 UKF 通过 UT 变换（Unscented Transform）比扩展卡尔曼滤波（EKF）能更精确地捕捉均值和方差。

算法流程与模块

A. 环境与参数初始化

• 目标运动：设定了初始位置、速度及恒定加速度。
• 噪声设置：定义了雷达的测距标准差（0.2m）和测角标准差（0.1 rad）。
• 模型构建：构建了状态转移矩阵F FF和过程噪声协方差矩阵Q QQ（基于连续白噪声加速度模型）。

B. UKF 核心步骤

1. Sigma 点生成：根据当前的估计值和协方差生成一组采样点（Sigma Points）。
2. 时间更新（预测）：将 Sigma 点通过线性状态方程预测下一时刻的位置。
3. 观测更新（校正）：
   • 将预测的 Sigma 点映射到观测空间（计算每个点对应的r rr和θ \thetaθ）。
   • 角度处理（关键点）：在计算残差和协方差时，特别使用了atan2(sin, cos)逻辑来处理角度跳变（− π -\pi−π到π \piπ之间）的问题。
   • 通过计算增益K KK更新状态估计。

C. 性能对比基准
为了证明 UKF 的有效性，代码同时计算并绘制了三条轨迹进行对比：

1. 雷达观测：直接由极坐标转换回 XY 坐标的原始含噪声数据。
2. 纯惯导（Inertial）：仅依靠初始状态进行盲推，不利用任何传感器更新（误差随时间累积）。
3. UKF 估计：结合了运动模型与观测数据的最优估计。

结果可视化与评价

代码运行结束后会生成四张图表，直观展示滤波效果：

• 轨迹对比图：展示真实路径、观测散点、纯惯导漂移路径以及 UKF 平滑后的跟踪路径。
• 误差曲线图：随时间变化的位置误差。
• 柱状图统计：对比三种方法的平均误差（Mean Error）和均方根误差（RMSE）。

亮点（创新点）

• 非线性处理：完整展示了 UKF 处理非线性观测方程的标准流程。
• 稳健性：代码中包含了对角度残差的归一化处理，防止了角度环绕导致的滤波发散。
• 量化分析：输出中明确给出了 UKF 相比于原始观测和纯惯导的相对改善百分比，方便直接评估性能。

运行结果

轨迹对比：

误差曲线：

误差柱状图对比：

命令行输出的结果：

MATLAB源代码

部分代码如下：

% 雷达测角测距定位-二维-单一雷达，轨迹用UKF，观测为距离和角度% 作者：matlabfilter（V同号），接定位与导航、滤波相关的matlab代码定制% 2025-12-07/Ver1clear;close all;clc;rng(0);%% 参数设置dt=0.01;N=400;target_position=[100;200];target_velocity=[2;5];target_acceleration=[0.5;-0.1];radar_position=[120;223];range_noise_std=0.2;%雷达测距误差angle_noise_std=0.1;%雷达测角误差%% 滤波状态模型F=[10dt0;010dt;0010;0001];q=0.1;Q=q*[dt^4/40dt^3/20;0dt^4/40dt^3/2;dt^3/20dt^20;0dt^3/20dt^2];% 测量噪声（r, theta）R=diag([range_noise_std^2,angle_noise_std^2]);% 初始状态估计% 位置RMSE柱状图figure;rmse_data=[rmse_radar,rmse_inertial,rmse_ukf];b=bar(rmse_data);b.FaceColor='flat';b.CData(1,:)=[000];% 黑色b.CData(2,:)=[101];% 洋红色b.CData(3,:)=[001];% 蓝色set(gca,'XTickLabel',{'雷达观测','纯惯导','UKF估计'});ylabel('位置RMSE (米)','FontSize',12);title('位置RMSE对比 (UKF)','FontSize',14,'FontWeight','bold');grid on;forj=1:length(rmse_data)text(j,rmse_data(j)+max(rmse_data)*0.04,sprintf('%.2f m',rmse_data(j)),...'HorizontalAlignment','center','FontSize',11,'FontWeight','bold');end

完整代码与更多公式介绍：
https://blog.csdn.net/callmeup/article/details/155665039

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者