【MATLAB例程】三维环境，基于TOA的动态轨迹定位，轨迹使用UKF（无迹卡尔曼滤波）进行滤波，模拟TOA/IMU的数据融合

本代码实现了一个基于到达时间（TOA）测距的三维定位系统，结合无迹卡尔曼滤波（UKF）对移动目标的轨迹进行优化。代码通过多锚节点（>3）的TOA测量数据，先进行初步定位解算，再通过UKF算法融合运动模型，实现对目标轨迹的高精度估计。

文章目录

代码概述
核心功能
代码结构
关键代码特点
结果示例
代码运行结果
- 使用说明
MATLAB源代码

代码概述

本代码实现了一个基于到达时间（TOA）测距的三维定位系统，结合**无迹卡尔曼滤波（UKF）**对移动目标的轨迹进行优化。代码通过多锚节点（>3）的TOA测量数据，先进行初步定位解算，再通过UKF算法融合运动模型，实现对目标轨迹的高精度估计。

核心功能

TOA定位仿真
- 在三维空间中生成9个锚节点（baseP），通过三角函数添加随机扰动，模拟实际部署环境。
- 目标沿预设轨迹（positions）运动：x轴匀速递增，y轴匀速递减，z轴保持恒定。
- 计算每个锚节点到目标的真实距离（R_real），加入高斯噪声模拟TOA测量误差，生成带噪声的观测距离（R_calcu）。
最小二乘定位解算
- 调用position_3dim函数，通过伪逆矩阵（pinv）求解超定方程组，实现目标位置的初步估计（p_out）。
UKF轨迹优化
- 构建状态空间模型：假设目标匀速运动（实际运动模型需根据场景调整）。
- 使用Sigma点法处理非线性观测（如代码中Z = [x²/20; y; z]的非线性转换），通过预测-校正步骤优化定位结果。
- 对比未滤波的纯观测解算（p_out）与UKF滤波结果，验证UKF对噪声的抑制能力。
可视化与误差分析
- 三维轨迹图显示真实路径、观测值、UKF估计值及未滤波值的对比。
- 分轴绘制绝对误差曲线，计算各时刻RMSE（均方根误差），量化定位精度。

代码结构

参数初始化
- 设置光速（c）、测距误差（range_err）、目标轨迹（positions）及锚节点坐标（baseP）。
数据生成
- 循环计算每个时刻目标的真实位置，生成含噪声的TOA测量数据。
UKF滤波
- 初始化噪声协方差矩阵（Q, R）及状态变量。
- Sigma点生成、状态预测、观测更新及协方差调整。
结果分析
- 绘制三维轨迹图及误差曲线，输出终点坐标误差及全程RMSE统计。

关键代码特点

锚节点生成 ：采用三角函数叠加随机扰动（如sin(1:n)），确保锚节点在三维空间非共面分布，避免定位歧义。
非线性观测模拟 ：在UKF观测模型中，故意将x坐标平方后除以20（Z = [x²/20; y; z]），测试滤波器处理非线性能力。

结果示例

轨迹对比：UKF估计值（蓝色点）紧密贴合真实轨迹（蓝色线），显著优于未滤波的观测值（散点）和纯惯性模拟（未滤波值）。
误差分析：UKF在各轴上的RMSE均低于直接观测，尤其在X轴（存在非线性观测）提升明显。
统计输出：终点距离误差UKF较纯观测减少约30%~50%，验证滤波有效性。

代码运行结果

定位示意图：

三轴误差图：

命令行输出截图：

RMSE曲线对比（三轴误差合并为距离误差）：

使用说明

参数调整
- 修改range_err调整测距噪声强度，Q和R影响滤波器的收敛速度与稳定性。
- 调整锚节点数量（n）及生成方式（baseP）以适应不同场景。
扩展方向
- 替换运动模型为更复杂的动力学方程（如加速度模型）。
- 改进观测模型，适配实际传感器的非线性特性。

MATLAB源代码

部分源代码如下：

matlab 复制代码

% TOA测距定位，三维任意（>3)个锚节点，对一个未知点定位、带UKF的轨迹解算
% author:Evand(VX:matlabfilter)
% 2025-03-17/Ver1
clear;clc;close all;
rng(0);
%% 主程序
c = 3e8; %信号传输速度，即光速
range_err = 1e-9; %时钟与时间计算误差
point1 = [1,2,1]; %待求点坐标真值
% 生成目标的运动
positions = repmat(point1,21,1)+[0:0.2:4;0:-0.2:-4;zeros(1,21)]';

for i1 = 1:size(positions,1)
    point1 = positions(i1,:);
    n = 9; %定义锚节点数量
    baseP = 2*[sin(1:n)+0.01*[1:n]+1;cos(4*(1:n))+0.01*[1:n]+1;cos(2*(1:n))+0.01*[1:n]+1]';
    R_real = sqrt(diag((point1-baseP)*(point1'-baseP')));
    TOA = R_real/c+range_err*randn; %含噪声的传播时间
    R_calcu = TOA*c;
    p_out(i1,:) = position_3dim(R_calcu,baseP);

end

%% UKF部分
% 滤波模型初始化
t = 1:1:size(positions,1); %设置时间序列
Q = 0.01 * diag([1, 1, 1]); % 过程噪声协方差矩阵
w = sqrt(Q) * randn(size(Q, 1), length(t)); % 过程噪声
R = 0.1 * diag([1, 1, 1]); % 观测噪声协方差矩阵
% v = sqrt(R) * randn(size(R, 1), length(t)); % 观测噪声
P0 = 0.2 * eye(3); % 初始状态协方差矩阵
X = zeros(3, length(t)); % 真实状态
X_ukf = zeros(3, length(t)); % 扩展卡尔曼滤波估计的状态
Z = zeros(3, length(t)); % 观测值形式

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

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