【电子战】第30篇：3D TDOA/FDOA 定位【含matlab代码】

第30篇：3D TDOA/FDOA 定位

一、作用：从平面到立体

前28篇，我们一直在二维平面上做定位。但真实系统运行在三维空间：

• 机载 ESM：多架飞机在不同高度编队飞行，协同定位地面雷达；
• 星载定位：低轨卫星（LEO）在 500~1000 km 高度，多星协同定位地面辐射源；
• 无人机集群：多架无人机在不同高度构型，对地面目标进行无源定位。

二维算法直接用到三维会出什么问题？

问题	后果
高度不可观测	如果传感器共面，目标高度无法估计
双曲线 → 双曲面	几何从"线交会"变成"面交会"
传感器数量	最少 4 个（而不是 3 个）才能唯一确定三维位置
GDOP 更复杂	三维误差椭球，高度方向可能极度退化

本篇核心作用：把第12~15篇的 2D TDOA/FDOA 定位算法扩展到 3D；推导球面相交法（SX）的三维版本；用 MATLAB 实现 5 传感器 3D TDOA+FDOA 联合定位；分析传感器高度分布对定位精度的影响——为什么"全部传感器在同一高度"是 3D 定位的天敌。

代码出处：book2_ch8_drawFigures、+tdoa/computeCRLB、+fdoa/computeCRLB。

---

二、原理：数学上发生了什么？

2.1 3D TDOA 测量模型

$N$ 个传感器，位置 ${\mathbf{x}}_i\in {\mathbb{R}}^3$（$i=1,\dots ,N$），目标位置 $\mathbf{x}\in {\mathbb{R}}^3$。

TDOA 测量（相对传感器 1）：

$$r_{i,1}=\Vert \mathbf{x}-{\mathbf{x}}_i\Vert -\Vert \mathbf{x}-{\mathbf{x}}_1\Vert +n_i,i=2,\dots ,N$$

几何上，每个 $r_{i,1}=\text{const}$ 对应一张双曲面（3D 中的二次曲面）。

最小传感器数：

• 2D：2 个 TDOA 方程 → 3 个传感器；
• 3D：3 个 TDOA 方程 → 4 个传感器。

2.2 3D 球面相交法

延续第12篇的 SX 推导。将参考传感器移到原点：

$${\mathbf{x}}_i^{\prime }={\mathbf{x}}_i-{\mathbf{x}}_1,{\mathbf{x}}^{\prime }=\mathbf{x}-{\mathbf{x}}_1$$

令 $R_1=\Vert {\mathbf{x}}^{\prime }\Vert$，$R_i=\Vert {\mathbf{x}}^{\prime }-{\mathbf{x}}_i^{\prime }\Vert$。由 $r_{i,1}=R_i-R_1$ 得 $R_i=R_1+r_{i,1}$。

展开 $R_i^2=\Vert {\mathbf{x}}^{\prime }-{\mathbf{x}}_i^{\prime }{\Vert }^2$：

$$\Vert {\mathbf{x}}^{\prime }{\Vert }^2-2{\mathbf{x}}^{\prime T}{\mathbf{x}}_i^{\prime }+\Vert {\mathbf{x}}_i^{\prime }{\Vert }^2=R_1^2+2R_1{r}_{i,1}+r_{i,1}^2$$

利用 $\Vert {\mathbf{x}}^{\prime }{\Vert }^2=R_1^2$，整理得：

$${\mathbf{x}}^{\prime T}{\mathbf{x}}_i^{\prime }+R_1{r}_{i,1}=\frac{1}{2}\left(\Vert {\mathbf{x}}_i^{\prime }{\Vert }^2-r_{i,1}^2\right)$$

写成矩阵形式（对 $i=2,\dots ,N$）：

$$\mathbf{A}{\mathbf{x}}^{\prime }+R_1\mathbf{r}=\mathbf{b}$$

其中：
$$\mathbf{A}=\left[\begin{array}{c}{\mathbf{x}}_2^{\prime T}\\ ⋮\\ {\mathbf{x}}_N^{\prime T}\end{array}\right]\in {\mathbb{R}}^{(N-1)\times 3},\mathbf{r}=\left[\begin{array}{c}{r}_{2,1}\\ ⋮\\ r_{N,1}\end{array}\right],\mathbf{b}=\frac{1}{2}\left[\begin{array}{c}\Vert {\mathbf{x}}_2^{\prime }{\Vert }^2-r_{2,1}^2\\ ⋮\\ \Vert {\mathbf{x}}_N^{\prime }{\Vert }^2-r_{N,1}^2\end{array}\right]$$

两步法：

1. 参数化：${\mathbf{x}}^{\prime }(R_1)={\mathbf{A}}^{†}(\mathbf{b}-R_1\mathbf{r})$；
2. 代入约束 $\Vert {\mathbf{x}}^{\prime }(R_1){\Vert }^2=R_1^2$，得到 $R_1$ 的二次方程：
   $$({\mathbf{x}}_0-R_1{\mathbf{x}}_r{)}^T({\mathbf{x}}_0-R_1{\mathbf{x}}_r)=R_1^2$$
   展开：
   $$(1-\Vert {\mathbf{x}}_r{\Vert }^2)R_1^2-2{\mathbf{x}}_0^T{\mathbf{x}}_r{R}_1-\Vert {\mathbf{x}}_0{\Vert }^2=0$$

解出 $R_1$（取正实根），代回得 ${\mathbf{x}}^{\prime }$，最后加回 ${\mathbf{x}}_1$。

关键：3D 和 2D 的 SX 算法结构完全相同——只是 $\mathbf{A}$ 从 $2\times 2$ 变成 $3\times 3$，二次方程的系数稍变。

2.3 3D FDOA 测量模型

传感器速度 ${\dot{\mathbf{x}}}_i\in {\mathbb{R}}^3$，目标速度 $\dot{\mathbf{x}}\in {\mathbb{R}}^3$。

FDOA 测量（相对传感器 1）：

$$f_{i,1}=-\frac{f_0}{c}\left[{\dot{\mathbf{x}}}^T({\mathbf{u}}_i-{\mathbf{u}}_1)-{\dot{\mathbf{x}}}_i^T{\mathbf{u}}_i+{\dot{\mathbf{x}}}_1^T{\mathbf{u}}_1\right]+n_{f,i}$$

其中 ${\mathbf{u}}_i=\frac{\mathbf{x}-{\mathbf{x}}_i}{\Vert \mathbf{x}-{\mathbf{x}}_i\Vert }$ 是 3D 单位向量。

2.4 3D 联合定位

状态向量：$\mathbf{z}=[\mathbf{x};\dot{\mathbf{x}}]\in {\mathbb{R}}^6$（6 个未知量）。

测量数：

• TDOA：$N-1$ 个；
• FDOA：$N-1$ 个；
• 总共：$2(N-1)$ 个。

可解条件：$2(N-1)\ge 6$ → $N\ge 4$。

推荐 $N=5$：8 个测量 vs 6 个未知量——2 个冗余，可以做异常检测和鲁棒估计。

2.5 高度可观测性——3D 定位的关键陷阱

核心问题：如果所有传感器在同一高度（共面），高度 $z$ 不可观测！

几何直觉：

• 所有传感器在 $z=0$ 平面上；
• TDOA 只反映距离差——平面内的传感器"看不到"目标在 $z$ 方向的位置；
• 从上方看，不同高度的目标投影到同一位置，TDOA 相同。

数学根源：Fisher 信息矩阵秩亏。

工程对策：

1. 至少一个传感器在不同高度——打破共面性；
2. 高度差越大，可观测性越好——用 GDOP 定量评估；
3. 如果做不到，只能用先验高度约束（如"目标在地面上"）。

---

三、完整代码（可直接运行）

%% ========================================================================
% 第30篇：3D TDOA/FDOA 定位
% 参考出处：O'Donoughue, "Practical Geolocation for EW using MATLAB",
%           book2_ch8_drawFigures.m, +tdoa/+fdoa 模块
%           https://github.com/nodonoughue/emitter-detection-book
% ========================================================================

clear; close all; clc;
rng(2024);

c = 3e8;
f0 = 1e9;   % 载频 1 GHz

%% ==================== 参数设置 ====================

% 5 个传感器（三维构型，含高度差）
x_sensors = [0, 10, 0, 10, 5;
             0, 0, 10, 10, 5;
             0, 0, 0, 0, 8];   % km，第 5 个传感器高度 8 km

v_sensors = [0.1,  0.1, -0.1, -0.1,  0.05;
             0.05, -0.05, 0.05, -0.05, 0;
             0, 0, 0, 0, 0];    % km/s

N = size(x_sensors, 2);

% 目标真实状态
x_true = [4; 5; 3];         % km
v_true = [0.05; -0.03; 0.01]; % km/s

fprintf('======= 3D 定位参数 =======\n');
fprintf('传感器数 N = %d\n', N);
fprintf('传感器位置:\n'); disp(x_sensors);
fprintf('目标位置: (%.2f, %.2f, %.2f) km\n', x_true);
fprintf('目标速度: (%.3f, %.3f, %.3f) km/s\n', v_true);

% 噪声
sigma_r = 0.03;   % 30 m TDOA 距离差误差
sigma_f = 5;      % 5 Hz FDOA 误差

%% ==================== 真实测量生成 ====================

% 真实距离
R_true = zeros(1, N);
for i = 1:N
    R_true(i) = norm(x_true - x_sensors(:,i));
end

% 真实 TDOA
r_true = R_true(2:end) - R_true(1);

% 真实 FDOA
f_true = zeros(1, N-1);
for i = 2:N
    f_true(i-1) = fdoa_3d_measurement(x_sensors(:,i), v_sensors(:,i), ...
                                       x_sensors(:,1), v_sensors(:,1), ...
                                       x_true, v_true, f0, c);
end

fprintf('\n真实 TDOA: '); fprintf('%.4f ', r_true); fprintf('km\n');
fprintf('真实 FDOA: '); fprintf('%.2f ', f_true); fprintf('Hz\n');

%% ==================== 图1：三维传感器构型与目标 ====================

figure('Name','Fig1 3D Geometry','Position',[80 80 900 700]);

% 画地面参考网格（z=0 平面）
[Xg, Yg] = meshgrid(linspace(-3, 15, 10), linspace(-3, 15, 10));
Zg = zeros(size(Xg));
surf(Xg, Yg, Zg, 'FaceAlpha', 0.15, 'EdgeColor', 'none', ...
    'FaceColor', [0.7, 0.7, 0.7]);
hold on;

% 画传感器
plot3(x_sensors(1,:), x_sensors(2,:), x_sensors(3,:), ...
    'ks', 'MarkerSize', 14, 'MarkerFaceColor', 'k');
for i = 1:N
    text(x_sensors(1,i)+0.3, x_sensors(2,i)+0.3, x_sensors(3,i)+0.3, ...
        sprintf('S_%d', i), 'FontSize', 11);
end

% 画目标
plot3(x_true(1), x_true(2), x_true(3), 'rp', ...
    'MarkerSize', 22, 'MarkerFaceColor', 'r');
text(x_true(1)+0.3, x_true(2)+0.3, x_true(3)+0.5, 'Target', ...
    'FontSize', 12, 'Color', 'r');

% 画从目标到传感器的连线
for i = 1:N
    plot3([x_true(1), x_sensors(1,i)], ...
          [x_true(2), x_sensors(2,i)], ...
          [x_true(3), x_sensors(3,i)], 'b--', 'LineWidth', 0.8);
end

xlabel('x [km]'); ylabel('y [km]'); zlabel('z [km]');
title('三维传感器构型与目标');
grid on; view([30, 25]); axis equal;
xlim([-2, 12]); ylim([-2, 12]); zlim([-1, 10]);

%% ==================== 图2：3D 定位——最小二乘迭代 ====================

fprintf('\n======= 3D 定位：高斯-牛顿迭代 =======\n');

% 生成带噪测量
r_meas = r_true + sigma_r * randn(1, N-1);
f_meas = f_true + sigma_f * randn(1, N-1);

% 初值（偏离真值）
z_init = [x_true + 0.5*randn(3,1); zeros(3,1)];

% 高斯-牛顿迭代
[z_est, hist_err] = gn_3d_tdoa_fdoa(x_sensors, v_sensors, ...
                                     r_meas, f_meas, sigma_r, sigma_f, ...
                                     z_init, f0, c, 50);

x_est = z_est(1:3);
v_est = z_est(4:6);

fprintf('估计位置: (%.4f, %.4f, %.4f) km\n', x_est);
fprintf('估计速度: (%.4f, %.4f, %.4f) km/s\n', v_est);
fprintf('位置误差: %.4f km\n', norm(x_est - x_true));
fprintf('速度误差: %.4f km/s\n', norm(v_est - v_true));

% 迭代收敛曲线
figure('Name','Fig2 Convergence','Position',[100 100 800 500]);
semilogy(1:length(hist_err), hist_err, 'bo-', 'LineWidth', 2, ...
    'MarkerSize', 6);
xlabel('迭代次数'); ylabel('||\Delta z||');
title('高斯-牛顿迭代收敛曲线');
grid on;

%% ==================== 图3：高度可观测性分析 ====================

fprintf('\n======= 高度可观测性分析 =======\n');

% 两种传感器构型：共面 vs 立体
x_sensors_coplanar = [0, 10, 0, 10;
                      0, 0, 10, 10;
                      0, 0, 0, 0];   % 全部在 z=0 平面

x_sensors_3d = [0, 10, 0, 10;
                0, 0, 10, 10;
                0, 0, 0, 8];       % 第 4 个传感器高度 8 km

% 目标
x_tgt = [5; 5; 3];

% 计算两种构型的 CRLB
crlb_coplanar = compute_crlb_3d_tdoa(x_sensors_coplanar, x_tgt, sigma_r);
crlb_3d = compute_crlb_3d_tdoa(x_sensors_3d, x_tgt, sigma_r);

fprintf('共面构型的 CRLB 标准差: (%.4f, %.4f, %.4f) km\n', ...
    sqrt(diag(crlb_coplanar)));
fprintf('立体构型的 CRLB 标准差: (%.4f, %.4f, %.4f) km\n', ...
    sqrt(diag(crlb_3d)));

% 扫描目标高度，看 CRLB 变化
h_scan = 0:0.5:8;
crlb_h_coplanar = zeros(3, numel(h_scan));
crlb_h_3d = zeros(3, numel(h_scan));

for idx = 1:numel(h_scan)
    x_t = [5; 5; h_scan(idx)];
    C1 = compute_crlb_3d_tdoa(x_sensors_coplanar, x_t, sigma_r);
    C2 = compute_crlb_3d_tdoa(x_sensors_3d, x_t, sigma_r);
    crlb_h_coplanar(:, idx) = sqrt(diag(C1));
    crlb_h_3d(:, idx) = sqrt(diag(C2));
end

figure('Name','Fig3 Height Observability','Position',[120 120 900 600]);

subplot(1,2,1);
semilogy(h_scan, crlb_h_coplanar(3,:), 'r-', 'LineWidth', 2, ...
    'DisplayName', '共面（z 全为 0）');
hold on;
semilogy(h_scan, crlb_h_3d(3,:), 'b-', 'LineWidth', 2, ...
    'DisplayName', '立体（含高度差）');
xlabel('目标高度 [km]'); ylabel('z 方向 CRLB 标准差 [km]');
title('高度估计精度 vs 目标高度');
grid on; legend('Location', 'northwest');

subplot(1,2,2);
bar([sqrt(diag(crlb_coplanar)), sqrt(diag(crlb_3d))]);
set(gca, 'XTickLabel', {'x', 'y', 'z'});
ylabel('CRLB 标准差 [km]');
title('两种构型的 CRLB 对比');
legend({'共面', '立体'}, 'Location', 'northwest');
grid on;

sgtitle('高度可观测性：共面构型 vs 立体构型', ...
    'FontSize', 14, 'FontWeight', 'bold');

%% ==================== 图4：蒙特卡洛验证——共面 vs 立体 ====================

fprintf('\n======= 蒙特卡洛验证 =======\n');

num_mc = 500;

% 共面构型的定位
err_coplanar = zeros(3, num_mc);
err_3d = zeros(3, num_mc);

for mc = 1:num_mc
    % 共面
    r_m = r_true_coplanar(x_sensors_coplanar, x_tgt) + sigma_r*randn(1,3);
    z0 = [x_tgt + 0.3*randn(3,1); zeros(3,1)];
    [z_e, ~] = gn_3d_tdoa_only(x_sensors_coplanar, r_m, ...
                                sigma_r, z0, 30);
    err_coplanar(:, mc) = z_e(1:3) - x_tgt;

    % 立体
    r_m = r_true_coplanar(x_sensors_3d, x_tgt) + sigma_r*randn(1,3);
    z0 = [x_tgt + 0.3*randn(3,1); zeros(3,1)];
    [z_e, ~] = gn_3d_tdoa_only(x_sensors_3d, r_m, ...
                                sigma_r, z0, 30);
    err_3d(:, mc) = z_e(1:3) - x_tgt;
end

rmse_coplanar = sqrt(mean(sum(err_coplanar.^2, 1)));
rmse_3d = sqrt(mean(sum(err_3d.^2, 1)));

fprintf('共面构型位置 RMSE: %.4f km\n', rmse_coplanar);
fprintf('立体构型位置 RMSE: %.4f km\n', rmse_3d);

figure('Name','Fig4 Monte Carlo','Position',[140 140 900 600]);

subplot(1,2,1);
histogram(err_coplanar(3,:), 30, 'FaceColor', 'r', 'FaceAlpha', 0.6);
hold on;
histogram(err_3d(3,:), 30, 'FaceColor', 'b', 'FaceAlpha', 0.6);
xlabel('z 方向误差 [km]'); ylabel('计数');
title('高度方向误差分布');
legend('共面构型', '立体构型', 'Location', 'northeast');
grid on;

subplot(1,2,2);
bar([rmse_coplanar, rmse_3d]);
set(gca, 'XTickLabel', {'共面', '立体'});
ylabel('位置 RMSE [km]');
title('整体定位精度对比');
grid on;
for i = 1:2
    vals = [rmse_coplanar, rmse_3d];
    text(i, vals(i)+0.01, sprintf('%.4f', vals(i)), ...
        'HorizontalAlignment', 'center', 'FontSize', 12);
end

sgtitle('蒙特卡洛验证：立体构型优势', ...
    'FontSize', 14, 'FontWeight', 'bold');

%% ==================== 图5：3D GDOP 热力图 ====================

fprintf('\n======= 3D GDOP 热力图 =======\n');

% 在 z = 3 km 平面上扫描目标位置
x_grid = 0:0.5:10;
y_grid = 0:0.5:10;
[XX, YY] = meshgrid(x_grid, y_grid);
GDOP_map = nan(size(XX));

for i = 1:size(XX, 1)
    for j = 1:size(XX, 2)
        x_t = [XX(i,j); YY(i,j); 3];
        R_t = zeros(1, N);
        for k = 1:N
            R_t(k) = norm(x_t - x_sensors(:,k));
        end
        if min(R_t) < 0.5
            continue;
        end
        try
            C = compute_crlb_3d_tdoa(x_sensors, x_t, sigma_r);
            GDOP_map(i,j) = sqrt(trace(C)) / sigma_r;
        catch
            GDOP_map(i,j) = NaN;
        end
    end
end

GDOP_map(isnan(GDOP_map)) = 10;

figure('Name','Fig5 3D GDOP','Position',[160 160 900 700]);
imagesc(x_grid, y_grid, GDOP_map);
set(gca, 'YDir', 'normal');
colorbar; clim([0, 10]);
hold on;
% 画传感器（投影到 z=3 平面）
plot(x_sensors(1,:), x_sensors(2,:), 'ks', 'MarkerSize', 16, ...
    'MarkerFaceColor', 'k');
xlabel('x [km]'); ylabel('y [km]');
title('3D 定位 GDOP 热力图（z = 3 km 平面）');
axis equal; grid on;

fprintf('GDOP 最小值: %.4f\n', min(GDOP_map(:)));
fprintf('GDOP 最大值: %.4f\n', max(GDOP_map(:)));

fprintf('\n======= 全部完成 =======\n');

%% ==================== 函数定义 ====================

function f_diff = fdoa_3d_measurement(x1, v1, x2, v2, x_t, v_t, f0, c)
    % 3D FDOA 计算（km, km/s 制）
    x1_m = x1 * 1e3; x2_m = x2 * 1e3; x_t_m = x_t * 1e3;
    v1_m = v1 * 1e3; v2_m = v2 * 1e3; v_t_m = v_t * 1e3;

    u1 = (x_t_m - x1_m) / norm(x_t_m - x1_m);
    u2 = (x_t_m - x2_m) / norm(x_t_m - x2_m);

    vr1 = (v_t_m - v1_m)' * u1;
    vr2 = (v_t_m - v2_m)' * u2;

    f1 = f0 * (1 - vr1 / c);
    f2 = f0 * (1 - vr2 / c);
    f_diff = f1 - f2;
end

function r = r_true_coplanar(x_sensors, x_tgt)
    % 计算真实 TDOA（相对传感器 1）
    N = size(x_sensors, 2);
    R = zeros(1, N);
    for i = 1:N
        R(i) = norm(x_tgt - x_sensors(:,i));
    end
    r = R(2:end) - R(1);
end

function [z_est, hist] = gn_3d_tdoa_fdoa(x_sensors, v_sensors, ...
    r_meas, f_meas, sigma_r, sigma_f, z_init, f0, c, max_iter)
    % 3D TDOA+FDOA 高斯-牛顿迭代
    % z = [x; y; z; vx; vy; vz]

    z_est = z_init(:);
    hist = zeros(max_iter, 1);
    tol = 1e-10;
    N = size(x_sensors, 2);

    for iter = 1:max_iter
        x = z_est(1:3);
        v = z_est(4:6);

        % 距离和单位向量
        R = zeros(1, N);
        u = zeros(3, N);
        for i = 1:N
            dx = x - x_sensors(:,i);
            R(i) = norm(dx);
            u(:,i) = dx / R(i);
        end

        % 预测 TDOA
        r_pred = R(2:end) - R(1);

        % 预测 FDOA
        f_pred = zeros(1, N-1);
        for i = 2:N
            f_pred(i-1) = fdoa_3d_measurement(...
                x_sensors(:,i), v_sensors(:,i), ...
                x_sensors(:,1), v_sensors(:,1), x, v, f0, c);
        end

        % 残差
        e = [r_meas(:) - r_pred(:); f_meas(:) - f_pred(:)];

        % 雅可比矩阵
        H_r = zeros(N-1, 6);
        for i = 2:N
            H_r(i-1, 1:3) = (u(:,i) - u(:,1))';
            H_r(i-1, 4:6) = 0;
        end

        % FDOA 雅可比（数值差分）
        H_f = zeros(N-1, 6);
        eps_x = 1e-6;
        eps_v = 1e-6;
        for i = 2:N
            for k = 1:3
                x_p = x; x_p(k) = x_p(k) + eps_x;
                x_m = x; x_m(k) = x_m(k) - eps_x;
                fp = fdoa_3d_measurement(x_sensors(:,i), v_sensors(:,i), ...
                    x_sensors(:,1), v_sensors(:,1), x_p, v, f0, c);
                fm = fdoa_3d_measurement(x_sensors(:,i), v_sensors(:,i), ...
                    x_sensors(:,1), v_sensors(:,1), x_m, v, f0, c);
                H_f(i-1, k) = (fp - fm) / (2*eps_x);

                v_p = v; v_p(k) = v_p(k) + eps_v;
                v_m = v; v_m(k) = v_m(k) - eps_v;
                fp = fdoa_3d_measurement(x_sensors(:,i), v_sensors(:,i), ...
                    x_sensors(:,1), v_sensors(:,1), x, v_p, f0, c);
                fm = fdoa_3d_measurement(x_sensors(:,i), v_sensors(:,i), ...
                    x_sensors(:,1), v_sensors(:,1), x, v_m, f0, c);
                H_f(i-1, 3+k) = (fp - fm) / (2*eps_v);
            end
        end

        H = [H_r; H_f];

        W = diag([1/sigma_r^2 * ones(1, N-1), ...
                  1/sigma_f^2 * ones(1, N-1)]);

        delta = (H' * W * H + 1e-10*eye(6)) \ (H' * W * e);
        z_est = z_est + delta;
        hist(iter) = norm(delta);

        if norm(delta) < tol
            hist = hist(1:iter);
            break;
        end
    end
end

function [z_est, hist] = gn_3d_tdoa_only(x_sensors, r_meas, ...
    sigma_r, z_init, max_iter)
    % 3D TDOA-only 高斯-牛顿迭代
    z_est = z_init(:);
    hist = zeros(max_iter, 1);
    tol = 1e-10;
    N = size(x_sensors, 2);

    for iter = 1:max_iter
        x = z_est(1:3);

        R = zeros(1, N);
        u = zeros(3, N);
        for i = 1:N
            dx = x - x_sensors(:,i);
            R(i) = norm(dx);
            u(:,i) = dx / R(i);
        end

        r_pred = R(2:end) - R(1);
        e = r_meas(:) - r_pred(:);

        H = zeros(N-1, 6);
        for i = 2:N
            H(i-1, 1:3) = (u(:,i) - u(:,1))';
            H(i-1, 4:6) = 0;
        end

        W = diag(1/sigma_r^2 * ones(N-1, 1));

        delta = (H' * W * H + 1e-10*eye(6)) \ (H' * W * e);
        z_est = z_est + delta;
        hist(iter) = norm(delta);

        if norm(delta) < tol
            hist = hist(1:iter);
            break;
        end
    end
end

function C = compute_crlb_3d_tdoa(x_sensors, x_tgt, sigma_r)
    % 3D TDOA CRLB
    N = size(x_sensors, 2);
    R = zeros(1, N);
    u = zeros(3, N);
    for i = 1:N
        dx = x_tgt - x_sensors(:,i);
        R(i) = norm(dx);
        u(:,i) = dx / R(i);
    end

    H = zeros(N-1, 3);
    for i = 2:N
        H(i-1, :) = (u(:,i) - u(:,1))';
    end

    I = H' * H / sigma_r^2;

    if rcond(I) < 1e-12
        C = nan(3, 3);
    else
        C = inv(I + 1e-10*eye(3));
    end
end

---

四、代码讲解：为什么这么写？

4.1 5 个传感器，为什么要一个"抬起来"？

x_sensors = [0, 10, 0, 10, 5;
             0, 0, 10, 10, 5;
             0, 0, 0, 0, 8];   % 第 5 个传感器在 z=8 km

前 4 个传感器在同一平面（$z=0$）——如果只有它们，高度不可观测。

第 5 个传感器"抬起来"到 8 km 高度——打破共面性，恢复高度可观测性。

图3的对比：

• 共面构型（红）：$z$ 方向 CRLB 从几 km 到无穷大；
• 立体构型（蓝）：$z$ 方向 CRLB 保持几十到几百米。

工程准则：至少一个传感器要"显著高于"其他传感器——高度差 $\Delta z$ 越大，高度估计越准。

4.2 SX 算法为什么 3D 和 2D 结构完全一样？

A = xs';        % 3D: (N-1) × 3；2D: (N-1) × 2
b = 0.5 * (sum(xs.^2, 1)' - r_meas(:).^2);

关键洞察：SX 推导中没有用到维度——它只依赖"范数展开 + 参数化 + 二次方程"这个结构。

维度的唯一影响：

• $\mathbf{A}$ 的列数从 2 变 3；
• 二次方程的标量运算依然适用；
• ${\mathbf{x}}_0$ 和 ${\mathbf{x}}_r$ 从 2D 向量变 3D 向量。

代码复用性：2D 和 3D 的 SX 可以用同一个函数，只要 $\mathbf{A}$ 自动适配维度。

4.3 FDOA 雅可比为什么用数值差分？

H_f(i-1, k) = (fp - fm) / (2*eps_x);

FDOA 对位置的偏导：

$$\frac{\partial f_i}{\partial \mathbf{x}}=-\frac{f_0}{c}\cdot \frac{(\mathbf{v}-{\mathbf{v}}_i{)}^T(I-{\mathbf{u}}_i{\mathbf{u}}_i^T)}{R_i}$$

3D 情况下矩阵维度：

• $(\mathbf{v}-{\mathbf{v}}_i)\in {\mathbb{R}}^3$；
• $(I-{\mathbf{u}}_i{\mathbf{u}}_i^T)\in {\mathbb{R}}^{3\times 3}$；
• 结果 $\in {\mathbb{R}}^3$。

公式本身是闭式的——但 3D 情况下手动推导容易出错。数值差分（中心差分）精度足够（$O({ϵ}^2)$），且代码更简洁。

代价：每次迭代要计算 6 次额外的 FDOA——但相比于整个迭代过程，这个代价很小。

4.4 为什么用 5 个传感器而不是 4 个？

N = size(x_sensors, 2);   % N = 5

可解性：4 个传感器 → 3 个 TDOA + 3 个 FDOA = 6 个测量 vs 6 个未知量——恰好可解。

为什么要 5 个？

1. 冗余：8 个测量 vs 6 个未知量——可以做异常检测；
2. 鲁棒性：一个传感器故障不会让系统崩溃；
3. 精度：多传感器平均，降低随机误差；
4. 解模糊：TDOA 的二次方程可能有多个根，多传感器可以消歧。

工程建议：$N=5$ 是 3D 联合定位的"甜点"——精度足够，成本可接受。

4.5 GDOP 热力图为什么只画 $z=3$ km 平面？

x_t = [XX(i,j); YY(i,j); 3];

3D GDOP 是四维的（$x,y,z$, GDOP）——无法直接可视化。

工程做法：固定一个高度，画 $z=\text{const}$ 的切片——这就是水平面上的 GDOP 热力图。

观察：

• 传感器投影到该平面的区域内，GDOP 最小；
• 远离传感器的区域，GDOP 增大；
• 在传感器连线方向，GDOP 有"脊"——几何退化。

---

五、运行结果解读

运行代码后，你将看到 5 张图：

图1：三维传感器构型

5 个传感器（黑方块）分布在一个三维构型中，第 5 个明显"抬起来"到 8 km 高度。目标（红五角星）在中心区域，蓝色虚线连接到每个传感器。

图2：迭代收敛曲线

高斯-牛顿迭代的残差快速下降——通常 5~10 次迭代收敛到机器精度。

图3：高度可观测性

• 左图：$z$ 方向 CRLB 随目标高度变化。共面构型（红）在目标高度 > 2 km 时 CRLB 急剧上升；立体构型（蓝）保持稳定；
• 右图：柱状图对比两种构型的 $x$、$y$、$z$ CRLB。共面构型的 $z$ CRLB 是立体的 100 倍以上。

图4：蒙特卡洛验证

• 左图：$z$ 方向误差直方图。共面构型（红）散布宽，立体构型（蓝）集中在 0 附近；
• 右图：整体 RMSE 对比。立体构型比共面精度高 10 倍以上。

图5：3D GDOP 热力图

在 $z=3$ km 平面上，GDOP 从中心的 ~1 到边缘的 > 5。传感器包围区域内 GDOP 最小。

---

六、工程实战的四条准则

准则	数学依据	工程做法
1. 传感器必须非共面	共面 → Fisher 矩阵秩亏	至少一个传感器高出 2~5 km
2. 至少 5 个传感器	4 个恰好可解，无冗余	5~7 个提供鲁棒性
3. 高度差越大越好	$z$ CRLB $\propto 1/\Delta z$	机载编队保持 1~3 km 高度差
4. 在传感器包围圈内精度最高	GDOP 在内部最小	作战时尽量"环绕"目标

---

七、延伸思考

7.1 3D 定位的误差椭球

2D 的误差椭圆在 3D 中变成误差椭球：

$${\mathbf{C}}_{CRLB}\in {\mathbb{R}}^{3\times 3},\text{置信水平}p:s^2={\chi }_{3,p}^2$$

1σ 椭球只覆盖 19.9%（比 2D 的 39.4% 更低）——因为三维中误差可以在三个方向同时出现。

7.2 高度约束的工程应用

如果已知目标在地面（$z=0$），可以加入硬约束：

$$\min \Vert \mathbf{z}-\mathbf{h}(\mathbf{x}){\Vert }^2\text{s.t.}z=0$$

这等效于把 3D 问题降为 2D——精度大幅提升，且只需 3 个传感器。这就是为什么"地面目标定位"比"空中目标定位"容易得多。

7.3 与后续篇章的关系

第29~31篇构成 3D 定位三部曲：坐标转换、TDOA/FDOA、AOA。第31篇我们将进入 3D AOA 定位——用方位角 + 俯仰角联合估计，处理传感器不共面的阵列构型。敬请期待！

---

下一篇预告（第31篇）：3D AOA 定位——方位+俯仰联合

TDOA/FDOA 是时间/频率模态，AOA 是角度模态。在 3D 场景中：

• 2D AOA 只测方位角（azimuth）——无法区分高度；
• 3D AOA 同时测方位角 + 俯仰角（elevation）——确定完整方向向量；
• L 型阵列或平面阵列——最简单的 3D AOA 硬件构型。

我们将会：

• 推导 3D AOA 测量模型（方位 + 俯仰）；
• 实现 L 型干涉仪的 3D 测向；
• 用 MATLAB 演示 2D AOA vs 3D AOA 的定位精度对比；
• 分析俯仰角精度对高度估计的影响。

敬请期待！

转载自 CSDN-专业IT技术社区

原文链接：https://blog.csdn.net/CAINIAOdaDIDI/article/details/167702564