【电子战】第31篇：3D AOA 定位——方位+俯仰联合

第31篇：3D AOA 定位------方位+俯仰联合

一、作用：让"射线"变成"空间方向"

前一篇我们讲了 3D TDOA/FDOA 定位------用时间和频率差确定三维位置。但 TDOA/FDOA 有一个隐含前提：

多个传感器之间需要精确的时间/频率同步。

这在工程上并不容易------需要原子钟、GPS 授时、光纤同步链路，成本高，部署难。

3D AOA（到达角）定位 提供了另一种思路：

不测时间，只测角度------用方位角 + 俯仰角确定一条空间射线。

在这里插入图片描述

2D AOA vs 3D AOA：

特性	2D AOA	3D AOA
测量量	只有方位角 ψ\psiψ	方位角 ψ\psiψ + 俯仰角 θ\thetaθ
约束	水平面内一条射线	空间一条射线
需要传感器数（3D定位）	至少 3 个	至少 2 个
硬件	均匀线阵（ULA）	L 型阵 / 平面阵 / 圆形阵
典型应用	地面 ESM	机载/星载 ESM

核心优势：

• 不需要传感器间时间同步------每个传感器独立测向；
• 单传感器即可测向------不需要多站协同；
• 但需要每个传感器内部多通道相位同步------这是干涉仪/阵列的硬件要求。

本篇核心作用 ：从 3D AOA 测量模型出发，推导方位角 + 俯仰角 的联合估计；实现 L 型阵列的干涉仪测向 ；用 MATLAB 演示 2D AOA 三站定位 vs 3D AOA 双站定位 的精度对比；分析俯仰角精度对高度估计的影响；复现原书第二本（Practical Geolocation）中关于 3D AOA 的代码。

代码出处 ：book2_ch8_drawFigures、+aoa/、+array/ 模块。

---

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

2.1 3D AOA 测量模型

传感器位于 xi=(xi,yi,zi)\mathbf{x}_i = (x_i, y_i, z_i)xi=(xi,yi,zi)，目标位于 x=(x,y,z)\mathbf{x} = (x, y, z)x=(x,y,z)。

方向向量（从传感器指向目标）：

di=x−xi=(x−xi,  y−yi,  z−zi)\mathbf{d}_i = \mathbf{x} - \mathbf{x}_i = (x - x_i, \; y - y_i, \; z - z_i)di=x−xi=(x−xi,y−yi,z−zi)

方位角（从北向起算，顺时针为正）：

ψi=atan2(y−yi,  x−xi)\psi_i = \text{atan2}(y - y_i, \; x - x_i)ψi=atan2(y−yi,x−xi)

俯仰角（相对水平面，向上为正）：

θi=arcsin⁡(z−zi∥di∥)=atan2(z−zi,  (x−xi)2+(y−yi)2)\theta_i = \arcsin\left( \frac{z - z_i}{\|\mathbf{d}_i\|} \right) = \text{atan2}\left( z - z_i, \; \sqrt{(x-x_i)^2 + (y-y_i)^2} \right)θi=arcsin(∥di∥z−zi)=atan2(z−zi,(x−xi)2+(y−yi)2 )

观测模型（带噪声）：

ψi,meas=ψi+nψ,i,θi,meas=θi+nθ,i\psi_{i,meas} = \psi_i + n_{\psi,i}, \quad \theta_{i,meas} = \theta_i + n_{\theta,i}ψi,meas=ψi+nψ,i,θi,meas=θi+nθ,i

2.2 L 型阵列的 3D 测向

L 型阵列是最简单的 3D 测向天线：

• x 轴子阵 ：NxN_xNx 个阵元沿 x 轴排列，间距 dxd_xdx；
• y 轴子阵 ：NyN_yNy 个阵元沿 y 轴排列，间距 dyd_ydy；
• 共用原点（或一个阵元）。

相位差：

沿 x 轴的相邻阵元相位差：

Δϕx=2πdxλcos⁡θcos⁡ψ\Delta\phi_x = \frac{2\pi d_x}{\lambda} \cos\theta \cos\psiΔϕx=λ2πdxcosθcosψ

沿 y 轴的相邻阵元相位差：

Δϕy=2πdyλcos⁡θsin⁡ψ\Delta\phi_y = \frac{2\pi d_y}{\lambda} \cos\theta \sin\psiΔϕy=λ2πdycosθsinψ

反解方向：

令 u=cos⁡θcos⁡ψu = \cos\theta \cos\psiu=cosθcosψ（方向余弦 x 分量），v=cos⁡θsin⁡ψv = \cos\theta \sin\psiv=cosθsinψ（方向余弦 y 分量），则：

u=Δϕx⋅λ2πdx,v=Δϕy⋅λ2πdyu = \frac{\Delta\phi_x \cdot \lambda}{2\pi d_x}, \quad v = \frac{\Delta\phi_y \cdot \lambda}{2\pi d_y}u=2πdxΔϕx⋅λ,v=2πdyΔϕy⋅λ

方位角 ：

ψ=atan2(v,u)\psi = \text{atan2}(v, u)ψ=atan2(v,u)

俯仰角 ：

θ=arccos⁡(u2+v2)=arcsin⁡(1−u2−v2)\theta = \arccos\left( \sqrt{u^2 + v^2} \right) = \arcsin\left( \sqrt{1 - u^2 - v^2} \right)θ=arccos(u2+v2 )=arcsin(1−u2−v2 )

关键：

• u,vu, vu,v 是方向余弦 ------它们的平方和 ≤1\leq 1≤1；
• u2+v2\sqrt{u^2 + v^2}u2+v2 就是 cos⁡θ\cos\thetacosθ------俯仰角的余弦；
• 无模糊条件 ：dx,dy≤λ/2d_x, d_y \leq \lambda/2dx,dy≤λ/2。

2.3 3D AOA 定位

两个传感器 足以定位 3D 目标------因为每个传感器给出一条空间射线 ，两条射线在空间中交于一点。

传感器 iii 给出的射线：

pi(t)=xi+t⋅ui,t≥0\mathbf{p}_i(t) = \mathbf{x}_i + t \cdot \mathbf{u}_i, \quad t \geq 0pi(t)=xi+t⋅ui,t≥0

其中方向向量：

ui=cos⁡θicos⁡ψicos⁡θisin⁡ψisin⁡θi\mathbf{u}_i = \begin{bmatrix} \cos\theta_i \cos\psi_i \\ \cos\theta_i \sin\psi_i \\ \sin\theta_i \end{bmatrix}ui= cosθicosψicosθisinψisinθi

两条射线的交点：求解线性方程组

x1+t1u1=x2+t2u2\mathbf{x}_1 + t_1 \mathbf{u}_1 = \mathbf{x}_2 + t_2 \mathbf{u}_2x1+t1u1=x2+t2u2

写成矩阵形式：

u1−u2t1t2=x2−x1\begin{bmatrix} \mathbf{u}_1 & -\mathbf{u}_2 \end{bmatrix} \begin{bmatrix} t_1 \\ t_2 \end{bmatrix} = \mathbf{x}_2 - \mathbf{x}_1u1−u2t1t2=x2−x1

但这是超定方程组 （3 个方程、2 个未知量 t1,t2t_1, t_2t1,t2）------因为有噪声，两条射线不会精确相交。

最小二乘解：找"最接近两条射线"的点。

对于 N≥3N \geq 3N≥3 个传感器 ：用加权最小二乘（WLS）。

2.4 3D AOA 的 CRLB

设传感器 iii 的方位角噪声标准差为 σψ\sigma_\psiσψ，俯仰角噪声标准差为 σθ\sigma_\thetaσθ。

Fisher 信息矩阵：

I(x)=∑i=1N1σψ2Jψ,iTJψ,i+1σθ2Jθ,iTJθ,i\mathbf{I}(\mathbf{x}) = \sum_{i=1}^N \left \\frac{1}{\\sigma_\\psi\^2} \\mathbf{J}_{\\psi,i}\^T \\mathbf{J}_{\\psi,i} + \\frac{1}{\\sigma_\\theta\^2} \\mathbf{J}_{\\theta,i}\^T \\mathbf{J}_{\\theta,i} \\rightI(x)=i=1∑Nσψ21Jψ,iTJψ,i+σθ21Jθ,iTJθ,i

其中方位角雅可比：

Jψ,i=∂ψi∂x=1dxy2−(y−yi)(x−xi)0\mathbf{J}{\psi,i} = \frac{\partial \psi_i}{\partial \mathbf{x}} = \frac{1}{d{xy}^2} \begin{bmatrix} -(y - y_i) & (x - x_i) & 0 \end{bmatrix}Jψ,i=∂x∂ψi=dxy21−(y−yi)(x−xi)0

其中 dxy2=(x−xi)2+(y−yi)2d_{xy}^2 = (x-x_i)^2 + (y-y_i)^2dxy2=(x−xi)2+(y−yi)2。

俯仰角雅可比：

Jθ,i=∂θi∂x=1∥di∥2dxy2−(x−xi)(z−zi)−(y−yi)(z−zi)dxy2\mathbf{J}{\theta,i} = \frac{\partial \theta_i}{\partial \mathbf{x}} = \frac{1}{\|\mathbf{d}i\|^2 \sqrt{d{xy}^2}} \begin{bmatrix} -(x-x_i)(z-z_i) & -(y-y_i)(z-z_i) & d{xy}^2 \end{bmatrix}Jθ,i=∂x∂θi=∥di∥2dxy2 1−(x−xi)(z−zi)−(y−yi)(z−zi)dxy2

CRLB：

CCRLB=I−1\mathbf{C}_{CRLB} = \mathbf{I}^{-1}CCRLB=I−1

2.5 高度可观测性的"俯仰角条件"

核心问题 ：如果传感器与目标几乎在同一高度 （z≈ziz \approx z_iz≈zi），俯仰角 θ≈0\theta \approx 0θ≈0------俯仰角的微小噪声会放大为巨大的高度误差。

高度估计的标准差：

σz≈R⋅σθcos⁡θ\sigma_z \approx \frac{R \cdot \sigma_\theta}{\cos\theta}σz≈cosθR⋅σθ

其中 R=∥di∥R = \|\mathbf{d}_i\|R=∥di∥ 是距离。当 θ→90°\theta \to 90°θ→90°（目标在头顶）时，cos⁡θ→0\cos\theta \to 0cosθ→0------精度极高 ；当 θ→0\theta \to 0θ→0（目标在地平线）时，精度崩塌。

工程准则：

• 俯仰角 > 10°：高度估计可用；
• 俯仰角 < 5°：高度估计不可靠，必须借助其他模态（TDOA/FDOA）。

---

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

%% ========================================================================
% 第31篇：3D AOA 定位------方位+俯仰联合
% 参考出处：O'Donoughue, "Practical Geolocation for EW using MATLAB",
%           book2_ch8_drawFigures.m, +aoa/, +array/
%           https://github.com/nodonoughue/emitter-detection-book
% ========================================================================

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

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

% 目标真实位置
x_true = [5; 4; 3];   % km

% 两个传感器（3D AOA 双站定位）
x_sensors = [0, 10;
             0, 0;
             0, 0];   % km，两个传感器都在地面

% 噪声：方位角和俯仰角
sigma_psi = deg2rad(2);     % 2° 方位角误差
sigma_theta = deg2rad(2);   % 2° 俯仰角误差

fprintf('======= 3D AOA 定位参数 =======\n');
fprintf('目标真实位置: (%.2f, %.2f, %.2f) km\n', x_true);
fprintf('传感器 1: (%.1f, %.1f, %.1f) km\n', x_sensors(:,1));
fprintf('传感器 2: (%.1f, %.1f, %.1f) km\n', x_sensors(:,2));
fprintf('方位角噪声: %.1f°\n', rad2deg(sigma_psi));
fprintf('俯仰角噪声: %.1f°\n', rad2deg(sigma_theta));

%% ==================== 真实 AOA 测量 ====================

N = size(x_sensors, 2);

% 真实方位角和俯仰角
psi_true = zeros(1, N);
theta_true = zeros(1, N);

for i = 1:N
    d = x_true - x_sensors(:,i);
    psi_true(i) = atan2(d(2), d(1));
    theta_true(i) = atan2(d(3), sqrt(d(1)^2 + d(2)^2));
end

fprintf('\n真实方位角: '); fprintf('%.4f ', rad2deg(psi_true)); fprintf('deg\n');
fprintf('真实俯仰角: '); fprintf('%.4f ', rad2deg(theta_true)); fprintf('deg\n');

% 带噪声的测量
psi_meas = psi_true + sigma_psi * randn(1, N);
theta_meas = theta_true + sigma_theta * randn(1, N);

fprintf('\n测量方位角: '); fprintf('%.4f ', rad2deg(psi_meas)); fprintf('deg\n');
fprintf('测量俯仰角: '); fprintf('%.4f ', rad2deg(theta_meas)); fprintf('deg\n');

%% ==================== 图1：L型阵列测向原理 ====================

figure('Name','Fig1 L-Shaped Array','Position',[80 80 900 700]);

% 阵列参数
d_lam = 0.5;
Nx = 4; Ny = 4;

% 阵元位置
x_elem = (0:Nx-1) * d_lam;
y_elem = (0:Ny-1) * d_lam;

subplot(2,2,1);
hold on;
plot(x_elem, zeros(size(x_elem)), 'bs', 'MarkerSize', 12, ...
    'MarkerFaceColor', 'b');
plot(zeros(size(y_elem)), y_elem, 'rs', 'MarkerSize', 12, ...
    'MarkerFaceColor', 'r');
plot(0, 0, 'ks', 'MarkerSize', 15, 'MarkerFaceColor', 'k');
xlabel('x/\lambda'); ylabel('y/\lambda');
title('L 型阵列几何');
grid on; axis equal;
legend('x 轴子阵', 'y 轴子阵', '原点', 'Location', 'northeast');
xlim([-0.5, 2.5]); ylim([-0.5, 2.5]);

% 方向余弦 vs 方位/俯仰角
subplot(2,2,2);
psi_scan = linspace(0, 2*pi, 100);
theta_scan = linspace(0, pi/2, 50);
[PSI, THETA] = meshgrid(psi_scan, theta_scan);
U_dir = cos(THETA) .* cos(PSI);
V_dir = cos(THETA) .* sin(PSI);

contourf(rad2deg(PSI), rad2deg(THETA), sqrt(U_dir.^2 + V_dir.^2), 20);
colorbar;
xlabel('方位角 [deg]'); ylabel('俯仰角 [deg]');
title('cos(\theta) = sqrt(u^2+v^2)');
grid on;

% 2D AOA vs 3D AOA 的定位能力
subplot(2,2,3);
hold on;
plot(x_sensors(1,:), x_sensors(2,:), 'ks', 'MarkerSize', 14, ...
    'MarkerFaceColor', 'k');
plot(x_true(1), x_true(2), 'rp', 'MarkerSize', 18, ...
    'MarkerFaceColor', 'r');
% 画 2D AOA 射线
for i = 1:N
    r_plot = 15;
    plot(x_sensors(1,i) + [0, r_plot*cos(psi_true(i))], ...
         x_sensors(2,i) + [0, r_plot*sin(psi_true(i))], ...
         'b--', 'LineWidth', 1.5);
end
xlabel('x [km]'); ylabel('y [km]');
title('2D AOA：水平面射线交会');
grid on; axis equal;
xlim([-2, 12]); ylim([-2, 12]);

subplot(2,2,4);
hold on;
% 3D 视图
plot3(x_sensors(1,:), x_sensors(2,:), x_sensors(3,:), ...
    'ks', 'MarkerSize', 14, 'MarkerFaceColor', 'k');
plot3(x_true(1), x_true(2), x_true(3), 'rp', 'MarkerSize', 18, ...
    'MarkerFaceColor', 'r');
for i = 1:N
    r_plot = 10;
    u_dir = [cos(theta_true(i))*cos(psi_true(i));
             cos(theta_true(i))*sin(psi_true(i));
             sin(theta_true(i))];
    plot3(x_sensors(1,i) + [0, r_plot*u_dir(1)], ...
          x_sensors(2,i) + [0, r_plot*u_dir(2)], ...
          x_sensors(3,i) + [0, r_plot*u_dir(3)], ...
          'b--', 'LineWidth', 1.5);
end
xlabel('x [km]'); ylabel('y [km]'); zlabel('z [km]');
title('3D AOA：空间射线交会');
grid on; view([30, 20]); axis equal;
xlim([-2, 12]); ylim([-2, 12]); zlim([-1, 8]);

sgtitle('3D AOA 定位原理', 'FontSize', 14, 'FontWeight', 'bold');

%% ==================== 图2：3D AOA 定位（两站交会） ====================

fprintf('\n======= 3D AOA 双站定位 =======\n');

% 方法：最小二乘求射线最近点
x_est_2 = aoa_3d_localization(x_sensors, psi_meas, theta_meas);

fprintf('真实位置: (%.4f, %.4f, %.4f) km\n', x_true);
fprintf('估计位置: (%.4f, %.4f, %.4f) km\n', x_est_2);
fprintf('位置误差: %.4f km\n', norm(x_est_2 - x_true));

% 可视化
figure('Name','Fig2 3D AOA Localization','Position',[100 100 900 700]);
hold on;

% 传感器
plot3(x_sensors(1,:), x_sensors(2,:), x_sensors(3,:), ...
    'ks', 'MarkerSize', 16, 'MarkerFaceColor', 'k');

% 真实射线
for i = 1:N
    u_dir = [cos(theta_true(i))*cos(psi_true(i));
             cos(theta_true(i))*sin(psi_true(i));
             sin(theta_true(i))];
    r_max = 12;
    plot3(x_sensors(1,i) + [0, r_max*u_dir(1)], ...
          x_sensors(2,i) + [0, r_max*u_dir(2)], ...
          x_sensors(3,i) + [0, r_max*u_dir(3)], ...
          'g-', 'LineWidth', 2);
end

% 测量射线（含噪声）
for i = 1:N
    u_dir = [cos(theta_meas(i))*cos(psi_meas(i));
             cos(theta_meas(i))*sin(psi_meas(i));
             sin(theta_meas(i))];
    r_max = 12;
    plot3(x_sensors(1,i) + [0, r_max*u_dir(1)], ...
          x_sensors(2,i) + [0, r_max*u_dir(2)], ...
          x_sensors(3,i) + [0, r_max*u_dir(3)], ...
          'b--', 'LineWidth', 1);
end

% 真实目标和估计目标
plot3(x_true(1), x_true(2), x_true(3), 'rp', ...
    'MarkerSize', 20, 'MarkerFaceColor', 'r');
plot3(x_est_2(1), x_est_2(2), x_est_2(3), 'go', ...
    'MarkerSize', 14, 'MarkerFaceColor', 'g', 'LineWidth', 2);

xlabel('x [km]'); ylabel('y [km]'); zlabel('z [km]');
title('3D AOA 双站定位：射线交会');
grid on; view([30, 20]); axis equal;
legend('传感器', '真实射线', '', '测量射线', '', ...
    '真实目标', '估计目标', 'Location', 'northeast');
xlim([-2, 12]); ylim([-2, 12]); zlim([-1, 8]);

%% ==================== 图3：CRLB 与俯仰角的关系 ====================

fprintf('\n======= CRLB 与俯仰角的关系 =======\n');

% 固定水平位置，扫描目标高度
h_scan = 0.5:0.5:15;   % km
n_h = numel(h_scan);

crlb_std = zeros(3, n_h);

for idx = 1:n_h
    x_t = [5; 4; h_scan(idx)];
    C = compute_crlb_3d_aoa(x_sensors, x_t, sigma_psi, sigma_theta);
    crlb_std(:, idx) = sqrt(diag(C));
end

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

subplot(1,2,1);
semilogy(h_scan, crlb_std(1,:), 'b-', 'LineWidth', 2, ...
    'DisplayName', '\sigma_x');
hold on;
semilogy(h_scan, crlb_std(2,:), 'r-', 'LineWidth', 2, ...
    'DisplayName', '\sigma_y');
semilogy(h_scan, crlb_std(3,:), 'g-', 'LineWidth', 2, ...
    'DisplayName', '\sigma_z');
xlabel('目标高度 [km]'); ylabel('CRLB 标准差 [km]');
title('CRLB vs 目标高度');
grid on; legend('Location', 'northeast');

subplot(1,2,2);
% 俯仰角与 CRLB_z 的关系
theta_scan = deg2rad(1:1:80);
crlb_z_vs_theta = zeros(size(theta_scan));

for idx = 1:numel(theta_scan)
    theta = theta_scan(idx);
    % 距离固定为 10 km
    R_dist = 10;
    % 水平距离
    R_horiz = R_dist * cos(theta);
    z_height = R_dist * sin(theta);
    x_t = [x_sensors(1,1) + R_horiz * cos(psi_true(1)); ...
           x_sensors(2,1) + R_horiz * sin(psi_true(1)); ...
           z_height];
    C = compute_crlb_3d_aoa(x_sensors, x_t, sigma_psi, sigma_theta);
    crlb_z_vs_theta(idx) = sqrt(C(3,3));
end

semilogy(rad2deg(theta_scan), crlb_z_vs_theta, 'b-', 'LineWidth', 2);
xlabel('俯仰角 [deg]'); ylabel('\sigma_z CRLB [km]');
title('高度精度 vs 俯仰角（R = 10 km）');
grid on;
xline(10, 'r--', 'LineWidth', 1.5);
text(11, 0.5, '俯仰角 > 10° 可用', 'FontSize', 10);

sgtitle('3D AOA 定位的 CRLB 分析', ...
    'FontSize', 14, 'FontWeight', 'bold');

fprintf('俯仰角与 \sigma_z 的关系:\n');
for theta_deg = [2, 5, 10, 20, 45]
    idx = find(abs(rad2deg(theta_scan) - theta_deg) < 0.6, 1);
    if ~isempty(idx)
        fprintf('  theta = %2d deg: sigma_z = %.4f km\n', ...
            theta_deg, crlb_z_vs_theta(idx));
    end
end

%% ==================== 图4：2D AOA vs 3D AOA 定位对比 ====================

fprintf('\n======= 2D AOA vs 3D AOA 对比 =======\n');

% 三个地面传感器（2D AOA 场景）
x_sensors_2d = [0, 10, 5;
                0, 0, 10;
                0, 0, 0];

% 3D AOA 双站（用两个传感器就够了）
x_sensors_3d = [0, 10;
                0, 0;
                0, 0];

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

% 蒙特卡洛
num_mc = 500;

err_2d = zeros(3, num_mc);
err_3d = zeros(3, num_mc);

for mc = 1:num_mc
    % 2D AOA：只有方位角
    psi_2d_meas = zeros(1, 3);
    for i = 1:3
        d = x_tgt_cmp - x_sensors_2d(:,i);
        psi_2d_meas(i) = atan2(d(2), d(1)) + sigma_psi * randn();
    end
    % 2D AOA 定位（假设 z 已知为 3 km，否则 z 不可观测）
    x_est_2d = aoa_2d_3sensor(x_sensors_2d, psi_2d_meas, x_tgt_cmp(3));
    err_2d(:, mc) = x_est_2d - x_tgt_cmp;

    % 3D AOA：方位角 + 俯仰角
    psi_3d_meas = zeros(1, 2);
    theta_3d_meas = zeros(1, 2);
    for i = 1:2
        d = x_tgt_cmp - x_sensors_3d(:,i);
        psi_3d_meas(i) = atan2(d(2), d(1)) + sigma_psi * randn();
        theta_3d_meas(i) = atan2(d(3), sqrt(d(1)^2 + d(2)^2)) + ...
                            sigma_theta * randn();
    end
    x_est_3d = aoa_3d_localization(x_sensors_3d, psi_3d_meas, theta_3d_meas);
    err_3d(:, mc) = x_est_3d - x_tgt_cmp;
end

rmse_2d = sqrt(mean(sum(err_2d.^2, 1)));
rmse_3d = sqrt(mean(sum(err_3d.^2, 1)));

fprintf('2D AOA（3 站，仅方位角）RMSE: %.4f km\n', rmse_2d);
fprintf('3D AOA（2 站，方位角+俯仰角）RMSE: %.4f km\n', rmse_3d);
fprintf('3D AOA 优势: %.2f 倍\n', rmse_2d / rmse_3d);

figure('Name','Fig4 2D vs 3D AOA','Position',[140 140 900 600]);

subplot(1,2,1);
histogram(err_2d(3,:), 30, 'FaceColor', 'b', 'FaceAlpha', 0.6);
hold on;
histogram(err_3d(3,:), 30, 'FaceColor', 'r', 'FaceAlpha', 0.6);
xlabel('z 方向误差 [km]'); ylabel('计数');
title('高度估计误差分布');
legend('2D AOA (z 假设已知)', '3D AOA (z 估计)', ...
    'Location', 'northeast');
grid on;

subplot(1,2,2);
bar([rmse_2d, rmse_3d]);
set(gca, 'XTickLabel', {'2D AOA', '3D AOA'});
ylabel('位置 RMSE [km]');
title('定位精度对比');
grid on;
vals = [rmse_2d, rmse_3d];
for i = 1:2
    text(i, vals(i)+0.02, sprintf('%.4f', vals(i)), ...
        'HorizontalAlignment', 'center', 'FontSize', 12);
end

sgtitle('2D AOA vs 3D AOA：精度与传感器数量权衡', ...
    'FontSize', 14, 'FontWeight', 'bold');

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

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

% 在 z = 3 km 平面扫描
x_grid = -2:0.3:12;
y_grid = -2:0.3:12;
[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_aoa(x_sensors, x_t, sigma_psi, sigma_theta);
            GDOP_map(i,j) = sqrt(trace(C)) / sqrt(trace(C));
        catch
            GDOP_map(i,j) = NaN;
        end
    end
end

% 用位置 CRLB 的更直观量度
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_aoa(x_sensors, x_t, sigma_psi, sigma_theta);
            GDOP_map(i,j) = sqrt(trace(C)) / max(sigma_psi, sigma_theta);
        catch
            GDOP_map(i,j) = NaN;
        end
    end
end

GDOP_map(isnan(GDOP_map)) = 5;
GDOP_map = min(GDOP_map, 5);

figure('Name','Fig5 3D AOA GDOP','Position',[160 160 900 700]);
imagesc(x_grid, y_grid, GDOP_map);
set(gca, 'YDir', 'normal');
colorbar; clim([0, 5]);
hold on;
plot(x_sensors(1,:), x_sensors(2,:), 'ks', 'MarkerSize', 16, ...
    'MarkerFaceColor', 'k');
plot(x_true(1), x_true(2), 'rp', 'MarkerSize', 18, ...
    'MarkerFaceColor', 'r');
xlabel('x [km]'); ylabel('y [km]');
title('3D AOA 定位 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 x_est = aoa_3d_localization(x_sensors, psi_meas, theta_meas)
    % 3D AOA 定位（射线最近点最小二乘）
    % 输入：
    %   x_sensors: 3 x N 传感器位置
    %   psi_meas : 1 x N 方位角（弧度）
    %   theta_meas: 1 x N 俯仰角（弧度）

    N = size(x_sensors, 2);

    % 构造射线方向向量
    U = zeros(3, N);
    for i = 1:N
        U(:,i) = [cos(theta_meas(i))*cos(psi_meas(i));
                  cos(theta_meas(i))*sin(psi_meas(i));
                  sin(theta_meas(i))];
    end

    % 最小二乘：找到点 x 使得到所有射线的距离平方和最小
    % 等价于解 (I - U_i U_i^T)(x - x_i) 最小化
    A = zeros(3, 3);
    b = zeros(3, 1);
    for i = 1:N
        P_perp = eye(3) - U(:,i) * U(:,i)';
        A = A + P_perp;
        b = b + P_perp * x_sensors(:,i);
    end

    x_est = A \ b;
end

function x_est = aoa_2d_3sensor(x_sensors, psi_meas, z_assumed)
    % 2D AOA 三站定位（假设 z 已知）
    % 用水平面内射线交会

    N = size(x_sensors, 2);
    x_sensors_2d = x_sensors(1:2, :);

    % 找水平面内的最小二乘交点
    A = zeros(2, 2);
    b = zeros(2, 1);
    for i = 1:N
        u_i = [cos(psi_meas(i)); sin(psi_meas(i))];
        P_perp = eye(2) - u_i * u_i';
        A = A + P_perp;
        b = b + P_perp * x_sensors_2d(:,i);
    end
    x_2d = A \ b;

    x_est = [x_2d; z_assumed];
end

function C = compute_crlb_3d_aoa(x_sensors, x_tgt, sigma_psi, sigma_theta)
    % 3D AOA 定位的 CRLB
    N = size(x_sensors, 2);

    I_total = zeros(3, 3);

    for i = 1:N
        d = x_tgt - x_sensors(:,i);
        dx = d(1); dy = d(2); dz = d(3);

        R_xy_sq = dx^2 + dy^2;
        R_total_sq = R_xy_sq + dz^2;
        R_total = sqrt(R_total_sq);
        R_xy = sqrt(R_xy_sq);

        if R_xy < 1e-6
            continue;
        end

        % 方位角雅可比
        J_psi = [-dy/R_xy_sq, dx/R_xy_sq, 0];

        % 俯仰角雅可比
        J_theta = [-dx*dz/(R_total_sq*R_xy), ...
                   -dy*dz/(R_total_sq*R_xy), ...
                    R_xy/R_total_sq];

        I_total = I_total + J_psi' * J_psi / sigma_psi^2 + ...
                              J_theta' * J_theta / sigma_theta^2;
    end

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

---

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

4.1 3D AOA 的"射线最近点"最小二乘

A = zeros(3, 3);
b = zeros(3, 1);
for i = 1:N
    P_perp = eye(3) - U(:,i) * U(:,i)';
    A = A + P_perp;
    b = b + P_perp * x_sensors(:,i);
end
x_est = A \ b;

几何直觉 ：每条射线 xi+tui\mathbf{x}_i + t\mathbf{u}_ixi+tui 定义了空间中的一条直线。我们想找一点 x\mathbf{x}x，使得到所有射线的垂直距离平方和最小。

数学推导 ：点 x\mathbf{x}x 到射线 iii 的垂直距离为：

di2=∥(I−uiuiT)(x−xi)∥2d_i^2 = \|(\mathbf{I} - \mathbf{u}_i \mathbf{u}_i^T)(\mathbf{x} - \mathbf{x}_i)\|^2di2=∥(I−uiuiT)(x−xi)∥2

对所有 iii 求和并最小化：

x∗=arg⁡min⁡x∑i(x−xi)TPi⊥(x−xi)\mathbf{x}^* = \arg\min_{\mathbf{x}} \sum_i (\mathbf{x} - \mathbf{x}_i)^T \mathbf{P}_i^\perp (\mathbf{x} - \mathbf{x}_i)x∗=argxmini∑(x−xi)TPi⊥(x−xi)

这是加权最小二乘问题，闭式解为：

x∗=(∑iPi⊥)−1(∑iPi⊥xi)\mathbf{x}^* = \left( \sum_i \mathbf{P}_i^\perp \right)^{-1} \left( \sum_i \mathbf{P}_i^\perp \mathbf{x}_i \right)x∗=(i∑Pi⊥)−1(i∑Pi⊥xi)

注意 ：Pi⊥=I−uiuiT\mathbf{P}_i^\perp = \mathbf{I} - \mathbf{u}_i \mathbf{u}_i^TPi⊥=I−uiuiT 是正交投影矩阵 ------它把向量投影到垂直于 ui\mathbf{u}_iui 的平面上。

4.2 为什么 3D AOA 只需要 2 个传感器？

x_sensors = [0, 10;
             0, 0;
             0, 0];   % 2 个传感器

几何直觉 ：每个 3D AOA 传感器给出一条空间射线 。两条射线在 3D 空间中相交于一点------只要它们不平行，就唯一确定目标位置。

对比：

• 2D AOA ：每个传感器给出一条水平射线，两条射线在平面内交于一点------但高度不可观测，所以需要 3 个传感器（或假设高度已知）；
• 3D AOA ：每个传感器给出一条空间射线，两条射线在 3D 空间交于一点------2 个传感器就够了。

代价 ：每个传感器必须能测俯仰角------硬件更复杂。

4.3 为什么图3的 σz\sigma_zσz 在低俯仰角时"爆炸"？

semilogy(rad2deg(theta_scan), crlb_z_vs_theta, 'b-', 'LineWidth', 2);

近似公式：

σz≈R⋅σθcos⁡θ\sigma_z \approx \frac{R \cdot \sigma_\theta}{\cos\theta}σz≈cosθR⋅σθ

其中 RRR 是距离，θ\thetaθ 是俯仰角。

观察：

• θ=45°\theta = 45°θ=45°：cos⁡θ=0.707\cos\theta = 0.707cosθ=0.707 → σz≈1.4⋅R⋅σθ\sigma_z \approx 1.4 \cdot R \cdot \sigma_\thetaσz≈1.4⋅R⋅σθ；
• θ=10°\theta = 10°θ=10°：cos⁡θ=0.985\cos\theta = 0.985cosθ=0.985 → σz≈1.0⋅R⋅σθ\sigma_z \approx 1.0 \cdot R \cdot \sigma_\thetaσz≈1.0⋅R⋅σθ；
• θ=2°\theta = 2°θ=2°：cos⁡θ=0.999\cos\theta = 0.999cosθ=0.999 → 看起来还行？但这时 R≈RhorizontalR \approx R_{horizontal}R≈Rhorizontal，几何上严重退化。

真实的退化机制 ：当 θ→0\theta \to 0θ→0 时，俯仰角的微小变化对应巨大的高度变化------因为：

z=Rsin⁡θ≈Rθ（小角度）z = R \sin\theta \approx R \theta \text{（小角度）}z=Rsinθ≈Rθ（小角度）

所以 σz≈Rσθ\sigma_z \approx R \sigma_\thetaσz≈Rσθ。关键是 θ\thetaθ 越小，相同高度对应的距离越小，高度信息越弱。

工程准则 ：俯仰角 > 10° 时，高度估计可靠；< 5° 时，必须用其他模态（如 TDOA）。

4.4 为什么 3D AOA 的 CRLB 要分方位/俯仰两部分？

I_total = I_total + J_psi' * J_psi / sigma_psi^2 + ...
                      J_theta' * J_theta / sigma_theta^2;

Fisher 信息可加 ：每个独立测量贡献一个 Fisher 信息矩阵。方位角和俯仰角是两个独立测量------所以信息相加。

方位角雅可比：

Jψ=−Δydxy2Δxdxy20\mathbf{J}\psi = \begin{bmatrix} -\frac{\Delta y}{d{xy}^2} & \frac{\Delta x}{d_{xy}^2} & 0 \end{bmatrix}Jψ=−dxy2Δydxy2Δx0

关键 ：对 zzz 的偏导为 0 ------方位角不包含高度信息！

俯仰角雅可比：

Jθ=−ΔxΔzR2dxy−ΔyΔzR2dxydxyR2\mathbf{J}\theta = \begin{bmatrix} -\frac{\Delta x \Delta z}{R^2 d{xy}} & -\frac{\Delta y \Delta z}{R^2 d_{xy}} & \frac{d_{xy}}{R^2} \end{bmatrix}Jθ=−R2dxyΔxΔz−R2dxyΔyΔzR2dxy

关键 ：zzz 分量为 dxyR2≠0\frac{d_{xy}}{R^2} \neq 0R2dxy=0 ------俯仰角包含高度信息。

结论 ：只有俯仰角能测高度------这就是为什么 2D AOA 无法测高，必须用 3D AOA。

4.5 2D AOA vs 3D AOA 的传感器数量权衡

% 2D AOA: 3 个传感器
x_sensors_2d = [0, 10, 5; 0, 0, 10; 0, 0, 0];
% 3D AOA: 2 个传感器
x_sensors_3d = [0, 10; 0, 0; 0, 0];

核心权衡：

• 2D AOA ：需要 3 个传感器，但每个传感器只需线阵（测方位角）；
• 3D AOA ：只需 2 个传感器，但每个传感器需平面阵（同时测方位+俯仰）。

成本对比：

• 2D AOA：3 × 线阵成本；
• 3D AOA：2 × 平面阵成本 ≈ 2 × (2 × 线阵成本) = 4 × 线阵成本。

结论：

• 传感器数量少 → 3D AOA；
• 单个传感器成本低 → 2D AOA；
• 机载/星载平台 → 3D AOA（体积/重量受限）。

---

五、运行结果解读

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

图1：3D AOA 定位原理

• 左上：L 型阵列几何------x 轴子阵（蓝）和 y 轴子阵（红）共原点；
• 右上 ：方向余弦与方位/俯仰角的关系------u2+v2=cos⁡θ\sqrt{u^2+v^2} = \cos\thetau2+v2 =cosθ；
• 左下：2D AOA 射线在水平面内交会；
• 右下 ：3D AOA 射线在空间交会------俯仰角提供了高度信息。

图2：3D AOA 双站定位

绿色实线是真实射线，蓝色虚线是测量射线。两条测量射线不会精确相交------但最小二乘找到的点（绿圈）非常接近真实目标（红五角星）。

图3：CRLB 与俯仰角

• 左图 ：σx,σy,σz\sigma_x, \sigma_y, \sigma_zσx,σy,σz 随目标高度的变化------高度越高，σz\sigma_zσz 越大；
• 右图 ：σz\sigma_zσz 随俯仰角的变化------低俯仰角时急剧上升，10° 以上趋于平稳。

数值验证：

• 俯仰角 2°：σz≈0.35\sigma_z \approx 0.35σz≈0.35 km；
• 俯仰角 10°：σz≈0.07\sigma_z \approx 0.07σz≈0.07 km；
• 俯仰角 45°：σz≈0.05\sigma_z \approx 0.05σz≈0.05 km。

图4：2D AOA vs 3D AOA

• 左图：高度误差直方图------3D AOA（红）集中在 0 附近；
• 右图 ：整体 RMSE------3D AOA 比 2D AOA 精度高约 30%（在本例参数下）。

图5：3D AOA GDOP 热力图

在 z=3z = 3z=3 km 平面上的 GDOP 分布：

• 两个传感器之间的"走廊" GDOP 较小；
• 垂直于传感器连线方向 GDOP 增大；
• 远离传感器 GDOP 迅速上升。

---

六、工程实战的四条准则

准则	数学依据	工程做法
1. 俯仰角 > 10° 才可靠	σz≈Rσθ/cos⁡θ\sigma_z \approx R\sigma_\theta/\cos\thetaσz≈Rσθ/cosθ	机载 ESM 从上方俯视目标
2. 3D AOA 需平面阵	同时测方位+俯仰	L 型阵 / 圆形阵
3. 双站 3D AOA 可定位	两条空间射线交会	两架飞机/两颗卫星
4. 低俯仰角时用 TDOA 补充	俯仰角不携带高度信息	多模态融合

---

七、延伸思考

7.1 L 型阵列的解模糊

如果 dx/λ>0.5d_x/\lambda > 0.5dx/λ>0.5，相位差会卷绕------出现模糊。

解决方案：

• 短基线解模糊 ：在 L 型阵原点附近加短基线 （d/λ=0.5d/\lambda = 0.5d/λ=0.5）；
• 多基线：长短基线组合，逐级解模糊；
• 频率跳变：多频测量，利用"频率-相位"关系解模糊。

7.2 3D AOA 与 TDOA 的融合

AOA + TDOA 联合定位：

• AOA 提供方向约束；
• TDOA 提供距离约束；
• 两者融合可以减少所需传感器数------2 个传感器（各测 AOA + 协同测 TDOA）即可定位 3D 目标。

CRLB 分析：联合 Fisher 信息 = AOA 信息 + TDOA 信息。两者在低信噪比下互补。

7.3 与后续篇章的关系

第29~31篇构成了 3D 定位三部曲：

篇号	主题	模态
29	坐标转换	基础工具
30	3D TDOA/FDOA	时间/频率
31	3D AOA	角度

第32~34篇进入 高级主题：约束优化、多目标分辨、非冗余 TDOA 集。敬请期待！

---

下一篇预告（第32篇）：约束优化在定位中的应用

前面的所有定位算法都假设目标可以在空间中任意位置 。但真实场景中，目标往往有物理约束：

• 地面目标：z=0z = 0z=0；
• 公路上的车辆：目标必须在某条曲线上；
• 建筑物内的辐射源：目标必须在某多边形区域内；
• 飞行器：速度有上限。

约束优化 把这些先验知识转化为数学约束，在定位的同时满足约束。

我们将会：

• 推导 KKT 条件------带约束优化的最优性条件；
• 实现 对数障碍法------把不等式约束转化为无约束优化；
• 用 MATLAB 演示 "目标必须在地面上"约束下的定位精度提升；
• 复现原书第二本（Practical Geolocation）第5章的约束优化代码。

敬请期待！