【MATLAB例程】多雷达（数量自适应）跟踪三维目标，目标匀速运动，带扩展卡尔曼滤波|观测量为（斜距、俯仰角、方位角）。附代码下载链接 - 教程

多雷达观测的三维目标跟踪框架，在存在测量噪声与过程噪声的情况下，对目标的空间位置、速度以及角速度进行融合估计。程序适用于多雷达协同目标跟踪算法的研究，建模过程考虑了目标运动的扰动、观测的距离噪声和角度噪声。

文章目录

• 程序详解
• • 算法原理概述
  • 主要参数可配置项
  • 结果输出说明
  • 应用与扩展
• 运行结果
• MATLAB源代码

程序详解

程序实现基于扩展卡尔曼滤波（Extended Kalman Filter, EKF）的多雷达三维目标跟踪系统，能够在存在测量噪声与过程噪声的情况下，对目标的空间位置、速度以及角速度进行融合估计。程序结构完整，适用于多雷达协同目标跟踪算法的研究、教学及性能对比实验。

算法原理概述

系统采用混合运动模型（直线+协调转弯），通过离散时间状态方程对目标进行动态预测；多部雷达则分别给出斜距、俯仰角与方位角三类非线性观测信息。
在滤波更新阶段，程序对各雷达观测采用序贯更新策略（Sequential Update），逐个融合雷达信息，从而提升整体估计精度并避免矩阵维度膨胀。

核心估计流程包括：

1. 预测步骤：根据上一时刻状态及运动模型，预测目标的状态与协方差。
2. 观测更新：依次融合各雷达观测信息，计算卡尔曼增益并修正状态估计。
3. 结果输出与可视化：生成3D轨迹、XY投影图、误差时序曲线及RMSE统计柱状图等。

主要参数可配置项

config.T：采样周期（默认0.1 s）
config.total_time：仿真时长（默认60 s）
config.num_radars：雷达数量（默认4，可自定义）
config.sigma_range、config.sigma_elevation、config.sigma_azimuth：各类测量噪声标准差
config.sigma_acc、config.sigma_omega：过程噪声标准差（控制滤波器响应能力）
radar_positions：各雷达坐标 [x, y, z]（米）

结果输出说明

运行后应用将输出以下内容：

1. 三维轨迹图
2. 误差时间历程
3. 位置分量误差图
4. 协方差迹变化图
5. 性能统计柱状图
6. 命令行输出统计报告

应用与扩展

• 多传感器信息融合与协同定位；
• 三维目标跟踪与航迹重建；
• 扩展卡尔曼滤波（EKF）与自适应滤波算法性能对比；
• 多雷达协同感知体系仿真验证。

运行结果

轨迹对比：


误差曲线对比：

跟踪性能统计结果（命令行截图）：

缘于是可调雷达数量的，下面是2个雷达时的结果：

MATLAB源代码

完整代码如下：

% 多雷达三维目标跟踪滤波系统 - EKF实现
% 匀速直线运动，雷达数量自定义
% 输入：雷达观测数据（斜距、俯仰角、方位角）
% 输出：目标状态估计（位置、速度、转弯角速度）
% 2025-10-29/Ver1
% 作者: matlabfilter
clc; clear; close all;
rng(0);
%% == 系统参数配置 ===
% 【可修改】基本参数
config.T = 0.1;                     % 采样周期 (s) - 可修改
config.total_time = 60;             % 总仿真时间 (s) - 可修改
config.num_steps = config.total_time / config.T;
config.num_radars = 4;
% 【可修改】雷达位置配置 [x, y, z] (m) - 固定位置，需要时可修改
radar_positions = [
400,   200,    0;     % 雷达1位置
200, 300,    0;     % 雷达2位置
400, 172, 0;     % 雷达3位置
400, 100, 0];
% 根据雷达数量截取相应位置
radar_positions = radar_positions(1:config.num_radars, :);
% 【可修改】测量噪声标准差 - 根据实际雷达精度调整
config.sigma_range = 5;            % 斜距测量噪声标准差 (m)
config.sigma_elevation = 0.1;      % 俯仰角测量噪声标准差 (rad)
config.sigma_azimuth = 0.1;        % 方位角测量噪声标准差 (rad)
% 【可修改】过程噪声标准差 - 影响滤波器的跟踪能力
config.sigma_acc = 0.001;             % 线性加速度过程噪声标准差 (m/s²)
config.sigma_omega = 0.001;           % 角速度过程噪声标准差 (rad/s)

完整代码：
https://download.csdn.net/download/callmeup/92224085

或前往付费专栏查看：

https://blog.csdn.net/callmeup/article/details/154070282?spm=1011.2415.3001.5331

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