【MATLAB例程】三维环境，雷达跟踪目标与UKF（无迹卡尔曼滤波）的轨迹滤波，雷达观测为：斜距、方向角、俯仰角。附完整代码的下载链接

程序原创，非AI生成，包运行成功，文中附有真实的运行截图

程序讲解

代码实现了 三维空间内的目标定位与姿态估计系统，利用来自 单站雷达 的极坐标观测（距离 r、俯仰角 el、方向角 az）以及 IMU 六维测量（三轴比力 & 三轴角速度），通过 无迹卡尔曼滤波（UKF） 对目标的 位置、速度与姿态（四元数） 进行融合估计。

思路：
仿真数据构建 → IMU 推算 → 雷达量测模型 → UKF 预测与更新 → 性能对比的完整流程，可直接作为三维融合定位的基础框架。

主要功能

1. 三维目标运动仿真

• 给定真实加速度（惯性系）与角速度（体坐标系），积分得到真值轨迹。
• 姿态用单位四元数表示，避免欧拉角万向节问题。
• 输出真值位置、速度与姿态序列作为对照。

2. 六轴 IMU 测量生成（含噪声）

• 根据真实姿态，将惯性加速度转换为比力并施加噪声，模拟实际 IMU。

• 模拟噪声：

  • 加速度噪声
  • 陀螺噪声

同时代码实现了 IMU 惯导纯推算（dead-reckoning）用于对比。

3. 雷达极坐标观测模拟

雷达站固定在空间中，通过真值位置计算：

• 距离
• 方位角
• 俯仰角

并加入测量噪声。随后将量测反算回 XYZ 以用于绘图对比。

UKF 状态设计

• Sigma 点生成参数（α, β, κ）
• 过程噪声矩阵 Q，分别给位置、速度、姿态加扰动
• 测量噪声矩阵 R，对应雷达 r、el、az 的噪声

UKF 预测模型

• 利用 IMU 的比力与角速度驱动系统：

  • 通过姿态将比力转换为惯性系加速度并加入重力
  • 积分得到位置、速度
  • 用小角度四元数更新法进行姿态积分

可视化结果

代码输出以下对比图：

• 三轴位置曲线（真值 vs UKF vs IMU vs 雷达）
• 三维轨迹图
• 误差随时间变化图

并输出以下统计指标：

• IMU 推算误差
• 雷达直接观测误差
• UKF 融合误差（平均误差、RMSE）

运行结果

滤波前后的轨迹图：

滤波前后的误差图像：

命令行输出的结果：

MATLAB源代码

部分代码如下：

% 三维空间中，雷达数据与IMU的融合，雷达观测距离、俯仰角、方向角，IMU包括6维（加速度与角速度）
% 3D 雷达(r,el,az) + IMU(3 acc, 3 gyro) 融合 - UKF (位置、速度、四元数)
% 作者：matlabfilter
% 2025-11-30/Ver1

clear; close all; clc;
rng(0);

%% 仿真参数
dt = 0.1;
N = 200;
g = [0; 0; -9.81];   % 重力 (导航系：z向上为正则用 [0;0;-9.81])

%% 初始真值（目标运动与姿态）
true_p = [0; 0; 0];           % 初始位置 m
true_v = [1; 0; 0.5];        % 初始速度 m/s
% 初始姿态（单位四元数 [q0; qv]，这里设为无旋转）
true_q = [1; 0; 0; 0];

% 给定目标加速度（惯性系）与角速度（体轴）时间序列（可自行修改）
% 这里做个简单示例：恒定小加速度和缓慢偏航
acc_inertial = repmat([0.1; 0.2; 0.0], 1, N);    % 真值惯性加速度（仅用于生成真轨迹）
angvel_body = repmat([0; 0; 0.01], 1, N);      % 真值角速度（rad/s）

%% 雷达位置（固定）
radar_pos = [50; 100; 10];

%% 传感器噪声

%% 滤波


%% 函数：四元数乘法、四元数到旋转矩阵

完整代码如下：
https://download.csdn.net/download/callmeup/92419542
或：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者