例程为三维空间中，三个雷达对目标测距、测角跟踪。目标非运用上运动，使用IMU和雷达观测数据，UPF滤波得到目标的三维轨迹。有轨迹示意图、雷达坐标、误差曲线、误差散点图、位置RMSE和速度RMSE等统计特性输出。
文章目录
程序简介
代码实现了 G P S GPS GPS、惯性导航系统（ I N S INS INS）的深度集成，通过三个观测站同步采集雷达观测的斜距、方位角、俯仰角数据，构建三维空间的非线性观测约束。针对不同传感器特性动态调整观测权重。
核心算法创新
智能粒子管理
- 动态重采样：根据有效粒子数阈值触发粒子更新，防止粒子退化
- 混合引导策略：结合无迹变换生成建议分布，提升粒子采样效率
- 异常值剔除：基于多观测站数据交叉验证，过滤野值干扰
非线性优化技术
- 角度周期性补偿：自动修正方位角/俯仰角的360度跳变误差
- 观测矩阵动态线性化：采用复合函数求导策略更新雅可比矩阵
- 协方差正则化：通过特征值截断保证矩阵正定性
改进建议
自适应运动建模
- 非匀速跟踪：引入加速度补偿机制，可处理目标速度渐变与突变场景
- 机动预测：内置转弯检测模块，当监测到持续速度方向变化时自动增强过程噪声
- 三维轨迹补偿：针对高空z轴运动特性，采用分层协方差调整策略
对比优势
- 相比传统EKF：位置精度提升，抗野值能力增强3倍
- 相比标准PF：粒子数减少情况下保持等效精度
- 支持多目标跟踪：可同步处理多个独立运动体的状态估计
运行结果
轨迹图：
三轴误差曲线：
误差散点图：
命令行输出的结果：
滤波结果保存至txt文件：
MATLAB源代码
程序结构
部分代码
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; % 观测站1
1000, 0, 0; % 观测站2
500, 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; % 粒子数量完整代码
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者