二维定位算法仿真的MATLAB代码，对比基于TOA（到达时间）、TDOA（到达时间差）以及两者混合的定位性能。通过MATLAB实现，涵盖了从数据生成、噪声建模、算法求解到结果可视化及误差分析的全过程。
原创代码，仅供学习和研究，禁止翻卖
文章目录
程序简介
代码功能概述
该程序在二维平面模拟了一个由任意数量锚节点（基站）组成的无线传感器网络。它通过模拟信号传播过程，添加高斯白噪声，生成TOA和TDOA观测数据，并分别使用三种不同的算法解算目标节点的位置，最后对定位精度进行量化对比。
核心算法模块
代码实现了三种核心定位求解器：
TOA 定位 (函数position_TOA)
- 原理：利用信号从目标到各锚点的绝对传输时间计算距离。几何上表现为多个圆的交点。
- 实现方法 ：线性最小二乘法 (Linear Least Squares)。
- 将非线性的圆方程通过减法消除二次项，转化为线性方程组 的形式。
- 直接求解矩阵逆运算，计算速度快，无需迭代。
TDOA 定位 (函数position_TDOA)
- 原理：利用信号到达不同锚点的时间差计算距离差。几何上表现为多个双曲线的交点。
- 实现方法 ：迭代最小二乘法。
- 以第一个锚点为参考基站。
- 采用两步迭代法：先进行粗略的最小二乘估计，再构建误差修正方程进行精细迭代（类似泰勒级数展开或Chan算法的迭代改进版），以提高非线性方程的求解精度。
TOA/TDOA 混合联合定位 (函数position_TOA_TDOA_Joint)
- 原理：同时利用绝对距离信息（TOA）和距离差信息（TDOA）进行数据融合。
- 实现方法 ：加权高斯-牛顿迭代法 (Weighted Gauss-Newton Iteration)。
- 构建包含TOA和TDOA两种观测模型的雅可比矩阵（Jacobian Matrix）。
- 引入加权矩阵 ，根据测量误差（
sigma_toa,sigma_tdoa）对不同数据源进行加权（误差越小，权重越大）。 - 通过迭代不断修正位置坐标，直至收敛。这是通常精度最高的方法。
运行结果
10个锚点时的运行结果：
100个锚点时的运行结果：
如需其他锚点，只需在这里修改数量：
有关于噪声的建模，噪声大小（标准差）在这里设置：
MATLAB源代码
代码结构：
完整代码如下：
matlab
% TOA与TDOA混合定位，二维平面下，任意（>3)个锚节点
% 作者:matlabfilter
% 2026-01-31/Ver1
clear;clc;close all;
rng(0);
%% 主程序
c = 3e8; %信号传输速度,即光速
point1 = [0.5,0.5]; %待求点坐标真值
n = 10; %定义锚节点数量
range_err_TOA = 1e-9; %时间计算误差
range_err_TDOA = 1e-9; %TDOA时间差计算误差
baseP = [sin(1:n)+0.01*[1:n];cos(1:n)+0.01*[1:n]]';
%% 计算真实距离，建模带噪声的时间/时间差
R_real = sqrt(diag((point1-baseP)*(point1'-baseP'))); %距离真实值
% TOA建模
TOA = R_real/c+range_err_TOA*randn(n,1); %含噪声的传播时间
R_calcu = TOA*c; %TOA计算的距离
% TDOA建模 (以第一个锚点为参考)
TDOA = (R_real-R_real(1))/c+range_err_TDOA*randn(n,1); %时间差
R_diff = TDOA*c; %距离差
%% TOA定位
[p_toa] = position_TOA(R_calcu,baseP);
%% TDOA定位
[p_tdoa] = position_TDOA(R_diff,baseP);
%% 混合定位 (TOA和TDOA联合)
[p_hybrid] = position_TOA_TDOA_Joint(R_calcu, R_diff, baseP, range_err_TOA, range_err_TDOA);
%% 绘图
% TOA定位绘图
figure;
plot(point1(1),point1(2),'ro','MarkerSize',10,'LineWidth',2);
hold on
plot(p_toa(1),p_toa(2),'b*','MarkerSize',10,'LineWidth',2);
scatter(baseP(:,1),baseP(:,2),20,'k','filled');
legend('真实位置','TOA定位','锚点');
title('TOA定位');
% TDOA定位单独绘图
figure;
plot(point1(1),point1(2),'ro','MarkerSize',10,'LineWidth',2);
hold on
plot(p_tdoa(1),p_tdoa(2),'g*','MarkerSize',10,'LineWidth',2);
scatter(baseP(:,1),baseP(:,2),20,'k','filled');
legend('真实位置','TDOA定位','锚点');
title('TDOA定位');
% 混合定位绘图与对比
figure;
plot(point1(1),point1(2),'ro','MarkerSize',10,'LineWidth',2);
hold on
plot(p_toa(1),p_toa(2),'b*','MarkerSize',8,'LineWidth',1.5);
plot(p_tdoa(1),p_tdoa(2),'g*','MarkerSize',8,'LineWidth',1.5);
plot(p_hybrid(1),p_hybrid(2),'m*','MarkerSize',10,'LineWidth',2);
scatter(baseP(:,1),baseP(:,2),20,'k','filled');
legend('真实位置','TOA定位','TDOA定位','混合定位','锚点');
title('三种方法对比');
%% 输出结果对比
fprintf('真实坐标: (%.6f, %.6f)\n',point1(1),point1(2));
% TOA定位结果
err_toa = norm(point1-p_toa);
fprintf('TOA定位结果: (%.6f, %.6f)\n',p_toa(1),p_toa(2));
fprintf('TOA定位误差: %.6f m\n',err_toa);
% TDOA定位结果
err_tdoa = norm(point1-p_tdoa);
fprintf('TDOA定位结果: (%.6f, %.6f)\n',p_tdoa(1),p_tdoa(2));
fprintf('TDOA定位误差: %.6f m\n',err_tdoa);
% 混合定位结果
err_hybrid = norm(point1-p_hybrid);
fprintf('混合定位结果: (%.6f, %.6f)\n',p_hybrid(1),p_hybrid(2));
fprintf('混合定位误差: %.6f m\n',err_hybrid);
% 性能对比
fprintf('\n性能对比:\n');
fprintf('TOA vs TDOA: ');
if err_toa < err_tdoa
fprintf('TOA更优 (误差减小 %.2f%%)\n',(err_tdoa-err_toa)/err_tdoa*100);
else
fprintf('TDOA更优 (误差减小 %.2f%%)\n',(err_toa-err_tdoa)/err_toa*100);
end
fprintf('混合 vs TOA: ');
if err_hybrid < err_toa
fprintf('混合更优 (误差减小 %.2f%%)\n',(err_toa-err_hybrid)/err_toa*100);
else
fprintf('TOA更优 (误差增加 %.2f%%)\n',(err_hybrid-err_toa)/err_toa*100);
end
fprintf('混合 vs TDOA: ');
if err_hybrid < err_tdoa
fprintf('混合更优 (误差减小 %.2f%%)\n',(err_tdoa-err_hybrid)/err_tdoa*100);
else
fprintf('TDOA更优 (误差增加 %.2f%%)\n',(err_hybrid-err_tdoa)/err_tdoa*100);
end
%% TOA定位函数
%% TDOA定位函数
%% TOA+TDOA联合优化融合定位完整代码与函数：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者