单雷达、极坐标量测（距离 + 方位角）的二维目标跟踪器
程序原创，包运行成功
文章目录
背景介绍
核心功能
本文所述程序程序模拟在二维平面内做匀加速运动的目标，并利用布置在固定位置的雷达对其进行监控。
- 观测模型 ：非线性。雷达直接获取的是极坐标数据 ( 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 F F 和过程噪声协方差矩阵 Q Q Q（基于连续白噪声加速度模型）。
B. UKF 核心步骤
- Sigma 点生成：根据当前的估计值和协方差生成一组采样点（Sigma Points）。
- 时间更新（预测）：将 Sigma 点通过线性状态方程预测下一时刻的位置。
- 观测更新（校正） ：
- 将预测的 Sigma 点映射到观测空间（计算每个点对应的 r r r 和 θ \theta θ）。
- 角度处理（关键点） ：在计算残差和协方差时，特别使用了
atan2(sin, cos)逻辑来处理角度跳变（ − π -\pi −π 到 π \pi π 之间）的问题。 - 通过计算增益 K K K 更新状态估计。
C. 性能对比基准
为了证明 UKF 的有效性，代码同时计算并绘制了三条轨迹进行对比：
- 雷达观测：直接由极坐标转换回 XY 坐标的原始含噪声数据。
- 纯惯导（Inertial）：仅依靠初始状态进行盲推，不利用任何传感器更新（误差随时间累积）。
- UKF 估计：结合了运动模型与观测数据的最优估计。
结果可视化与评价
代码运行结束后会生成四张图表，直观展示滤波效果：
- 轨迹对比图：展示真实路径、观测散点、纯惯导漂移路径以及 UKF 平滑后的跟踪路径。
- 误差曲线图：随时间变化的位置误差。
- 柱状图统计 ：对比三种方法的平均误差（Mean Error）和均方根误差（RMSE）。
亮点（创新点）
- 非线性处理：完整展示了 UKF 处理非线性观测方程的标准流程。
- 稳健性：代码中包含了对角度残差的归一化处理，防止了角度环绕导致的滤波发散。
- 量化分析 ：输出中明确给出了 UKF 相比于原始观测和纯惯导的相对改善百分比，方便直接评估性能。
运行结果
轨迹对比：
误差曲线：
误差柱状图对比：
命令行输出的结果：
MATLAB源代码
部分代码如下：
matlab
% 雷达测角测距定位-二维-单一雷达，轨迹用UKF，观测为距离和角度
% 作者：matlabfilter（V同号），接定位与导航、滤波相关的matlab代码定制
% 2025-12-07/Ver1
clear; 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 = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1];
q = 0.1;
Q = q * [dt^4/4 0 dt^3/2 0;
0 dt^4/4 0 dt^3/2;
dt^3/2 0 dt^2 0;
0 dt^3/2 0 dt^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,:) = [0 0 0]; % 黑色
b.CData(2,:) = [1 0 1]; % 洋红色
b.CData(3,:) = [0 0 1]; % 蓝色
set(gca, 'XTickLabel', {'雷达观测', '纯惯导', 'UKF估计'});
ylabel('位置RMSE (米)', 'FontSize', 12);
title('位置RMSE对比 (UKF)', 'FontSize', 14, 'FontWeight', 'bold');
grid on;
for j = 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
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者