第14篇：TOA 定位------圆交汇与到达时间
一、作用：最"直接"的定位方式
从第11篇到第13篇，我们走完了 AOA、TDOA、FDOA 三大无源定位体制。它们共同的特点是：不需要目标配合------无论辐射源是否知道我们在监听，都能定位。
但还有一类定位场景，目标必须主动配合：
- GPS/北斗卫星：卫星广播带时间戳的信号，接收机测量绝对到达时间；
- 雷达应答机（IFF） ：应答机在收到询问后延迟固定时间回复；
- 蜂窝网络上行定位（E-CID） ：基站测量手机信号的绝对到达时间；
- 合作目标跟踪：己方无人机、导弹上的遥测信标。
这些场景的核心都是 TOA（Time of Arrival，到达时间） ------测量信号从目标到接收机的绝对传播时间。
TOA 与 TDOA 的本质区别：
| 特性 | TOA | TDOA |
|---|---|---|
| 测量量 | 绝对到达时间 τi\tau_iτi | 到达时间差 τi−τj\tau_i - \tau_jτi−τj |
| 需要目标配合 | 是（已知发射时刻） | 否 |
| 需要接收端时钟同步 | 是 | 是 |
| 每条测量约束 | 圆 （2D）/ 球（3D） | 双曲线 （2D）/ 双曲面（3D） |
| 最少传感器数（2D） | 3（消去未知 t0t_0t0） | 3 |
| 最少传感器数（3D） | 4 | 5 |
本篇核心作用 ：从 TOA 测量的物理模型出发，推导圆交汇几何；实现三站 TOA 定位 （代数解 + 最小二乘）；对比 TOA 与 TDOA 的定位精度和适用场景；并演示未知发射时刻 情形下如何用 3 个传感器消去 t0t_0t0。
二、原理：数学上发生了什么？
2.1 TOA 测量模型
设目标位于 x\mathbf{x}x，在未知时刻 t0t_0t0 发射信号。传感器 iii 位于 xi\mathbf{x}_ixi，接收到的信号到达时刻为：
τi=t0+∥x−xi∥c+ni\tau_i = t_0 + \frac{\|\mathbf{x} - \mathbf{x}_i\|}{c} + n_iτi=t0+c∥x−xi∥+ni
其中 nin_ini 是测量噪声。乘以光速 ccc，转换为距离测量：
ri=c⋅τi=c⋅t0+∥x−xi∥+c⋅nir_i = c \cdot \tau_i = c \cdot t_0 + \|\mathbf{x} - \mathbf{x}_i\| + c \cdot n_iri=c⋅τi=c⋅t0+∥x−xi∥+c⋅ni
定义 ρ0=c⋅t0\rho_0 = c \cdot t_0ρ0=c⋅t0（未知的发射距离偏置），则：
ri=ρ0+∥x−xi∥+ϵi\boxed{ r_i = \rho_0 + \|\mathbf{x} - \mathbf{x}_i\| + \epsilon_i }ri=ρ0+∥x−xi∥+ϵi
关键观察：
- 如果 ρ0\rho_0ρ0 已知（目标主动广播发射时刻），每个 rir_iri 直接给出目标到一个传感器为圆心、ri−ρ0r_i - \rho_0ri−ρ0 为半径的圆；
- 如果 ρ0\rho_0ρ0 未知 ，每个 rir_iri 是"圆 + 偏置"，多一个未知量，需要多一个传感器。
2.2 圆交汇几何
对于已知 t0t_0t0 的情况（如 GPS 卫星广播时间戳），目标位于以传感器 iii 为圆心、Ri=ri−ρ0R_i = r_i - \rho_0Ri=ri−ρ0 为半径的圆上。两个圆相交于两点 ，三个圆通常交于一点（冗余可验证）。
但实际中更多是未知 t0t_0t0 的场景（如合作目标信标，但信标不知道发射时刻会经过多少延迟），此时方程中包含未知的 ρ0\rho_0ρ0，需要至少 N=3N = 3N=3 个传感器（2D 定位）：
{r1=ρ0+∥x−x1∥r2=ρ0+∥x−x2∥r3=ρ0+∥x−x3∥\begin{cases} r_1 = \rho_0 + \|\mathbf{x} - \mathbf{x}_1\| \\ r_2 = \rho_0 + \|\mathbf{x} - \mathbf{x}_2\| \\ r_3 = \rho_0 + \|\mathbf{x} - \mathbf{x}_3\| \end{cases}⎩ ⎨ ⎧r1=ρ0+∥x−x1∥r2=ρ0+∥x−x2∥r3=ρ0+∥x−x3∥
三个方程、三个未知量（x∈R2\mathbf{x} \in \mathbb{R}^2x∈R2 和 ρ0\rho_0ρ0），恰好可解。
2.3 消去 ρ0\rho_0ρ0------代数解
对 i=1i=1i=1 求解 ρ0=r1−∥x−x1∥\rho_0 = r_1 - \|\mathbf{x} - \mathbf{x}_1\|ρ0=r1−∥x−x1∥，代入其他两个方程：
ri−r1=∥x−xi∥−∥x−x1∥,i=2,3r_i - r_1 = \|\mathbf{x} - \mathbf{x}_i\| - \|\mathbf{x} - \mathbf{x}_1\|, \quad i = 2, 3ri−r1=∥x−xi∥−∥x−x1∥,i=2,3
这恰好是 TDOA 方程！也就是说：
未知 t0t_0t0 的 TOA 定位，等价于 TDOA 定位。
因此，第12篇的所有算法（SX 闭式解、泰勒迭代）都可以直接复用------只是测量量从"TDOA"变成了"相邻 TOA 之差"。
2.4 已知 t0t_0t0 的 TOA 定位：直接最小二乘
当 t0t_0t0 已知时，Ri=ri−ρ0R_i = r_i - \rho_0Ri=ri−ρ0 是已知量，目标是找 x\mathbf{x}x 使：
x^=argminx∑i=1N(∥x−xi∥−Ri)2\hat{\mathbf{x}} = \arg\min_{\mathbf{x}} \sum_{i=1}^{N} \left( \|\mathbf{x} - \mathbf{x}_i\| - R_i \right)^2x^=argxmini=1∑N(∥x−xi∥−Ri)2
这是非线性最小二乘 问题，没有闭式解，但可以用泰勒级数迭代：
x←x+(HTH)−1HTe\mathbf{x} \leftarrow \mathbf{x} + \left( \mathbf{H}^T \mathbf{H} \right)^{-1} \mathbf{H}^T \mathbf{e}x←x+(HTH)−1HTe
其中：
- H\mathbf{H}H 的第 iii 行是 ∂∥x−xi∥∂x=uiT\frac{\partial \|\mathbf{x} - \mathbf{x}_i\|}{\partial \mathbf{x}} = \mathbf{u}_i^T∂x∂∥x−xi∥=uiT（单位向量）；
- e\mathbf{e}e 是残差向量 ∥x−xi∥−Ri\\\|\\mathbf{x} - \\mathbf{x}_i\\\| - R_i∥x−xi∥−Ri。
2.5 CRLB
未知 t0t_0t0 情形（等价 TDOA）：
Cx=(HTDOATCr−1HTDOA)−1\mathbf{C}{\mathbf{x}} = \left( \mathbf{H}{TDOA}^T \mathbf{C}r^{-1} \mathbf{H}{TDOA} \right)^{-1}Cx=(HTDOATCr−1HTDOA)−1
HTDOA\mathbf{H}_{TDOA}HTDOA 的每行是 ui−u1\mathbf{u}_i - \mathbf{u}_1ui−u1（与第12篇一致）。
已知 t0t_0t0 情形 ：位置与 ρ0\rho_0ρ0 解耦，Fisher 信息矩阵为：
I(x)=1σr2∑i=1NuiuiT\mathbf{I}(\mathbf{x}) = \frac{1}{\sigma_r^2} \sum_{i=1}^{N} \mathbf{u}_i \mathbf{u}_i^TI(x)=σr21i=1∑NuiuiT
Cx=σr2(∑i=1NuiuiT)−1\boxed{ \mathbf{C}{\mathbf{x}} = \sigma_r^2 \left( \sum{i=1}^{N} \mathbf{u}_i \mathbf{u}_i^T \right)^{-1} }Cx=σr2(i=1∑NuiuiT)−1
关键结论：
- Cx\mathbf{C}_{\mathbf{x}}Cx 只依赖传感器到目标的单位向量 ui\mathbf{u}_iui 的分布------传感器环绕目标时精度最高；
- 与 TDOA 相比，TOA 因为多了 t0t_0t0 的信息，精度通常更高（t0t_0t0 已知时不必消去一个未知量）。
三、完整代码（可直接运行）
matlab
%% ========================================================================
% 第14篇：TOA 定位------圆交汇与到达时间（完整修复版）
% 基于：ch11_drawFigures（图11.1a）+ TOA 最小二乘
% ========================================================================
clear; close all; clc;
rng(2024);
c = 3e8; % 光速 m/s
%% ==================== 图1：TOA 圆交汇几何 ====================
fprintf('======= TOA 圆交汇演示 =======\n');
% 三个传感器
x_s1 = [0; 0];
x_s2 = [1.5; 0.7];
x_s3 = [-0.1; 0.8];
x_sensors = [x_s1, x_s2, x_s3];
% 目标
x_true = [0.5; 0.5];
% 真实距离
R_true = zeros(1, 3);
for i = 1:3
R_true(i) = norm(x_true - x_sensors(:,i));
end
fprintf('真实目标: (%.2f, %.2f)\n', x_true);
fprintf('真实距离: [%.4f, %.4f, %.4f]\n', R_true);
% 可视化
figure;
hold on;
% 绘制三个圆
theta = linspace(0, 2*pi, 500);
colors = lines(3);
for i = 1:3
plot(x_sensors(1,i) + R_true(i)*cos(theta), ...
x_sensors(2,i) + R_true(i)*sin(theta), ...
'-', 'Color', colors(i,:), 'LineWidth', 1.5, ...
'DisplayName', sprintf('R_%d = %.3f', i, R_true(i)));
end
% 绘制径向射线
for i = 1:3
plot([x_sensors(1,i), x_true(1)], ...
[x_sensors(2,i), x_true(2)], ...
'k--', 'LineWidth', 0.5, 'HandleVisibility', 'off');
mid_x = (x_sensors(1,i) + x_true(1)) / 2;
mid_y = (x_sensors(2,i) + x_true(2)) / 2;
text(mid_x + 0.05, mid_y, sprintf('R_%d', i), 'FontSize', 10);
end
plot(x_sensors(1,:), x_sensors(2,:), 'ks', 'MarkerSize', 14, ...
'MarkerFaceColor', 'k', 'DisplayName', '传感器');
plot(x_true(1), x_true(2), 'rp', 'MarkerSize', 18, ...
'MarkerFaceColor', 'r', 'DisplayName', '真实目标');
xlabel('x [km]'); ylabel('y [km]');
title('TOA 圆交汇几何（三圆交于一点）');
axis equal; grid on;
xlim([-1.5, 2.5]); ylim([-1, 2]);
legend('Location', 'northwest');
%% ==================== 图2：已知 t0 的 TOA 定位 ====================
fprintf('\n======= 已知 t0 的 TOA 定位 =======\n');
x_sensors_known = [0, 5, 2.5;
0, 0, 4]; % km
x_true_k = [2.5; 3.0];
R_true_k = zeros(1, 3);
for i = 1:3
R_true_k(i) = norm(x_true_k - x_sensors_known(:,i));
end
sigma_r = 0.03; % 30 m
R_meas_k = R_true_k + sigma_r * randn(1, 3);
x_init_k = mean(x_sensors_known, 2);
x_est_k = toa_ls_known_t0(x_sensors_known, R_meas_k, x_init_k, 50);
fprintf('真实位置: (%.4f, %.4f)\n', x_true_k);
fprintf('估计位置: (%.4f, %.4f)\n', x_est_k);
fprintf('定位误差: %.4f km\n', norm(x_est_k - x_true_k));
figure;
hold on;
theta = linspace(0, 2*pi, 500);
colors = lines(3);
for i = 1:3
plot(x_sensors_known(1,i) + R_meas_k(i)*cos(theta), ...
x_sensors_known(2,i) + R_meas_k(i)*sin(theta), ...
'-', 'Color', colors(i,:), 'LineWidth', 1.5, ...
'DisplayName', sprintf('测量圆 %d', i));
end
plot(x_sensors_known(1,:), x_sensors_known(2,:), 'ks', 'MarkerSize', 14, ...
'MarkerFaceColor', 'k', 'DisplayName', '传感器');
plot(x_true_k(1), x_true_k(2), 'rp', 'MarkerSize', 18, ...
'MarkerFaceColor', 'r', 'DisplayName', '真实目标');
plot(x_est_k(1), x_est_k(2), 'go', 'MarkerSize', 12, 'LineWidth', 2, ...
'DisplayName', 'TOA 估计');
xlabel('x [km]'); ylabel('y [km]');
title('已知 t_0 的 TOA 定位（三圆近似交于一点）');
axis equal; grid on;
xlim([-3, 8]); ylim([-3, 8]);
legend('Location', 'northwest');
%% ==================== 图3：未知 t0 的 TOA 定位 ====================
fprintf('\n======= 未知 t0 的 TOA 定位 =======\n');
x_sensors_unk = x_sensors_known;
x_true_u = x_true_k;
R_true_u = zeros(1, 3);
for i = 1:3
R_true_u(i) = norm(x_true_u - x_sensors_unk(:,i));
end
rho0_true = 1.5;
r_meas_u = rho0_true + R_true_u + sigma_r * randn(1, 3);
fprintf('真实发射偏置: %.4f km\n', rho0_true);
fprintf('TOA 测量: [%.4f, %.4f, %.4f]\n', r_meas_u);
% 方法1：转化为 TDOA
r_tdoa_from_toa = r_meas_u(2:end) - r_meas_u(1);
x_est_u1 = tdoa_sx(x_sensors_unk, r_tdoa_from_toa);
% 方法2：联合估计 (x, rho0)
x_init_u = mean(x_sensors_unk, 2);
[x_est_u2, rho0_est] = toa_ls_unknown_t0(x_sensors_unk, r_meas_u, x_init_u, 50);
fprintf('未知 t0 - SX 估计: (%.4f, %.4f)\n', x_est_u1);
fprintf('未知 t0 - 误差: %.4f km\n', norm(x_est_u1 - x_true_u));
fprintf('未知 t0 - 联合估计: (%.4f, %.4f)\n', x_est_u2);
fprintf('未知 t0 - 估计 rho0: %.4f (真实: %.4f)\n', rho0_est, rho0_true);
fprintf('未知 t0 - 误差: %.4f km\n', norm(x_est_u2 - x_true_u));
figure;
hold on;
theta = linspace(0, 2*pi, 500);
for i = 1:3
plot(x_sensors_unk(1,i) + r_meas_u(i)*cos(theta), ...
x_sensors_unk(2,i) + r_meas_u(i)*sin(theta), ...
'-', 'Color', colors(i,:), 'LineWidth', 1.5, ...
'DisplayName', sprintf('TOA 测量圆 %d', i));
end
for i = 1:3
plot(x_sensors_unk(1,i) + R_true_u(i)*cos(theta), ...
x_sensors_unk(2,i) + R_true_u(i)*sin(theta), ...
'--', 'Color', colors(i,:), 'LineWidth', 0.8, ...
'HandleVisibility', 'off');
end
plot(x_sensors_unk(1,:), x_sensors_unk(2,:), 'ks', 'MarkerSize', 14, ...
'MarkerFaceColor', 'k', 'DisplayName', '传感器');
plot(x_true_u(1), x_true_u(2), 'rp', 'MarkerSize', 18, ...
'MarkerFaceColor', 'r', 'DisplayName', '真实目标');
plot(x_est_u1(1), x_est_u1(2), 'b^', 'MarkerSize', 12, 'LineWidth', 2, ...
'DisplayName', 'SX 估计（TDOA 等效）');
plot(x_est_u2(1), x_est_u2(2), 'go', 'MarkerSize', 12, 'LineWidth', 2, ...
'DisplayName', '联合估计 (x, \rho_0)');
xlabel('x [km]'); ylabel('y [km]');
title('未知 t_0 的 TOA 定位（实线=含偏置圆，虚线=真实圆）');
axis equal; grid on;
xlim([-5, 10]); ylim([-5, 10]);
legend('Location', 'northwest');
%% ==================== 图4：TOA vs TDOA 精度对比 ====================
fprintf('\n======= TOA vs TDOA 精度对比 =======\n');
num_mc = 500;
sigma_r_vec = [0.01, 0.03, 0.1, 0.3];
rmse_toa = zeros(size(sigma_r_vec));
rmse_tdoa = zeros(size(sigma_r_vec));
crlb_toa = zeros(size(sigma_r_vec));
crlb_tdoa = zeros(size(sigma_r_vec));
for idx_s = 1:length(sigma_r_vec)
sigma_r = sigma_r_vec(idx_s);
err_toa = zeros(2, num_mc);
err_tdoa = zeros(2, num_mc);
for mc = 1:num_mc
% TOA：直接加噪
R_meas = R_true_k + sigma_r * randn(1, 3);
x_est = toa_ls_known_t0(x_sensors_known, R_meas, mean(x_sensors_known, 2), 50);
err_toa(:, mc) = x_est - x_true_k;
% TDOA：从三个带噪 TOA 相减
R_meas_t = R_true_k + sigma_r * randn(1, 3);
r_tdoa = R_meas_t(2:end) - R_meas_t(1);
x_est_t = tdoa_sx(x_sensors_known, r_tdoa);
err_tdoa(:, mc) = x_est_t - x_true_k;
end
rmse_toa(idx_s) = sqrt(mean(sum(err_toa.^2, 1)));
rmse_tdoa(idx_s) = sqrt(mean(sum(err_tdoa.^2, 1)));
% CRLB - TOA
H_toa = zeros(3, 2);
for i = 1:3
H_toa(i, :) = ((x_true_k - x_sensors_known(:,i)) / R_true_k(i))';
end
C_toa = inv(H_toa' * H_toa + 1e-12*eye(2)) * sigma_r^2;
crlb_toa(idx_s) = sqrt(real(trace(C_toa)));
% CRLB - TDOA
H_tdoa = zeros(2, 2);
for i = 2:3
u_i = (x_true_k - x_sensors_known(:,i)) / R_true_k(i);
u_1 = (x_true_k - x_sensors_known(:,1)) / R_true_k(1);
H_tdoa(i-1, :) = (u_i - u_1)';
end
C_tdoa = inv(H_tdoa' * H_tdoa + 1e-12*eye(2)) * sigma_r^2 * 2;
crlb_tdoa(idx_s) = sqrt(real(trace(C_tdoa)));
end
figure;
loglog(sigma_r_vec, rmse_toa, 'bo-', 'LineWidth', 2, 'MarkerSize', 10, ...
'DisplayName', 'TOA (已知 t_0)');
hold on;
loglog(sigma_r_vec, rmse_tdoa, 'rs-', 'LineWidth', 2, 'MarkerSize', 10, ...
'DisplayName', 'TDOA (等效)');
loglog(sigma_r_vec, crlb_toa, 'b--', 'LineWidth', 1.5, ...
'DisplayName', 'TOA CRLB');
loglog(sigma_r_vec, crlb_tdoa, 'r--', 'LineWidth', 1.5, ...
'DisplayName', 'TDOA CRLB');
xlabel('距离测量误差 \sigma_r [km]'); ylabel('定位 RMSE [km]');
title('TOA vs TDOA 定位精度对比');
grid on; legend('Location', 'northwest');
text(0.02, 0.5, 'TOA 优势: 多一维信息 (t_0 已知)', ...
'FontSize', 10, 'Color', 'b');
%% ==================== 图5：GDOP 热力图 ====================
fprintf('\n计算 TOA GDOP 热力图...\n');
xs_gdop = [0, 5, 2.5;
0, 0, 4];
x_grid = -5:0.3:10;
y_grid = -5:0.3:10;
[XX, YY] = meshgrid(x_grid, y_grid);
GDOP_toa = nan(size(XX));
GDOP_tdoa = nan(size(XX));
sigma_r_gdop = 0.03;
GDOP_max = 5; % 上限截断，避免奇异区域数值爆炸
for i = 1:size(XX,1)
for j = 1:size(XX,2)
x_t = [XX(i,j); YY(i,j)];
R_t = zeros(1, 3);
for k = 1:3
R_t(k) = norm(x_t - xs_gdop(:,k));
end
% 太靠近传感器 -> 跳过（避免除零）
if min(R_t) < 0.5
continue;
end
% ---- TOA CRLB ----
H_toa = zeros(3, 2);
for k = 1:3
H_toa(k, :) = ((x_t - xs_gdop(:,k)) / R_t(k))';
end
HtH_toa = H_toa' * H_toa;
% 加对角加载保证可逆，避免奇异警告
C_t = inv(HtH_toa + 1e-8 * eye(2)) * sigma_r_gdop^2;
g_toa = sqrt(max(real(trace(C_t)), 0));
GDOP_toa(i,j) = min(g_toa, GDOP_max);
% ---- TDOA CRLB ----
H_tdoa = zeros(2, 2);
for k = 2:3
u_k = (x_t - xs_gdop(:,k)) / R_t(k);
u_1 = (x_t - xs_gdop(:,1)) / R_t(1);
H_tdoa(k-1, :) = (u_k - u_1)';
end
HtH_tdoa = H_tdoa' * H_tdoa;
C_t2 = inv(HtH_tdoa + 1e-8 * eye(2)) * sigma_r_gdop^2 * 2;
g_tdoa = sqrt(max(real(trace(C_t2)), 0));
GDOP_tdoa(i,j) = min(g_tdoa, GDOP_max);
end
end
% 替换 NaN 为最大值（便于显示）
GDOP_toa(isnan(GDOP_toa)) = GDOP_max;
GDOP_tdoa(isnan(GDOP_tdoa)) = GDOP_max;
figure;
subplot(1, 2, 1);
imagesc(x_grid, y_grid, real(GDOP_toa));
colorbar; set(gca, 'ydir', 'normal');
hold on;
plot(xs_gdop(1,:), xs_gdop(2,:), 'ks', 'MarkerSize', 14, ...
'MarkerFaceColor', 'k', 'DisplayName', '传感器');
xlabel('x [km]'); ylabel('y [km]');
title('TOA GDOP 热力图');
axis equal; clim([0, GDOP_max]);
legend('Location', 'northwest');
subplot(1, 2, 2);
imagesc(x_grid, y_grid, real(GDOP_tdoa));
colorbar; set(gca, 'ydir', 'normal');
hold on;
plot(xs_gdop(1,:), xs_gdop(2,:), 'ks', 'MarkerSize', 14, ...
'MarkerFaceColor', 'k', 'DisplayName', '传感器');
xlabel('x [km]'); ylabel('y [km]');
title('TDOA GDOP 热力图');
axis equal; clim([0, GDOP_max]);
legend('Location', 'northwest');
sgtitle('TOA vs TDOA 的 GDOP 对比', 'FontSize', 13, 'FontWeight', 'bold');
fprintf('======= 全部完成 =======\n');
%% ==================== 函数定义 ====================
function x_est = toa_ls_known_t0(x_sensors, R_meas, x_init, max_iter)
% 已知 t0 的 TOA 最小二乘（泰勒迭代）
if nargin < 4, max_iter = 50; end
tol = 1e-8;
x_est = x_init(:);
N = size(x_sensors, 2);
for iter = 1:max_iter
R_pred = zeros(1, N);
u = zeros(2, N);
for i = 1:N
dx = x_est - x_sensors(:,i);
R_pred(i) = norm(dx);
u(:,i) = dx / R_pred(i);
end
e = R_pred(:) - R_meas(:);
H = u';
delta = (H' * H + 1e-12*eye(2)) \ (H' * e);
x_est = x_est - delta;
if norm(delta) < tol
break;
end
end
end
function [x_est, rho0_est] = toa_ls_unknown_t0(x_sensors, r_meas, x_init, max_iter)
% 未知 t0 的 TOA 联合估计 (x, rho0)
if nargin < 4, max_iter = 50; end
tol = 1e-8;
N = size(x_sensors, 2);
z = [x_init(:); 0];
for iter = 1:max_iter
x = z(1:2);
rho0 = z(3);
R_pred = zeros(1, N);
u = zeros(2, N);
for i = 1:N
dx = x - x_sensors(:,i);
R_pred(i) = norm(dx);
u(:,i) = dx / R_pred(i);
end
r_pred = rho0 + R_pred;
e = r_pred(:) - r_meas(:);
H = [u', ones(N, 1)];
delta = (H' * H + 1e-12*eye(3)) \ (H' * e);
z = z - delta;
if norm(delta) < tol
break;
end
end
x_est = z(1:2);
rho0_est = z(3);
end
function x_est = tdoa_sx(x_sensors, r_meas)
% TDOA 球面相交法
N = size(x_sensors, 2);
x1 = x_sensors(:,1);
xs = x_sensors(:,2:end) - x1;
A = xs';
b = 0.5 * (sum(xs.^2, 1)' - r_meas(:).^2);
r = r_meas(:);
A_pinv = pinv(A);
x_0 = A_pinv * b;
x_r = A_pinv * r;
a_q = sum(x_r.^2) - 1;
b_q = -2 * x_0' * x_r;
c_q = sum(x_0.^2);
R1_candidates = roots([a_q, b_q, c_q]);
R1_candidates = R1_candidates(imag(R1_candidates) == 0);
R1_candidates = R1_candidates(R1_candidates > 0);
if isempty(R1_candidates)
x_est = x_0 + x1;
return;
end
R1 = min(R1_candidates);
x_est = x_0 - R1 * x_r + x1;
end四、代码讲解：为什么这么写？
4.1 TOA 圆交汇的几何直观
图1是最基础的几何示意：三个传感器各自画一个以真实距离为半径的圆，三圆精确交于一点（无噪时）。这是 TOA 定位的几何本质。
注意 ：三个圆能确定唯一位置，是因为 2D 空间只有两个自由度（x,yx, yx,y），第三个圆起到冗余验证作用。
4.2 已知 t0t_0t0 与未知 t0t_0t0 的处理差异
代码中两个函数的核心区别：
已知 t0t_0t0 （toa_ls_known_t0）：
- 状态向量 z=x,yT\mathbf{z} = x, y^Tz=x,yT（2维）；
- 雅可比 H\mathbf{H}H 的第 iii 行是 uiT\mathbf{u}_i^TuiT（2维）；
- 直接最小二乘。
未知 t0t_0t0 （toa_ls_unknown_t0）：
- 状态向量 z=x,y,ρ0T\mathbf{z} = x, y, \\rho_0^Tz=x,y,ρ0T（3维）；
- 雅可比 H\mathbf{H}H 的第 iii 行是 uiT,1\\mathbf{u}_i\^T, 1uiT,1（3维）；
- 多了一个未知量 ρ0\rho_0ρ0，需要多一个传感器才能确定唯一解。
4.3 为什么 TDOA 是 TOA 的"消元版"？
代码中展示了两种等价路径：
路径1（等价 TDOA） ：
matlab
r_tdoa_from_toa = r_meas_u(2:end) - r_meas_u(1);
x_est_u1 = tdoa_sx(x_sensors_unk, r_tdoa_from_toa);把 TOA 测量两两相减，得到 TDOA 测量，直接复用第12篇的 SX 闭式解。
路径2（联合估计） ：
matlab
[x_est_u2, rho0_est] = toa_ls_unknown_t0(x_sensors_unk, r_meas_u, x_init_u, 50);把 ρ0\rho_0ρ0 也作为未知量一起估计。
两者数学上等价 ，但路径1更快（闭式解），路径2更灵活（可以处理非均匀 σr\sigma_rσr）。
4.4 TOA vs TDOA 的 CRLB 差异
代码中计算两个 CRLB：
matlab
C_toa = inv(H_toa' * H_toa) * sigma_r^2; % TOA
C_tdoa = inv(H_tdoa' * H_tdoa) * sigma_r^2 * 2; % TDOA (注意 sqrt(2) 因子)为什么 TDOA 有 2\sqrt{2}2 倍劣势？
TDOA 测量 τi−τj\tau_i - \tau_jτi−τj 的噪声是两个独立 TOA 噪声之差 ，方差为 2σr22\sigma_r^22σr2。所以：
- TOA 定位精度 ∝σr\propto \sigma_r∝σr；
- TDOA 定位精度 ∝2σr\propto \sqrt{2}\sigma_r∝2 σr。
结论 ：在距离测量精度相同的情况下，TOA 比 TDOA 精度高约 3 dB。这就是为什么合作目标（可以已知发射时刻）的定位精度通常远高于非合作目标。
4.5 为什么 TOA 的 GDOP 图更"平滑"？
对比图5的左右子图：
- TOA GDOP：在传感器三角形内部非常均匀；
- TDOA GDOP：三角形中心有轻微"凹"（因为 TDOA 需要消元，丢失了一部分信息）。
物理直觉 ：TOA 用 3 个独立约束（3个距离），TDOA 用 2 个独立约束（2个距离差）。多一个约束意味着对几何退化更鲁棒。
五、运行结果解读
运行代码后，你将看到 5 张图：
-
图1（圆交汇） ：三个圆（不同颜色）精确交于红五角星。这是 TOA 定位的几何精髓------圆形约束，交会即位置。
-
图2（已知 t0t_0t0 定位） ：带噪声后，三个圆不再精确交于一点，而是形成一个小三角形"不确定区"。绿色圆点是泰勒迭代找到的最优估计。
-
图3（未知 t0t_0t0 定位） ：实线 是含偏置 ρ0=1.5\rho_0 = 1.5ρ0=1.5 的 TOA 圆（半径比真实距离大 1.5 km），虚线 是真实距离圆。两种估计（蓝色三角 = SX，绿色圆 = 联合估计）重合，都正确落在真实目标附近。
-
图4（精度对比） ：对数坐标下，蓝线（TOA）始终比红线（TDOA）低约 1.4 倍（即 3 dB 优势）。随着测量精度提高，两者差距保持恒定。
-
图5（GDOP 热力图） ：TOA（左）和 TDOA（右）在传感器三角形内部都很小，但远离传感器时，TDOA 的 GDOP 上升更快------TOA 对几何退化的容忍度更高。
六、工程实战的三条准则
| 准则 | 数学依据 | 工程做法 |
|---|---|---|
| 1. TOA 需要"已知发射时刻"或"多一维未知量" | ρ0\rho_0ρ0 消元需要 N≥3N \geq 3N≥3（2D） | 合作目标用信标授时；非合作目标转 TDOA |
| 2. 时钟同步精度决定定位精度 | σx=c⋅σt\sigma_x = c \cdot \sigma_tσx=c⋅σt | 原子钟、GPS 授时，σt≤10\sigma_t \leq 10σt≤10 ns |
| 3. TOA 精度优于 TDOA 约 3 dB | σTDOA=2σTOA\sigma_{TDOA} = \sqrt{2} \sigma_{TOA}σTDOA=2 σTOA | 能用 TOA 就不用 TDOA（合作场景） |
七、延伸思考
- GPS 定位 ：本质是 3D TOA （卫星到接收机的距离 = 光速 × 传播时间）。但卫星钟与接收机钟不同步，多一个未知量 t0t_0t0，所以至少需要 4 颗卫星（3D 位置 + 1 个钟差）。
- TOA + AOA 混合：如果接收机同时测向，可以大幅减少所需传感器数（2 个 TOA + 1 个 AOA 即可）。
- 多径对 TOA 的影响 ：城市环境中的反射会导致 TOA 测量偏大（NLOS 误差），需要用 NLOS 识别与抑制算法（如残差分析、卡尔曼滤波）。
- TOA 与雷达测距 ：雷达通过测量回波延迟得到目标距离，也是 TOA 原理。区别在于雷达目标是主动反射，不需要合作。
下一篇预告（第15篇）：混合定位------TDOA/FDOA/AOA 联合
AOA 提供瞬时方向，TDOA 提供高精度位置，FDOA 提供速度约束，TOA 提供绝对距离。这四种测量模态各有优劣，能不能把它们组合起来，得到"1+1>2"的效果？
答案是肯定的------混合定位（Hybrid Geolocation） 正是当前多站无源定位研究的前沿。下篇将用原书第13章的代码，演示 TDOA + FDOA + AOA 三模态联合定位，用 CRLB 分析每种模态的贡献，并揭示"哪些测量对精度提升最有效"这一关键工程问题。敬请期待！