【matlab例程】无迹粒子滤波（UPF）的例程，用于三维环境下多雷达目标跟踪，非线性系统

例程为三维空间中，三个雷达对目标测距、测角跟踪。目标非运用上运动，使用IMU和雷达观测数据，UPF滤波得到目标的三维轨迹。有轨迹示意图、雷达坐标、误差曲线、误差散点图、位置RMSE和速度RMSE等统计特性输出。

程序简介

代码实现了$GPS$、惯性导航系统（$INS$）的深度集成，通过三个观测站同步采集雷达观测的斜距、方位角、俯仰角数据，构建三维空间的非线性观测约束。针对不同传感器特性动态调整观测权重。

核心算法创新

智能粒子管理

• 动态重采样：根据有效粒子数阈值触发粒子更新，防止粒子退化
• 混合引导策略：结合无迹变换生成建议分布，提升粒子采样效率
• 异常值剔除：基于多观测站数据交叉验证，过滤野值干扰

非线性优化技术

• 角度周期性补偿：自动修正方位角/俯仰角的360度跳变误差
• 观测矩阵动态线性化：采用复合函数求导策略更新雅可比矩阵
• 协方差正则化：通过特征值截断保证矩阵正定性

改进建议

自适应运动建模

• 非匀速跟踪：引入加速度补偿机制，可处理目标速度渐变与突变场景
• 机动预测：内置转弯检测模块，当监测到持续速度方向变化时自动增强过程噪声
• 三维轨迹补偿：针对高空z轴运动特性，采用分层协方差调整策略

对比优势

• 相比传统EKF：位置精度提升，抗野值能力增强3倍
• 相比标准PF：粒子数减少情况下保持等效精度
• 支持多目标跟踪：可同步处理多个独立运动体的状态估计

运行结果

轨迹图：

三轴误差曲线：

误差散点图：

命令行输出的结果：
滤波结果保存至txt文件：

MATLAB源代码

程序结构

部分代码

% 无迹粒子滤波（Unscented Particle Filter）优化版
% 三维滤波，状态量为位置和速度、观测量为距离和角度（非线性）
% 作者:matlabfilter
% 2025-07-27/Ver1
%% 初始化
clc; clear; close all;
rng(0);% 时间步长和总时间
dt = 0.1;          % 时间步长 (秒)
T_total = 100;     % 模拟总时间 (秒) - 减少时间以便更好观察
N = T_total / dt;  % 总时间步数% 目标初始位置和速度（更复杂的运动轨迹）
target_pos = [500, 500, 200];  % 目标初始位置 (x, y, z)
target_vel = [2, -1, 3];      % 目标初始速度向量 (vx, vy, vz)
target_acc = [0.1, 0.05, -0.02]; % 目标加速度（非匀速运动）% 观测站位置（多个观测站）
observer_stations = [0, 0, 0;        % 观测站11000, 0, 0;     % 观测站2500, 1000, 0;   % 观测站3
];
num_stations = size(observer_stations, 1);% 初始估计状态
est_pos = target_pos + 2*randn(1,3);    % 初始估计位置（带误差）
est_speed = target_vel + 2*randn(1,3);   % 初始速度向量（带误差）% UPF参数
alpha = 1e-3;   % 控制Sigma点分布的参数
beta = 2;       % 包含高阶矩信息
kappa = 0;      % 调节协方差矩阵
lambda = 3;  % 复合参数 (状态维度为6)
num_particles = 100; % 粒子数量

完整代码

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