本代码实现了一个基于到达时间（TOA）测距的三维定位系统，结合无迹卡尔曼滤波（UKF）对移动目标的轨迹进行优化。代码通过多锚节点（>3）的TOA测量数据，先进行初步定位解算，再通过UKF算法融合运动模型，实现对目标轨迹的高精度估计。
文章目录
- 代码概述
- 核心功能
- 代码结构
- 关键代码特点
- 结果示例
- 代码运行结果
- 使用说明
- MATLAB源代码
代码概述
本代码实现了一个基于到达时间（TOA）测距的三维定位系统，结合**无迹卡尔曼滤波（UKF）**对移动目标的轨迹进行优化。代码通过多锚节点（>3）的TOA测量数据，先进行初步定位解算，再通过UKF算法融合运动模型，实现对目标轨迹的高精度估计。
核心功能
-
TOA定位仿真
- 在三维空间中生成9个锚节点（
baseP），通过三角函数添加随机扰动，模拟实际部署环境。 - 目标沿预设轨迹（
positions）运动：x轴匀速递增，y轴匀速递减，z轴保持恒定。 - 计算每个锚节点到目标的真实距离（
R_real），加入高斯噪声模拟TOA测量误差，生成带噪声的观测距离（R_calcu）。
- 在三维空间中生成9个锚节点（
-
最小二乘定位解算
- 调用
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源代码
部分源代码如下：
% 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
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者
