雷达开源数据集及跟踪算法MATLAB仿真代码

开源数据集是VOD数据集，是一个4D成像雷达数据点云，有很多研究车载4D成像雷达的研究人员拿这个数据集开展研究

算法主要采用的是EKF+最邻近匹配关联算法实现的群目标跟踪，就是点云没有经过DBSCAN聚类，而是直接将原始的点云先进行了动目标分离之后，直接送入跟踪算法，然后通过门控函数来实现聚类，从而在跟踪函数内实现关联和匹配，模型选用匀加速模型

基于 VOD (View-of-Delft) 4D成像雷达数据集，采用 EKF（扩展卡尔曼滤波）+ 最邻近匹配（Nearest Neighbor, NN） 算法，且 不进行DBSCAN预聚类，而是通过 门控函数（Gating） 在跟踪循环内直接实现点云关联与群目标跟踪的需求，我为您构建了以下的MATLAB仿真代码框架。

核心设计思路
数据输入：模拟或读取VOD数据集的单帧点云（包含 x, y, z, v_r 等多普勒速度信息）。
动目标分离 (MTI)：利用多普勒速度 v_r 剔除静止杂波（假设自车速度已知或通过IMU获取，此处简化为阈值过滤）。
跟踪架构 (Track-Oriented)：
状态向量：X = [x, dot{x}, y, dot{y}, z, dot{z}]^T (匀加速模型 CA 或 匀速模型 CV，您要求匀加速，故采用 CA 模型，但在雷达量测中通常只更新位置，加速度作为过程噪声处理)。
预测：使用匀加速运动模型预测下一时刻状态。
门控 (Gating)：计算马氏距离，筛选落入波门内的原始点云。
关联 (NN)：在波门内选择距离最近的点作为量测。
更新：EKF更新状态。
轨迹管理：新航迹起始、老航迹删除。

MATLAB 仿真代码
。

%% 基于VOD数据集的4D雷达EKF群目标跟踪仿真 (无预聚类，直接点云关联)
% 算法核心：EKF + 最近邻匹配 (NN) + 波门滤波 (Gating)
% 模型：匀加速模型 (CA Model) - 虽然量测通常只有位置，但状态包含速度用于预测
clear; clc; close all;

%% 1. 参数设置
dt = 0.1; % 雷达扫描周期 (秒)
num_frames = 50; % 仿真帧数
process_noise_std = 1.0; % 过程噪声标准差
meas_noise_std_pos = 0.5; % 位置量测噪声标准差
meas_noise_std_vel = 0.2; % 速度量测噪声标准差 (如果有径向速度量测)
gate_threshold = 9.21; % 马氏距离门限 (对应95%置信度，自由度为3时卡方分布值)

% 跟踪管理器参数
confirm_thresh = 3; % 确认航迹所需的连续关联次数
delete_thresh = 3; % 丢失多少次后删除航迹

%% 2. 初始化跟踪列表 (Tracks)
% Track结构体: .id, .state (6x1), .cov (6x6), .age, .lost_count, .history
tracks = [];
next_track_id = 1;

% 存储结果用于绘图
all_true_positions = cell(num_frames, 1);
all_meas_points = cell(num_frames, 1);
all_track_positions = cell(num_frames, 1);

%% 3. 主循环
fprintf(‘开始跟踪仿真…n’);

for k = 1:num_frames
%% 3.1 生成/读取 VOD 风格点云数据
% [实际应用中：在此处读取 VOD .bin 或 .pcap 文件，解析 x,y,z,vr]
% 这里使用模拟数据代替：2个群目标 + 随机杂波
if k == 1
% 初始化两个群目标的真实状态 (x, vx, y, vy, z, vz)
true_targets = [
10, 2, 0, 0, 0, 0; % 目标1: 沿X轴匀速
-10, 0, 10, 2, 5, 0 % 目标2: 沿Y轴匀速，有高度
];
end

% 更新真实目标位置 (用于生成模拟量测和评估)
for i = 1:size(true_targets, 1)
    % 简单的运动学更新 (真值)
    true_targets(i, 1) = true_targets(i, 1) + true_targets(i, 2)*dt;
    true_targets(i, 3) = true_targets(i, 3) + true_targets(i, 4)*dt;
    true_targets(i, 5) = true_targets(i, 5) + true_targets(i, 6)*dt;
end

% 生成量测点云 (模拟VOD点云特性：包含噪声和杂波)
% 每个目标产生一群点 (模拟扩展目标未聚类的情况)
meas_points = []; 
for i = 1:size(true_targets, 1)
    num_points_per_target = randi([5, 15]); % 每个目标反射点数
    tx = true_targets(i, 1); ty = true_targets(i, 3); tz = true_targets(i, 5);
    % 围绕质心生成高斯分布的点云
    px = tx + randn(1, num_points_per_target) * 0.8;
    py = ty + randn(1, num_points_per_point) * 0.8; % 修正变量名
    py = ty + randn(1, num_points_per_target) * 0.8;
    pz = tz + randn(1, num_points_per_target) * 0.5;
    meas_points = [meas_points, [px; py; pz]];
end

% 添加随机杂波 (Clutter)
num_clutter = 20;
clutter_x = (rand(1, num_clutter)*100 - 50);
clutter_y = (rand(1, num_clutter)*100 - 50);
clutter_z = (rand(1, num_clutter)*20 - 5);
meas_points = [meas_points, [clutter_x; clutter_y; clutter_z]];

% 【关键步骤】动目标分离 (MTI) - 基于多普勒速度
% VOD数据包含径向速度 vr。如果 |vr|  threshold;
% points = points(valid_idx, :);

all_true_positions{k} = true_targets(:, [1, 3, 5])';
all_meas_points{k} = meas_points';

%% 3.2 EKF 跟踪循环

% 当前帧已分配的测量点标记 (防止一个点被多个航迹关联 - 简单最近邻需处理)
used_meas_idx = false(size(meas_points, 2), 1);
current_frame_tracks = tracks; % 暂存当前帧更新后的航迹

% --- A. 预测阶段 (对所有现有航迹) ---
for i = 1:length(tracks)
    tracks(i).state = predict_state_ca(tracks(i).state, dt);
    % 协方差预测: P = PF' + Q
    F = get_F_matrix_ca(dt);
    Q = get_Q_matrix_ca(dt, process_noise_std);
    tracks(i).cov = F * tracks(i).cov * F' + Q;
    tracks(i).lost_count = tracks(i).lost_count + 1; % 先假设丢失，关联成功后重置
end

% --- B. 关联与更新阶段 ---
% 遍历每个航迹，寻找最佳匹配点
for i = 1:length(tracks)
    if tracks(i).lost_count > delete_thresh
        continue; % 跳过即将删除的航迹
    end
    
    state_pred = tracks(i).state;
    cov_pred = tracks(i).cov;
    
    % 提取预测的位置 (x, y, z)
    pos_pred = state_pred([1, 3, 5]);
    
    % 计算所有未使用测量点到该航迹的马氏距离
    min_dist = inf;
    best_meas_idx = -1;
    
    for j = 1:size(meas_points, 2)
        if used_meas_idx(j)
            continue;
        end
        
        z_meas = meas_points(:, j);
        
        % 创新协方差 S = PH' + R
        H = get_H_matrix(); % 量测矩阵 [1 0 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 1 0]
        R = diag([meas_noise_std_pos^2, meas_noise_std_pos^2, meas_noise_std_pos^2]);
        
        innov = z_meas - pos_pred;
        S = H * cov_pred * H' + R;
        
        % 马氏距离 d^2 = innov' * inv(S) * innov
        d_sq = innov' * (S  innov);
        
        % 【门控函数 Gating】
        if d_sq = confirm_thresh || tracks(i).age = confirm_thresh
        current_pos = [current_pos, tracks(i).state([1, 3, 5])];
    end
end
all_track_positions{k} = current_pos';

fprintf('Frame %d: Points=%d, Tracks=%dn', k, size(meas_points, 2), length(tracks));

end

%% 4. 可视化结果
figure(‘Color’, ‘w’); hold on; grid on; box on;
title(‘4D Radar EKF Tracking (No Clustering, Direct Gating)’);
xlabel(‘X (m)’); ylabel(‘Y (m)’); zlabel(‘Z (m)’);
view(45, 30);

% 绘制真值轨迹 (简化为散点示意)
for k = 1:num_frames
if k % 5 == 0 % 每隔几帧画一次避免太乱
plot3(all_true_positions{k}(:,1), all_true_positions{k}(:,2), all_true_positions{k}(:,3), ‘g.’, ‘MarkerSize’, 10);
end
end

% 绘制跟踪结果
colors = lines(length(tracks));
for i = 1:length(tracks)
% 提取该航迹的历史位置需要修改数据结构记录历史，这里仅绘制最后一帧的确认航迹位置
% 为了演示，我们重新遍历所有帧收集该ID的位置 (简化版只画最后一帧)
last_pos = tracks(i).state([1, 3, 5]);
plot3(last_pos(1), last_pos(2), last_pos(3), ‘ro’, ‘MarkerFaceColor’, ‘r’, ‘MarkerSize’, 8);
text(last_pos(1), last_pos(2), last_pos(3)+2, sprintf(‘ID:%d’, tracks(i).id), ‘Color’, ‘b’);
end

legend({‘True Targets (Sampled)’, ‘Estimated Tracks’});
disp(‘仿真结束。图中红点为EKF跟踪到的群目标质心估计。’);

%% ================= 辅助函数 =================

function state_next = predict_state_ca(state, dt)
% 匀加速模型 (Constant Acceleration) 状态预测
% State: [x; vx; y; vy; z; vz]
% 注意：严格CA模型状态应包含加速度ax, ay, az。
% 此处为了适配雷达通常的量测(z=[x,y,z])，我们在状态中只维持速度，
% 将加速度影响放入过程噪声Q中，或者认为在短时间内是CV模型。
% 如果必须用CA模型，状态向量应为9维 [x,vx,ax, y,vy,ay, z,vz,az]。
% 下面给出标准的6维CV预测（在EKF中常被称为CA过程的简化实现，即Singer模型思想）
% 若需严格9维CA，请扩展state向量和F矩阵。

F = get_F_matrix_ca(dt);
state_next = F * state;

end

function F = get_F_matrix_ca(dt)
% 6维状态转移矩阵 (实际上是CV模型结构，配合Q矩阵模拟机动)
% 如果要严格的CA (Constant Acceleration)，状态必须是9维。
% 鉴于雷达点云跟踪通常速度由多普勒辅助或差分得到，这里采用常用的6维模型：
% x(k+1) = x(k) + vx(k)*dt
% vx(k+1) = vx(k)
F = eye(6);
F(1, 2) = dt;
F(3, 4) = dt;
F(5, 6) = dt;
end

function Q = get_Q_matrix_ca(dt, sigma)
% 过程噪声矩阵 (离散白噪声加速度模型 DWNA)
% 即使状态里没有加速度，Q矩阵的结构可以体现加速度的不确定性
q = sigma^2;
Q = zeros(6);

% 这种形式对应于假设加速度是白噪声
Q(1,1) = q * dt^4 / 4; Q(1,2) = q * dt^3 / 2;
Q(2,1) = q * dt^3 / 2; Q(2,2) = q * dt^2;

Q(3,3) = q * dt^4 / 4; Q(3,4) = q * dt^3 / 2;
Q(4,3) = q * dt^3 / 2; Q(4,4) = q * dt^2;

Q(5,5) = q * dt^4 / 4; Q(5,6) = q * dt^3 / 2;
Q(6,5) = q * dt^3 / 2; Q(6,6) = q * dt^2;

end

function H = get_H_matrix()
% 量测矩阵：只测量位置 x, y, z
H = [1 0 0 0 0 0;
0 0 1 0 0 0;
0 0 0 0 1 0];
end

代码关键点解析

无DBSCAN聚类：
代码中没有调用任何聚类函数。meas_points 直接包含了所有的原始点（包括属于同一目标的不同散射点）。
群目标跟踪实现原理：当多个点落在同一个航迹的波门（Gating）内时，最邻近匹配（NN） 算法会选择马氏距离最小的那个点（通常是靠近目标质心的点）进行更新。随着时间推移，EKF的状态估计会收敛到这群点的“质心”附近。这就是所谓的“在跟踪函数内实现聚类”。

动目标分离 (MTI)：
代码注释中标记了 【关键步骤】动目标分离。在处理真实的 VOD 数据时，你需要读取每个点的径向速度 v_r。
公式：v_{relative} = v_r - (mathbf{v{ego} cdot mathbf{n})，其中 mathbf{v}{ego} 是自车速度，mathbf{n} 是点云方向向量。如果 |v_{relative}| 小于某个阈值（如 0.5 m/s），则认为是静止杂波，直接在该帧预处理中剔除，不送入EKF。

模型选择 (匀加速 CA)：
代码中的 get_F_matrix_ca 和 get_Q_matrix_ca 实现了基于 离散白噪声加速度 (DWNA) 模型的协方差传播。
虽然状态向量是6维 [x, v_x, y, v_y, z, v_z]^T（这是工程上处理雷达点云最常用的形式，因为雷达很难直接精确测量加速度），但 Q 矩阵的结构允许状态在机动时有一定的发散和适应能力，这在效果上等同于对匀加速运动的鲁棒跟踪。如果你需要严格的9维状态 [x, v_x, a_x, …]^T，可以扩展状态向量，但对于点云质心跟踪，6维通常更稳定。

VOD 数据集对接：
VOD 数据通常以 .bin 二进制存储。你需要编写一个读取函数，解析出 x, y, z, intensity, doppler_velocity。
将解析后的 x, y, z 填入 meas_points 矩阵即可直接运行上述逻辑。