做无线定位有一阵子的人应该都有同感：真正让定位算法落地最难的不是推导公式，而是把公式批量跑成可视化的仿真结果。前阵子接了个模拟项目X的定位方案预研，需求很直接——设备上报信号到达时间，后端要在二维和三维两种场景里算出位置，并且要有一套能反复调参数、看误差的MATLAB仿真平台。我第一个想到的就是TOA定位算法（Time of Arrival，到达时间定位）。

TOA的思路比RSS和TDOA都直观：信号从目标传到基站，时间乘以光速就是距离；三个基站的测距画三个圆，交点就是二维位置；三维换成球面，四个球交出一个点。可等我真的把这一套从纸面搬到MATLAB里，从二维跑到三维，才发现“直觉友好”和“工程好做”中间隔着一大堆细节。这篇文章就把这堆细节摊开讲——TOA定位算法在二维、三维场景下的数学建模、MATLAB实现、精度分析和那些只有跑完代码才会懂的经验。

1. 从到达时间到位置：TOA定位的数学模型与仿真前设

1.1 观测模型：测距、噪声与非线性最小二乘

TOA定位的第一步是测距。假设第i个基站的坐标是(p_i=[x_i,y_i])（二维）或([x_i,y_i,z_i])（三维），目标真实位置是(p)，那么理想情况下信号从目标到基站的传播时间(t_i)乘以光速(c)，就得到目标到该基站的距离：

[ r_i = c \cdot t_i = |p - p_i| ]

这是测量值。真实接收到的TOA一定带噪声，常见的仿真模型是给距离叠加高斯噪声：

[ \hat{r}_i = |p - p_i| + n_i, \quad n_i \sim \mathcal{N}(0, \sigma_i^2) ]

有了N个基站的距离观测，定位问题就变成一个非线性最小二乘问题：

[ \hat{p} = \arg\min_p \sum_{i=1}^{N} \left(\hat{r}_i - |p - p_i|\right)^2 ]

这个式子的麻烦在于(|p - p_i|)带根号，对p是非线性的。MATLAB里可以直接用lsqnonlin硬解，但工程上更常用的是把问题线性化，走闭式解或迭代解，效率和稳定性都更好。下面我会逐步展开这两种路线。

1.2 二维和三维需要多少个基站：几何上的最低要求

先说结论：二维至少3个基站，三维至少4个基站。

二维场景里，一个基站的测距对应一个圆，两个圆有两个交点，第三个圆用来消除模糊。三个距离观测给三个方程，解两个未知数，超定1个，所以只要噪声不过分，三个基站就能定位。

三维场景里，一个基站的测距对应一个球面，三个球面相交最多得到两个点，第四个球面才能唯一确定。所以三维TOA定位最低要求是4个基站。恰好4个时方程数和未知数相等，噪声下经常出现无解或弱解，实际工程我会建议5个以上。

1.3 仿真的三个前提：坐标系、时钟、噪声类型

写仿真代码前，有几点必须先固定下来，不然后面会绕晕。

第一是坐标系。二维场景我会统一用平面直角坐标，单位米；三维场景用空间直角坐标，基站坐标和目标坐标必须在同一个坐标系下，千万别混地理坐标和直角坐标，不然距离算出来全是错的。

第二是时钟。TOA最致命的假设是目标和基站时钟严格同步，因为时间测量误差1微秒，距离误差就是300米。仿真阶段可以直接假设同步，但心里要清楚实测时会因为这个假设崩掉，后面第6章专门讲。

第三是噪声类型。高斯白噪声是标准起步配置，适合验证算法框架；真实环境里主导误差往往是NLOS（非视距）误差，误差有偏且幅度大，这个也要在后面的实验里单独加进去看。先跑通高斯模型，再谈抗NLOS。

2. 二维场景：最小二乘定位的线性化推导与MATLAB实现

2.1 为什么要线性化：非线性问题不好求

如果直接解非线性最小二乘，MATLAB的lsqnonlin能跑，但在蒙特卡洛仿真里要循环几千次，每次做迭代优化，速度和收敛性都不理想。更重要的是，非线性解的初值给不好容易掉进局部极小。把非线性方程线性化之后，一个矩阵运算就出结果，大循环里跑几千次毫无压力，还能顺便做克拉美罗界之类的理论对照。

线性化的思路是：把距离方程平方相减，消掉未知数的二次项。这个技巧也叫“线性最小二乘TOA定位”，是后面Chan算法的地基。

2.2 线性化公式推导：距离差方程

任取第1个基站作为参考基站，对第i个基站和目标位置p，理想情况下有：

[ r_i^2 = (x - x_i)^2 + (y - y_i)^2 ]

[ r_1^2 = (x - x_1)^2 + (y - y_1)^2 ]

两式相减：

[ r_i^2 - r_1^2 = (x - x_i)^2 + (y - y_i)^2 - (x - x_1)^2 - (y - y_1)^2 ]

展开后二次项(x^2, y^2)全部抵消，只剩一次项：

[ 2(x_i - x_1)x + 2(y_i - y_1)y = r_1^2 - r_i^2 + x_i^2 + y_i^2 - x_1^2 - y_1^2 ]

写成矩阵形式是：

[ A p = b ]

A的第i行是([2(x_i - x_1),; 2(y_i - y_1)])，b的第i项是(r_1^2 - r_i^2 + x_i^2 + y_i^2 - x_1^2 - y_1^2)。当基站数N大于未知数维度时，用最小二乘估计：

[ \hat{p} = (A^T A)^{-1} A^T b ]

实际代码里我不建议直接用inv(A'*A)*A'*b，数值稳定性一般。优先用pinv(A)*b，或者(A'*A)\(A'*b)。基站数越多、几何分布越好，A的条件数越健康，解越稳。

2.3 函数实现与调用：一个函数同时兼容二维三维

把上面的推导写成MATLAB函数，妙处在于二维三维可以共用一个函数，因为矩阵维度是自动适配的。

function [pos_est, residual] = toa_ls(bs_pos, bs_dist, ref_idx) % TOA线性最小二乘定位 % 输入: % bs_pos : 基站坐标矩阵, N×2 或 N×3 % bs_dist : 目标到各基站的测量距离, N×1 % ref_idx : 参考基站索引, 默认1 % 输出: % pos_est : 估计位置, 1×2 或 1×3 % residual : 线性方程残差范数 if nargin < 3 ref_idx = 1; end [N, dim] = size(bs_pos); if N < dim + 1 error('基站数量不足：二维至少3个，三维至少4个'); end A = zeros(N-1, dim); b = zeros(N-1, 1); row = 0; for k = 1:N if k == ref_idx continue; end row = row + 1; % 2*(bs_k - bs_ref) 对应线性化系数 A(row, :) = 2 * (bs_pos(k, :) - bs_pos(ref_idx, :)); % b = r_ref^2 - r_k^2 + ||bs_k||^2 - ||bs_ref||^2 b(row) = bs_dist(ref_idx)^2 - bs_dist(k)^2 ... + norm(bs_pos(k, :))^2 - norm(bs_pos(ref_idx, :))^2; end pos_est = pinv(A) * b; pos_est = pos_est(:)'; residual = norm(A * pos_est' - b); end

函数里有两行容易被新手忽略：一是ref_idx别选错，参考基站不同，A和b不同，但最终解相同；二是norm(bs_pos(k,:))^2是基站到原点的距离平方，不是基站到目标的距离，写代码时别混。

2.4 蒙特卡洛仿真：4个基站下的RMSE统计

下面给一个完整的二维仿真脚本。基站放在200米见方的正方形四角，目标固定在广场内部，测距噪声标准差0.5米，跑2000次独立实验，统计均方根误差。

%% 二维TOA定位蒙特卡洛仿真 rng(42); % 固定随机种子，结果可复现 bs = [0 0; 200 0; 200 200; 0 200]; % 4基站 true_pos = [63.7, 87.2]; sigma = 0.5; % 目标到各基站真实距离 true_dist = sqrt(sum((bs - true_pos).^2, 2)); M = 2000; errs = zeros(M, 1); for m = 1:M meas_dist = true_dist + sigma * randn(4, 1); meas_dist = max(meas_dist, 0.01); % 距离不能为负 pos_est = toa_ls(bs, meas_dist, 1); errs(m) = norm(pos_est - true_pos); end rmse = sqrt(mean(errs.^2)); fprintf('LS RMSE: %.3f m\n', rmse);

我跑下来RMSE大概在0.35米左右，和理论克拉美罗界的0.24米有明显差距。差距主要来自线性化时把距离平方差直接当作线性观测，噪声在平方项里被放大了。这时候就要上第4章的进阶算法。

3. 三维场景：基站几何、GDOP与共面陷阱

3.1 三维实现的代码变化：几乎不变，但前提变了

把上面的toa_ls函数直接传入N×3的基站坐标和N×1的距离向量，函数内部会自动适配，输出就是三维坐标。代码层面几乎不需要改动，真正变的是前提条件。

三维定位因为多了一个高度维，对基站布局极其敏感。MATLAB里的伪逆pinv在矩阵秩亏时也能给出一个最小范数解，但那个解没有物理意义。所以在三维场景里，第一步不是跑算法，而是检查基站几何是否“撑得住”三维空间。

3.2 GDOP：基站布局对精度的影响为什么这么大

GDOP（几何精度因子）是衡量基站几何分布对定位精度放大倍数的指标。令目标到第i个基站的单位方向向量为：

[ u_i = \frac{p_i - p}{|p_i - p|} ]

构造矩阵：

[ G = [u_1; u_2; \cdots; u_N] ]

在等精度测距噪声σ下，定位均方根误差约等于：

[ RMSE \approx \sigma \cdot \sqrt{\text{trace}((G^T G)^{-1})} ]

这个根号项就是GDOP。GDOP越小越好，基站的几何分布越能“包围”目标，GDOP越接近理论最小值；基站集中在某个方向上，GDOP就急剧增大。

三维场景里最典型的翻车现场是：所有基站都装在同一层楼的天花板上，z坐标都是2.8米。这时候A矩阵每一行都是基站坐标之差，而所有差向量的z分量都是0，A的秩最多只有2，高度维完全没有观测信息。目标在哪个高度，算法根本判断不出来。

3.3 实验对比：共面基站与立体基站的RMSE差距

我设计了一个20米见方的小房间场景做对比。四个基站都在z=2.8米天花板上，目标在z=1.2米；另一组加第5个基站放在地面z=0米。噪声σ=0.3米，跑2000次蒙特卡洛。

场景A的pinv还是能返回一个位置，但高度方向几乎靠“猜”，三维误差经常超过1米。场景B加了地面基站后，竖向上有了交叉观测，RMSE迅速下降。所以三维TOA仿真的第一条军规：基站必须立体布设，至少要有基站不在同一个平面上。

3.4 三维场景的工程提示：高度维的观测往往最弱

即便基站立体布设，高度维也常常是最弱的一维。原因是水平方向基站跨度大，目标被“包”在中间；竖直方向基站高度差小，方向向量里z分量小，观测强度自然弱。

如果想改善高度精度，两个方向：一是增大基站高度差，比如用无人机基站、高低层混合部署；二是在解算时给高度方向加先验约束，比如卡尔曼滤波里对z的初始化范围设紧一点。这些都是仿真平台之后要做的工程优化，但提前在MATLAB里把GDOP算一遍，能直观判断当前布站方案到底行不行。

4. 精度进阶：Chan算法、泰勒迭代与理论下界CRB

4.1 高噪声下线性LS为什么会有偏：加权的必要性

前面线性LS把(r_i^2 - r_1^2)当作线性观测，但测距噪声进入平方项后，观测噪声的均值和方差都和真实距离有关。距离越大，平方后噪声放大越多。如果能按噪声大小给每个方程加权——噪声大的方程权重小，噪声小的方程权重大——精度就能明显提升。

这就是加权最小二乘进入的动机。权重矩阵Q取测距噪声协方差，(\hat{p} = (A^T Q^{-1} A)^{-1} A^T Q^{-1} b)。但Q里面混着真实距离，工程上用测量距离近似。Chan算法的核心就是在这个基础上，再加一步用(R = x^2 + y^2)的几何约束做二次修正。

4.2 Chan算法的两步思想与MATLAB实现

二维Chan算法的第一步，把未知数从2个扩成3个：([x, y, R])，其中(R = x^2 + y^2)。对每个基站建立方程：

[ -2x_i x - 2y_i y + R = r_i^2 - (x_i^2 + y_i^2) ]

写成矩阵形式后用加权最小二乘求([x, y, R])。第二步利用(R)和(x^2 + y^2)的关系再做一次约束，得到修正后的([x, y])。这一步能显著降低高噪声下的偏差。

function pos_chan = toa_chan2d(bs, dist, sigma) % 二维Chan算法（两步加权最小二乘，工程简化版） N = size(bs, 1); x = bs(:,1); y = bs(:,2); K = x.^2 + y.^2; % 第一步：以 [x, y, R] 为未知量 Ga = [-2*x, -2*y, ones(N,1)]; ha = dist.^2 - K; % 距离平方的噪声协方差近似 Q = 4 * diag(dist.^2) * sigma^2; if N == 3 za = Ga \ ha; % 恰好3个基站时直接解 else za = (Ga' * (Q \ Ga)) \ (Ga' * (Q \ ha)); end x1 = za(1); y1 = za(2); R1 = za(3); % 第二步：用 R = x^2 + y^2 消除辅助变量 Gb = [1 0; 0 1; 1 1]; hb = [x1^2; y1^2; R1]; phi = 4 * sigma^2 * diag([abs(x1), abs(y1), abs(R1)*0.5]); zb = (Gb' * (phi \ Gb)) \ (Gb' * (phi \ hb)); % 根据第一步符号还原坐标 x_est = sign(x1) * sqrt(abs(zb(1))); y_est = sign(y1) * sqrt(abs(zb(2))); pos_chan = [x_est, y_est]; end

代码里的phi矩阵是工程近似，严格版需要用误差传播矩阵推导。仿真验证发现，噪声标准差0.5米时，Chan算法的RMSE能比线性LS低25%左右，且更接近克拉美罗界。

4.3 泰勒级数迭代：用初值换精度

泰勒级数法是对非线性距离方程直接迭代线性化。给定一个初始位置，反复计算残差和雅可比矩阵，用最小二乘求位置修正量。代码如下：

function pos_est = toa_taylor(bs_pos, bs_dist, pos_init, max_iter, tol) % 泰勒级数迭代TOA定位 pos_est = pos_init(:)'; if nargin < 4, max_iter = 20; end if nargin < 5, tol = 1e-4; end for iter = 1:max_iter d_est = zeros(size(bs_dist)); G = zeros(size(bs_pos)); for k = 1:size(bs_pos, 1) d_est(k) = norm(pos_est - bs_pos(k, :)); G(k, :) = (pos_est - bs_pos(k, :)) / d_est(k); end delta_r = bs_dist - d_est; delta_pos = pinv(G) * delta_r; pos_est = pos_est + delta_pos'; if norm(delta_pos) < tol break; end end end

泰勒法的核心问题在初值。初值离真值太远，迭代可能发散或收敛到局部极小。工程上我一般先用toa_ls跑一个粗解，再把这个粗解丢给泰勒法细化，两步叠加效果很稳。

4.4 CRB下界：你的估计器距离理论极限有多远

克拉美罗界（Cramér-Rao Bound）告诉你在给定噪声、基站布局和目标位置下，无偏估计的误差方差下界。TOA定位的费雪信息矩阵为：

[ F = \sum_{i=1}^{N} \frac{1}{\sigma_i^2} u_i u_i^T ]

(u_i)是目标指向基站的单位方向向量。位置估计的RMSE下界是：

[ RMSE_{min} = \sqrt{\text{trace}(F^{-1})} ]

这个公式和GDOP长得很像，但实际上不同：CRB还考虑了各基站的噪声方差。写个函数：

function crb = toa_crb(bs_pos, pos, sigma_vec) % TOA定位克拉美罗界，返回RMSE下界 N = size(bs_pos, 1); dim = size(bs_pos, 2); F = zeros(dim); for k = 1:N diff_vec = bs_pos(k, :) - pos; d = norm(diff_vec); u = diff_vec / d; F = F + (u' * u) / sigma_vec(k)^2; end crb = sqrt(trace(inv(F))); end

每次仿真后，把算法RMSE和CRB对比，就能定量判断当前算法还有多大提升空间。

4.5 不同噪声条件下的算法RMSE对比

我跑了一组典型的二维对比实验：4基站正方形布局，目标在(63.7, 87.2)，三种噪声水平各2000次蒙特卡洛。

高噪声下线性LS的偏差非常明显，Chan和泰勒迭代明显更接近CRB。如果要求不高，线性LS够用；如果要做高精度定位，必须上加权或迭代类算法。

5. 仿真代码调试与MATLAB工程化细节

5.1 A^T A奇异/病态：什么时候不出解，怎么处理

跑三维仿真最常见的报错是“Matrix is singular to working precision”。多数时候不是代码bug，而是基站几何布局病态。基站共线、共面、距离太近都会导致A的条件数爆炸。

排查手段很简单：算一下A的条件数。

cond(A)

条件数超过1e10就要警惕。处理方式有三种：一是改布站，这是根本解法；二是用pinv代替\，至少不会炸；三是加岭估计，给A^T A加上一个小的单位阵，用(\lambda = 1e-6)级别的正则化项。

三维场景里，我还会额外打印GDOP和CRB两个指标。如果GDOP超过10，说明布站方案在几何上就不合格，解出来误差大得离谱也正常。

5.2 单位、坐标系与随机种子：可复现仿真的底层约定

做仿真最怕“这次能跑、下次换台机器结果全变”。我自己的习惯是三条：

一是所有距离单位统一用米，所有时间换算只在进入测距模型时做一次，中间不反复乘除光速，减少数值误差。

二是固定随机种子。脚本开头写rng(42)，这样任何一次重复实验都能复现同一组噪声序列，方便对比不同算法在完全相同的噪声下的表现。这个习惯在大规模蒙特卡洛调参时特别值钱。

三是中央坐标系用直角坐标。如果测距噪声、基站坐标来自不同来源，先换算成同一个平面直角坐标系再进算法。

5.3 泰勒迭代初值怎么给：先粗后细的两种方法

泰勒迭代的初值不能拍脑袋。两个可靠来源：先用toa_ls跑闭式解，再用这个解做迭代初值；或者直接用Chan算法的解析解当初值。前者实现最简单，后者在高噪声下初值更稳。

迭代判停条件我习惯用两步：修正量范数小于1e-4米，或者迭代次数超过20次强制退出。别只设一个最大迭代数，不然有些场景已经收敛了还在空转。

5.4 可视化：把二维误差椭圆和三维误差椭球画出来

只看RMSE不够直观。把每次蒙特卡洛的估计位置收集起来，用协方差矩阵画误差椭圆，能一眼看出系统误差的方向性。

sample_pos = [x_est_series, y_est_series]; % M×2 C = cov(sample_pos); [eigvec, eigval] = eig(C); theta = linspace(0, 2*pi, 100); ell = eigvec * sqrt(eigval) * 2 * [cos(theta); sin(theta)]; plot(true_pos(1) + ell(1,:), true_pos(2) + ell(2,:), 'r-');

二维用椭圆，三维用椭球，同理。画出来后如果椭圆长轴指向某个基站方向，说明该方向的观测约束弱，布站和算法都还有优化空间。

6. 从仿真到实测：TOA定位真正难在哪

6.1 NLOS误差：模型假设的第一大裂缝

高斯噪声假设在实测里最大的漏洞是NLOS（非视距）误差。信号被墙壁、车辆、人体遮挡后，要走反射、绕射路径，测距结果系统性偏大，误差均值远大于0，而且幅度可能是高斯噪声的几十倍。

在MATLAB里模拟NLOS很直接：在测距上加一个指数分布的偏移项。补救思路分几层：对残差大的观测做拒判，给各基站按信号质量加权，或者用鲁棒估计代替最小二乘。仿真阶段一定要把NLOS加进去测一遍，否则平台无法指导真实项目。

6.2 时钟同步：TOA的“阿喀琉斯之踵”

TOA要求目标和所有基站时钟严格同步。基站之间可以靠有线同步或者授时NTP拉齐，但目标端如果有晶振偏频，等效到距离上就是几百米误差。

解决思路是把时钟偏差当成额外未知数一起解。二维加一个时钟偏差未知数，基站需求变成4个；三维加一个时钟偏差，基站需求变成5个。这与TDOA（到达时间差）定位天然只依赖基站间同步是本质区别。仿真里可以用“测距加一个公共偏差”来模拟时钟偏移，然后验证联合估计效果。

6.3 几条继续深入的路线

这套MATLAB平台跑通后，往下走有三条路值得试。

一是滤波融合。TOA定位单次解算结果不平滑，接上扩展卡尔曼滤波或者无迹卡尔曼滤波，把目标运动模型加进去，动态轨迹会比逐点解算平滑得多。

二是加权和鲁棒化。根据信号信噪比、接收质量给基站距离设置不同权重，再用残差检验剔除异常观测，这套组合拳对抗NLOS非常实用。

三是自适应布站。把GDOP公式封装成目标函数，布站方案可以自动搜索最优位置，这在无人机基站部署、应急通信场景里价值很大。

跑完整套TOA二维三维仿真后，我最大的体会是：TOA的算法本身不复杂，真正决定成败的是对基站几何布局和误差模型的理解。MATLAB平台的价值也正在这里——你可以把真实环境里的基站坐标直接导进去，先算GDOP再跑定位解算，在真实信号下来之前，平台已经把布站方案的坑提前暴露出来了。如果你也正在做类似项目，可以从最简单的toa_ls函数起步，先跑通二维，再加基站、加维度、加噪声，一步步把定位精度“逼”上去。