【雷达跟踪滤波例程】3个雷达的三维目标跟踪滤波系统，目标匀速运动，滤波为扩展卡尔曼|雷达观测：斜距、俯仰角、方位角。MATLAB，附下载链接

基于扩展卡尔曼滤波(Extended Kalman Filter, EKF) 的多雷达三维目标跟踪系统。该系统能够融合多个雷达的观测数据，对空中目标进行高精度的位置和速度估计。系统状态线性、观测非线性

程序简介

基于扩展卡尔曼滤波(Extended Kalman Filter, EKF) 的多雷达三维目标跟踪系统。该系统能够融合多个雷达的观测数据，对空中目标进行高精度的位置和速度估计。

1. 多雷达数据融合

• 支持自定义雷达数量（默认3个）
• 每个雷达提供独立的观测数据：斜距、俯仰角、方位角

2. 运动模型

• 匀速直线运动模型
• 7维状态向量：[x, y, z, vx, vy, vz, omega_z]
  • 位置：x, y, z (米)
  • 速度：vx, vy, vz (米/秒)
  • 角速度：omega_z (弧度/秒，预留转弯机动能力)

3. 观测模型

• 雷达球坐标系测量：
  • 斜距 (range): 目标到雷达的距离
  • 俯仰角 (elevation): 垂直方向角度
  • 方位角 (azimuth): 水平方向角度
• 非线性观测方程 + 雅可比矩阵线性化

可配置参数

基本参数、雷达配置、噪声参数

核心算法流程

EKF预测步骤

1. 状态转移矩阵$F$和过程噪声$Q$计算
2. 状态预测：x_pred = F * x_est
3. 协方差预测：P_pred = F * P_est * F' + Q

EKF更新步骤（每个雷达）

1. 计算预测观测值和观测雅可比矩阵 H
2. 新息(innovation)：y = z_actual - z_pred
3. 卡尔曼增益：K = P * H' / (H * P * H' + R)
4. 状态更新：x = x + K * y
5. 协方差更新：P = (I - K * H) * P

系统生成6个分析图表：

1. 3D轨迹对比图：真实轨迹 vs EKF估计 vs 雷达观测
2. 位置误差时间历程：各雷达观测误差 + 滤波后误差
3. 各轴位置误差分量：X/Y/Z轴独立误差分析
4. XY平面投影图：俯视角度轨迹对比
5. 协方差迹变化：滤波器不确定性收敛过程
6. 性能统计柱状图：RMSE对比（各雷达 vs 滤波融合）

• 位置RMSE：均方根误差(米)
• 速度RMSE：速度估计精度(米/秒)
• 协方差迹：整体不确定性

运行结果

三维轨迹对比：

平面轨迹：

滤波后各轴误差分量：

命令行的误差分析：

MATLAB源代码

部分代码如下：

% 3雷达三维目标跟踪滤波系统 - EKF实现
% 匀速直线运动，雷达数量自定义
% 输入：雷达观测数据（斜距、俯仰角、方位角）
% 输出：目标状态估计（位置、速度、转弯角速度）
% 2025-10-29/Ver1
% 作者: matlabfilterclc; 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 = 3;
% 【可修改】雷达位置配置 [x, y, z] (m) - 固定位置，需要时可修改
radar_positions = [400,   200,    0;     % 雷达1位置200, 300,    0;     % 雷达2位置400, 172, 0];     % 雷达3位置
% 根据雷达数量截取相应位置
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)%% == 初始化EKF滤波器 ==
% 混合运动模型状态向量: [x, y, z, vx, vy, vz, omega_z]'
% x,y,z: 位置 (m), vx,vy,vz: 速度 (m/s), omega_z: 垂直轴角速度 (rad/s)% 【可修改】初始状态估计 - 根据先验信息调整
x_est = [0; 0; 350; 8; 4; -5; 0];  % 初始状态估计% 【可修改】初始协方差矩阵 - 反映初始状态的不确定性
P_est = diag([20^2, 20^2, 10^2, 15^2, 15^2, 5^2, 0.1^2]);% 状态转移矩阵和过程噪声将在每步动态计算（混合模型）

完整代码的下载链接：
https://download.csdn.net/download/callmeup/92224043

或前往专栏查看：
https://blog.csdn.net/callmeup/article/details/154068605?spm=1011.2415.3001.5331

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