第15篇：混合定位------TDOA/FDOA/AOA 联合
一、作用：从"单模态"到"多模态融合"
前四篇博客（第11~14篇），我们系统讲解了四种定位体制：
| 模态 | 测量量 | 几何形状 | 约束 | 特点 |
|---|---|---|---|---|
| AOA | 到达角 | 射线 | 位置 | 精度受测向限制，但瞬时输出 |
| TDOA | 到达时间差 | 双曲线 | 位置 | 精度高，需要时钟同步 |
| FDOA | 到达频率差 | 等多普勒线 | 位置+速度 | 提供速度约束 |
| TOA | 绝对到达时间 | 圆 | 位置（含 t 0 t_0 t0） | 需要目标配合 |
每一种模态都有短板：
- AOA 在低 SNR 下测向误差大，定位精度差；
- TDOA 只能定位，不能测速；
- FDOA 单独使用模糊严重（等多普勒线形状复杂）；
- TOA 需要目标配合，不适用于非合作辐射源。
那么问题来了：能不能把多种测量模态组合起来，取长补短？
答案是肯定的。这就是混合定位（Hybrid Geolocation） 的核心思想：
利用不同物理量之间的互补性，用最小代价换取最大精度。
实际应用场景：
- 现代战机 ESM 系统：机载阵列同时测向（AOA）+ 多机协同测时差（TDOA）+ 多普勒频差（FDOA）；
- 无人机集群定位：部分无人机测向，部分测时差，数据链融合；
- 5G 基站定位：基站同时测 TOA 和 AOA。
本篇核心作用 ：从统一的最大似然框架出发，推导 TDOA+FDOA+AOA 三模态联合定位的数学模型；用 CRLB 分析每种模态对精度的贡献；并用蒙特卡洛验证"1+1+1 > 3"的融合增益。
延伸阅读
在进入本篇混合定位的正式内容之前，推荐一篇与"多模态融合"直接相关的会议论文，供有兴趣的读者延伸参考：
该论文针对GNSS在高楼林立的城市峡谷和室内场景中定位精度恶化的问题，构建了基站网络并设计了随机移动终端的仿真场景，系统对比了多种经典定位算法（TOA-LLOP、TOA-CHAN、TDOA-CHAN、TDOA-TAYLOR、RSSI-KNN、RSSI-WKNN、RSSI-NN），并在此基础上实现了一种平权融合（等权融合）算法------即对不同模态或不同算法的定位结果赋予相同权重进行融合。实验表明，即使采用这种简单的等权融合策略，也能在特定场景下将定位精度提升约29%。
为什么推荐？
本篇（第15篇）讲的是TDOA / FDOA / AOA 三模态最大似然联合定位 ，核心是加权融合 ------需要已知各模态的噪声统计特性（ σ ψ , σ r , σ f \sigma_\psi, \sigma_r, \sigma_f σψ,σr,σf），并通过加权最小二乘实现最优融合。而Shang的论文则展示了另一种思路：在缺乏先验噪声信息或模型不够精确时，平权融合（等权融合）也能带来可观的工程收益。
| 维度 | 本篇博客 | Shang 2023 |
|---|---|---|
| 融合策略 | 加权最大似然（最优权重） | 平权/等权融合（相同权重） |
| 模态 | TDOA + FDOA + AOA | TOA / TDOA / RSSI |
| 场景 | 电子战无源定位 | 城市/室内蜂窝定位 |
| 先验信息 | 需要噪声方差 | 无需精细噪声模型 |
| 精度提升 | 理论最优 | 实测约29% |
如果你对平权融合 、RSSI定位 、或多算法横向对比感兴趣，这篇论文是很好的补充材料。它也印证了本系列一直强调的一个观点：
没有一种融合策略在所有场景下都最优------有先验信息时用加权融合，缺乏先验时平权融合也能"保底"。
二、原理：数学上发生了什么？
2.1 统一观测模型
设目标状态为 z = x ; x ˙ \mathbf{z} = \\mathbf{x}; \\dot{\\mathbf{x}} z=x;x˙（位置 + 速度）， N N N 个传感器。三种测量的通用形式：
AOA 测量 （传感器 i i i 观测方位角）：
ψ i = atan2 ( y − y i , x − x i ) + n ψ , i \psi_i = \text{atan2}(y - y_i, x - x_i) + n_{\psi,i} ψi=atan2(y−yi,x−xi)+nψ,i
TDOA 测量 （传感器 i i i 相对传感器 1 的距离差）：
r i , 1 = ∥ x − x i ∥ − ∥ x − x 1 ∥ + n r , i r_{i,1} = \|\mathbf{x} - \mathbf{x}_i\| - \|\mathbf{x} - \mathbf{x}1\| + n{r,i} ri,1=∥x−xi∥−∥x−x1∥+nr,i
FDOA 测量 （传感器 i i i 相对传感器 1 的频率差）：
f i , 1 = − f 0 c x ˙ T ( u i − u 1 ) − x ˙ i T u i + x ˙ 1 T u 1 + n f , i 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} fi,1=−cf0x˙T(ui−u1)−x˙iTui+x˙1Tu1+nf,i
将所有测量堆叠为向量 m \mathbf{m} m，所有未知量堆叠为 z \mathbf{z} z，观测模型简写为：
m = h ( z ) + n , n ∼ N ( 0 , C m ) \mathbf{m} = \mathbf{h}(\mathbf{z}) + \mathbf{n}, \quad \mathbf{n} \sim \mathcal{N}(0, \mathbf{C}_m) m=h(z)+n,n∼N(0,Cm)
2.2 最大似然估计
由于噪声是高斯的，最大似然估计等价于加权最小二乘：
z ^ = arg min z m − h ( z ) T C m − 1 m − h ( z ) \hat{\mathbf{z}} = \arg\min_{\mathbf{z}} \left \\mathbf{m} - \\mathbf{h}(\\mathbf{z}) \\right^T \mathbf{C}_m^{-1} \left \\mathbf{m} - \\mathbf{h}(\\mathbf{z}) \\right z^=argzminm−h(z)TCm−1m−h(z)
由于 h ( z ) \mathbf{h}(\mathbf{z}) h(z) 非线性，用高斯-牛顿迭代求解：
z k + 1 = z k + ( J k T C m − 1 J k ) − 1 J k T C m − 1 m − h ( z k ) \mathbf{z}_{k+1} = \mathbf{z}_k + \left( \mathbf{J}_k^T \mathbf{C}_m^{-1} \mathbf{J}_k \right)^{-1} \mathbf{J}_k^T \mathbf{C}_m^{-1} \left \\mathbf{m} - \\mathbf{h}(\\mathbf{z}_k) \\right zk+1=zk+(JkTCm−1Jk)−1JkTCm−1m−h(zk)
其中雅可比矩阵 J k = ∂ h ∂ z ∣ z k \mathbf{J}k = \frac{\partial \mathbf{h}}{\partial \mathbf{z}}\bigg|{\mathbf{z}_k} Jk=∂z∂h zk，它的结构直接体现了每种模态对位置/速度的约束。
2.3 雅可比矩阵的结构
对于 3 模态联合系统（ N N N 个传感器，AOA 全部测量，TDOA/FDOA 相对传感器 1），雅可比矩阵分块为：
J = J A O A , x 0 J T D O A , x 0 J F D O A , x J F D O A , v \mathbf{J} = \begin{bmatrix} \mathbf{J}{AOA,x} & \mathbf{0} \\ \mathbf{J}{TDOA,x} & \mathbf{0} \\ \mathbf{J}{FDOA,x} & \mathbf{J}{FDOA,v} \end{bmatrix} J= JAOA,xJTDOA,xJFDOA,x00JFDOA,v
关键结构：
- AOA 只约束位置 x \mathbf{x} x（对速度的导数为零）；
- TDOA 只约束位置 x \mathbf{x} x（同上）；
- FDOA 同时约束位置和速度------只有 FDOA 能"看见"速度。
这就解释了一个深刻的事实：没有 FDOA，就无法估计目标速度。
2.4 CRLB 与模态贡献
Fisher 信息矩阵为：
I ( z ) = J T C m − 1 J \mathbf{I}(\mathbf{z}) = \mathbf{J}^T \mathbf{C}_m^{-1} \mathbf{J} I(z)=JTCm−1J
位置 CRLB 和速度 CRLB 分别为：
C x = I − 1 1 : 2 , 1 : 2 , C v = I − 1 3 : 4 , 3 : 4 \mathbf{C}x = \left \\mathbf{I}\^{-1} \\right{1:2, 1:2}, \quad \mathbf{C}v = \left \\mathbf{I}\^{-1} \\right{3:4, 3:4} Cx=I−11:2,1:2,Cv=I−13:4,3:4
混合定位的核心问题 ：每种模态对 C x \mathbf{C}_x Cx 的贡献有多大？
设三种测量独立，Fisher 信息可加：
I = I A O A + I T D O A + I F D O A \mathbf{I} = \mathbf{I}{AOA} + \mathbf{I}{TDOA} + \mathbf{I}_{FDOA} I=IAOA+ITDOA+IFDOA
因此加入一种模态，只会增加信息，不会降低精度 。这就是"1+1+1 ≥ 3"的数学根源------但要小心，如果新模态的测量精度太差，可能反而拖累收敛。
2.5 模态互补的物理直觉
| 场景 | AOA 弱 | TDOA 弱 | FDOA 弱 | 混合优势 |
|---|---|---|---|---|
| 目标很远 | ✓（测向精度降） | ✗（时差精度高） | ✗ | TDOA+FDOA 主导 |
| 传感器近共线 | ✗ | ✓（几何退化） | ✗ | AOA 主导 |
| 目标静止 | ✗ | ✗ | ✓（FDOA 失效） | AOA+TDOA |
| 目标高速 | ✗ | ✓ | ✗ | 全部有效 |
混合定位的精髓 ：没有任何一种模态在所有场景下都最优，但组合起来可以覆盖绝大部分场景。
三、完整代码（可直接运行）
matlab
%% ========================================================================
% 第15篇：混合定位------TDOA/FDOA/AOA 联合（完整修复版）
% 基于：ch13_drawFigures（图13.1、13.2a~d）
% 修复：
% 1) CRLB 使用 pinv 处理秩亏
% 2) 精度敏感度分析传入正确的 meas_struct
% 3) AOA-only / TDOA-only 时速度 CRLB 设为 NaN
% ========================================================================
clear; close all; clc;
rng(2024);
c = 3e8; % 光速 m/s
f0 = 1e9; % 载频 1 GHz
%% ==================== 图1：几何配置示意 ====================
fprintf('======= 混合定位几何演示 =======\n');
x_sensors = [0, 3;
0, 0]; % km
v_sensors = [0.1, 0.1;
0, 0]; % km/s
x_true = [2; 4];
v_true = [0.05; 0.02];
fprintf('传感器1: 位置 (%.1f, %.1f), 速度 (%.2f, %.2f)\n', ...
x_sensors(:,1), v_sensors(:,1));
fprintf('传感器2: 位置 (%.1f, %.1f), 速度 (%.2f, %.2f)\n', ...
x_sensors(:,2), v_sensors(:,2));
fprintf('目标: 位置 (%.1f, %.1f), 速度 (%.3f, %.3f)\n', ...
x_true, v_true);
figure;
hold on;
plot(x_sensors(1,:), x_sensors(2,:), 'ks', 'MarkerSize', 16, ...
'MarkerFaceColor', 'k', 'DisplayName', '传感器');
quiver(x_sensors(1,:), x_sensors(2,:), v_sensors(1,:), v_sensors(2,:), ...
5, 'b', 'LineWidth', 2, 'MaxHeadSize', 0.5, 'DisplayName', '传感器速度');
plot(x_true(1), x_true(2), 'rp', 'MarkerSize', 20, ...
'MarkerFaceColor', 'r', 'DisplayName', '目标');
quiver(x_true(1), x_true(2), v_true(1), v_true(2), ...
8, 'm', 'LineWidth', 2, 'MaxHeadSize', 0.6, 'DisplayName', '目标速度');
for i = 1:2
psi = atan2(x_true(2) - x_sensors(2,i), x_true(1) - x_sensors(1,i));
r_plot = 5;
plot(x_sensors(1,i) + [0, r_plot*cos(psi)], ...
x_sensors(2,i) + [0, r_plot*sin(psi)], ...
'k-', 'LineWidth', 1.2, 'DisplayName', sprintf('AOA 线 %d', i));
end
R_true = zeros(1, 2);
for i = 1:2
R_true(i) = norm(x_true - x_sensors(:,i));
end
r_tdoa = R_true(2) - R_true(1);
plot_hyperbola(x_sensors(:,1), x_sensors(:,2), r_tdoa, [0.8 0.4 0], 2);
f_fdoa = fdoa_measurement(x_sensors(:,1), v_sensors(:,1), ...
x_sensors(:,2), v_sensors(:,2), ...
x_true, v_true, f0, c);
plot_isodoppler(x_sensors(:,1), v_sensors(:,1), ...
x_sensors(:,2), v_sensors(:,2), ...
f_fdoa, f0, c, [0.4 0 0.8], 2);
xlabel('x [km]'); ylabel('y [km]');
title('混合定位几何：AOA（射线）+ TDOA（双曲线）+ FDOA（等多普勒线）');
axis equal; grid on;
xlim([-2, 6]); ylim([-2, 7]);
legend('Location', 'northwest');
%% ==================== 图2：最大似然定位（三模态联合） ====================
fprintf('\n======= 三模态联合最大似然定位 =======\n');
psi_true = zeros(1, 2);
for i = 1:2
psi_true(i) = atan2(x_true(2) - x_sensors(2,i), x_true(1) - x_sensors(1,i));
end
f_fdoa_true = fdoa_measurement(x_sensors(:,1), v_sensors(:,1), ...
x_sensors(:,2), v_sensors(:,2), ...
x_true, v_true, f0, c);
fprintf('真实 AOA: [%.4f, %.4f] rad\n', psi_true);
fprintf('真实 TDOA: %.4f km\n', r_tdoa);
fprintf('真实 FDOA: %.4f Hz\n', f_fdoa_true);
sigma_psi = deg2rad(2);
sigma_r = 0.03;
sigma_f = 5;
psi_meas = psi_true + sigma_psi * randn(1, 2);
r_meas = r_tdoa + sigma_r * randn();
f_meas = f_fdoa_true + sigma_f * randn();
m_meas = [psi_meas(:); r_meas; f_meas];
C_m = diag([sigma_psi^2*ones(2,1); sigma_r^2; sigma_f^2]);
z_init = [x_true + 0.5*randn(2,1); 0; 0];
[z_est, hist_err] = hybrid_gn(x_sensors, v_sensors, m_meas, C_m, ...
z_init, f0, c, 30);
x_est = z_est(1:2);
v_est = z_est(3:4);
fprintf('估计位置: (%.4f, %.4f) km\n', x_est);
fprintf('估计速度: (%.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;
semilogy(1:length(hist_err), hist_err, 'bo-', 'LineWidth', 2, 'MarkerSize', 8);
xlabel('迭代次数'); ylabel('||\Delta z||');
title('高斯-牛顿迭代收敛曲线');
grid on;
%% ==================== 图3：各模态贡献对比 ====================
fprintf('\n======= 各模态的定位精度贡献对比 =======\n');
configs = {
'AOA only', 1, 0, 0;
'TDOA only', 0, 1, 0;
'AOA+TDOA', 1, 1, 0;
'AOA+TDOA+FDOA', 1, 1, 1;
};
sigma_psi_c = deg2rad(2);
sigma_r_c = 0.03;
sigma_f_c = 5;
num_mc = 500;
n_cfg = size(configs, 1);
rmse_pos = nan(n_cfg, 1);
rmse_vel = nan(n_cfg, 1);
crlb_pos = nan(n_cfg, 1);
crlb_vel = nan(n_cfg, 1);
for idx_cfg = 1:n_cfg
use_aoa = logical(configs{idx_cfg, 2});
use_tdoa = logical(configs{idx_cfg, 3});
use_fdoa = logical(configs{idx_cfg, 4});
fprintf('\t 计算配置: %s...\n', configs{idx_cfg, 1});
meas_struct = struct('use_aoa', use_aoa, 'use_tdoa', use_tdoa, ...
'use_fdoa', use_fdoa);
[m_sub, C_sub] = build_measurement(psi_true, r_tdoa, f_fdoa_true, ...
sigma_psi_c, sigma_r_c, sigma_f_c, ...
use_aoa, use_tdoa, use_fdoa);
err_pos = nan(2, num_mc);
err_vel = nan(2, num_mc);
for mc = 1:num_mc
m_noisy = m_sub + chol(C_sub + 1e-12*eye(size(C_sub)))' * randn(size(m_sub));
z0 = [x_true + 0.3*randn(2,1); 0; 0];
try
[z_est_mc, ~] = hybrid_gn_masked(x_sensors, v_sensors, m_noisy, ...
C_sub, z0, f0, c, 30, meas_struct);
err_pos(:, mc) = z_est_mc(1:2) - x_true;
% 只有 FDOA 存在时，速度才可观测
if use_fdoa
err_vel(:, mc) = z_est_mc(3:4) - v_true;
end
catch
% 忽略失败试验
end
end
err_pos = err_pos(:, ~any(isnan(err_pos), 1));
if ~isempty(err_pos)
rmse_pos(idx_cfg) = sqrt(mean(sum(err_pos.^2, 1)));
end
if use_fdoa
err_vel = err_vel(:, ~any(isnan(err_vel), 1));
if ~isempty(err_vel)
rmse_vel(idx_cfg) = sqrt(mean(sum(err_vel.^2, 1)));
end
end
% CRLB
C_crlb = hybrid_crlb_masked(x_sensors, v_sensors, x_true, v_true, ...
sigma_psi_c, sigma_r_c, sigma_f_c, ...
f0, c, meas_struct);
crlb_pos(idx_cfg) = sqrt(max(real(trace(C_crlb(1:2,1:2))), 0));
if use_fdoa
crlb_vel(idx_cfg) = sqrt(max(real(trace(C_crlb(3:4,3:4))), 0));
end
end
fprintf('\n配置\t\t位置 RMSE\t速度 RMSE\t位置 CRLB\t速度 CRLB\n');
for i = 1:n_cfg
fprintf('%-15s\t%.4f\t\t%.4f\t\t%.4f\t\t%.4f\n', configs{i,1}, ...
rmse_pos(i), rmse_vel(i), crlb_pos(i), crlb_vel(i));
end
% 绘图（用 NaN 显示"不可观测"）
figure;
subplot(1, 2, 1);
b1 = bar(rmse_pos);
set(gca, 'XTickLabel', configs(:,1), 'XTickLabelRotation', 20);
ylabel('位置 RMSE [km]');
title('各配置的位置定位精度');
grid on;
for i = 1:n_cfg
if ~isnan(rmse_pos(i))
text(i, rmse_pos(i)+0.005, sprintf('%.3f', rmse_pos(i)), ...
'HorizontalAlignment', 'center', 'FontSize', 9);
end
end
subplot(1, 2, 2);
b2 = bar(rmse_vel);
set(gca, 'XTickLabel', configs(:,1), 'XTickLabelRotation', 20);
ylabel('速度 RMSE [km/s]');
title('各配置的速度定位精度（NaN = 不可观测）');
grid on;
for i = 1:n_cfg
if ~isnan(rmse_vel(i))
text(i, rmse_vel(i)+0.002, sprintf('%.4f', rmse_vel(i)), ...
'HorizontalAlignment', 'center', 'FontSize', 9);
else
text(i, 0.005, 'N/A', 'HorizontalAlignment', 'center', ...
'FontSize', 10, 'Color', 'r', 'FontWeight', 'bold');
end
end
sgtitle('混合定位各模态贡献对比', 'FontSize', 13, 'FontWeight', 'bold');
%% ==================== 图4：位置 CRLB 热力图 ====================
fprintf('\n======= CRLB 热力图 =======\n');
x_grid = -2:0.3:8;
y_grid = -2:0.3:8;
[XX, YY] = meshgrid(x_grid, y_grid);
v_true_g = [0.05; 0.02];
cfg_names = {'AOA only', 'TDOA only', 'AOA+TDOA+FDOA'};
cfg_flags = [1 0 0; 0 1 0; 1 1 1];
GDOP_maps = cell(3, 1);
for idx_cfg = 1:3
use_aoa = logical(cfg_flags(idx_cfg, 1));
use_tdoa = logical(cfg_flags(idx_cfg, 2));
use_fdoa = logical(cfg_flags(idx_cfg, 3));
meas_struct = struct('use_aoa', use_aoa, 'use_tdoa', use_tdoa, ...
'use_fdoa', use_fdoa);
G_map = nan(size(XX));
for i = 1:size(XX, 1)
for j = 1:size(XX, 2)
x_t = [XX(i,j); YY(i,j)];
R_t = zeros(1, 2);
for k = 1:2
R_t(k) = norm(x_t - x_sensors(:,k));
end
if min(R_t) < 0.3
continue;
end
psi_t = zeros(1, 2);
for k = 1:2
psi_t(k) = atan2(x_t(2)-x_sensors(2,k), x_t(1)-x_sensors(1,k));
end
r_t = R_t(2) - R_t(1);
f_t = fdoa_measurement(x_sensors(:,1), v_sensors(:,1), ...
x_sensors(:,2), v_sensors(:,2), ...
x_t, v_true_g, f0, c);
try
C_crlb = hybrid_crlb_masked(x_sensors, v_sensors, x_t, v_true_g, ...
sigma_psi_c, sigma_r_c, sigma_f_c, ...
f0, c, meas_struct);
g = sqrt(max(real(trace(C_crlb(1:2,1:2))), 0));
G_map(i,j) = min(g, 3);
catch
G_map(i,j) = NaN;
end
end
end
G_map(isnan(G_map)) = 3;
GDOP_maps{idx_cfg} = G_map;
end
figure;
for idx_cfg = 1:3
subplot(1, 3, idx_cfg);
imagesc(x_grid, y_grid, real(GDOP_maps{idx_cfg}));
colorbar; set(gca, 'ydir', 'normal');
hold on;
plot(x_sensors(1,:), x_sensors(2,:), 'ks', 'MarkerSize', 12, ...
'MarkerFaceColor', 'k');
xlabel('x [km]'); ylabel('y [km]');
title(cfg_names{idx_cfg});
axis equal; clim([0, 3]);
grid on;
end
sgtitle('不同配置下的位置 CRLB 热力图 [km]', 'FontSize', 13, 'FontWeight', 'bold');
%% ==================== 图5：精度 vs 各模态噪声 ====================
fprintf('\n======= 精度对噪声的敏感度 =======\n');
sigma_psi_sweep = deg2rad(0.5:0.5:10);
sigma_r_sweep = 0.005:0.005:0.1;
sigma_f_sweep = 0.5:0.5:20;
rmse_vs_sigma_psi = zeros(size(sigma_psi_sweep));
rmse_vs_sigma_r = zeros(size(sigma_r_sweep));
rmse_vs_sigma_f = zeros(size(sigma_f_sweep));
s_p_base = deg2rad(2);
s_r_base = 0.03;
s_f_base = 5;
% 三模态全开的 meas_struct（关键修复）
meas_struct_full = struct('use_aoa', true, 'use_tdoa', true, 'use_fdoa', true);
for k = 1:length(sigma_psi_sweep)
C = hybrid_crlb_masked(x_sensors, v_sensors, x_true, v_true, ...
sigma_psi_sweep(k), s_r_base, s_f_base, ...
f0, c, meas_struct_full);
rmse_vs_sigma_psi(k) = sqrt(max(real(trace(C(1:2,1:2))), 0));
end
for k = 1:length(sigma_r_sweep)
C = hybrid_crlb_masked(x_sensors, v_sensors, x_true, v_true, ...
s_p_base, sigma_r_sweep(k), s_f_base, ...
f0, c, meas_struct_full);
rmse_vs_sigma_r(k) = sqrt(max(real(trace(C(1:2,1:2))), 0));
end
for k = 1:length(sigma_f_sweep)
C = hybrid_crlb_masked(x_sensors, v_sensors, x_true, v_true, ...
s_p_base, s_r_base, sigma_f_sweep(k), ...
f0, c, meas_struct_full);
rmse_vs_sigma_f(k) = sqrt(max(real(trace(C(1:2,1:2))), 0));
end
figure;
subplot(1, 3, 1);
plot(rad2deg(sigma_psi_sweep), rmse_vs_sigma_psi, 'b-', 'LineWidth', 2);
xlabel('AOA 误差 [度]'); ylabel('位置 RMSE [km]');
title('AOA 精度影响');
grid on;
subplot(1, 3, 2);
plot(sigma_r_sweep*1000, rmse_vs_sigma_r, 'r-', 'LineWidth', 2);
xlabel('TDOA 误差 [m]'); ylabel('位置 RMSE [km]');
title('TDOA 精度影响');
grid on;
subplot(1, 3, 3);
plot(sigma_f_sweep, rmse_vs_sigma_f, 'm-', 'LineWidth', 2);
xlabel('FDOA 误差 [Hz]'); ylabel('位置 RMSE [km]');
title('FDOA 精度影响');
grid on;
sgtitle('混合定位对三种测量模态噪声的敏感度', 'FontSize', 13, 'FontWeight', 'bold');
fprintf('======= 全部完成 =======\n');
%% ==================== 函数定义 ====================
function [m, C] = build_measurement(psi, r_tdoa, f_fdoa, ...
sigma_psi, sigma_r, sigma_f, ...
use_aoa, use_tdoa, use_fdoa)
% 根据配置构造测量向量和协方差
m = [];
C_diag = [];
if use_aoa
m = [m; psi(:)];
C_diag = [C_diag; sigma_psi^2 * ones(length(psi), 1)];
end
if use_tdoa
m = [m; r_tdoa];
C_diag = [C_diag; sigma_r^2];
end
if use_fdoa
m = [m; f_fdoa];
C_diag = [C_diag; sigma_f^2];
end
C = diag(C_diag);
end
function [z_est, err_hist] = hybrid_gn(x_sensors, v_sensors, m, C, z_init, f0, c, max_iter)
meas_struct = struct('use_aoa', true, 'use_tdoa', true, 'use_fdoa', true);
[z_est, err_hist] = hybrid_gn_masked(x_sensors, v_sensors, m, C, z_init, ...
f0, c, max_iter, meas_struct);
end
function [z_est, err_hist] = hybrid_gn_masked(x_sensors, v_sensors, m, C, z_init, ...
f0, c, max_iter, meas_struct)
% 带测量掩码的高斯-牛顿迭代
z_est = z_init(:);
err_hist = zeros(max_iter, 1);
tol = 1e-10;
for iter = 1:max_iter
x = z_est(1:2);
v = z_est(3:4);
[h_pred, J] = predict_and_jacobian(x_sensors, v_sensors, x, v, ...
f0, c, meas_struct);
e = m(:) - h_pred(:);
W = diag(1 ./ diag(C));
% 使用 pinv 处理秩亏（AOA-only / TDOA-only 时速度不可观测）
delta = pinv(J' * W * J) * (J' * W * e);
z_est = z_est + delta;
err_hist(iter) = norm(delta);
if norm(delta) < tol
err_hist = err_hist(1:iter);
break;
end
end
end
function [h, J] = predict_and_jacobian(x_sensors, v_sensors, x, v, f0, c, meas_struct)
% 预测测量值 + 计算雅可比矩阵
N = size(x_sensors, 2);
R = zeros(1, N);
u = zeros(2, N);
for i = 1:N
dx = x - x_sensors(:,i);
R(i) = norm(dx);
u(:,i) = dx / R(i);
end
h = [];
J = [];
% AOA
if meas_struct.use_aoa
for i = 1:N
h = [h; atan2(x(2) - x_sensors(2,i), x(1) - x_sensors(1,i))];
dpsi_dx = [-u(2,i)/R(i), u(1,i)/R(i)];
J = [J; dpsi_dx, 0, 0];
end
end
% TDOA
if meas_struct.use_tdoa
for i = 2:N
h = [h; R(i) - R(1)];
dr_dx = u(:,i)' - u(:,1)';
J = [J; dr_dx, 0, 0];
end
end
% FDOA
if meas_struct.use_fdoa
for i = 2:N
f_val = fdoa_measurement(x_sensors(:,i), v_sensors(:,i), ...
x_sensors(:,1), v_sensors(:,1), ...
x, v, f0, c);
h = [h; f_val];
eps_x = 1e-6;
eps_v = 1e-6;
df_dz = zeros(1, 4);
for k = 1:2
x_p = x; x_p(k) = x_p(k) + eps_x;
x_m = x; x_m(k) = x_m(k) - eps_x;
fp = fdoa_measurement(x_sensors(:,i), v_sensors(:,i), ...
x_sensors(:,1), v_sensors(:,1), ...
x_p, v, f0, c);
fm = fdoa_measurement(x_sensors(:,i), v_sensors(:,i), ...
x_sensors(:,1), v_sensors(:,1), ...
x_m, v, f0, c);
df_dz(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_measurement(x_sensors(:,i), v_sensors(:,i), ...
x_sensors(:,1), v_sensors(:,1), ...
x, v_p, f0, c);
fm = fdoa_measurement(x_sensors(:,i), v_sensors(:,i), ...
x_sensors(:,1), v_sensors(:,1), ...
x, v_m, f0, c);
df_dz(2+k) = (fp - fm) / (2*eps_v);
end
J = [J; df_dz];
end
end
end
function C = hybrid_crlb_masked(x_sensors, v_sensors, x, v, sigma_psi, sigma_r, sigma_f, ...
f0, c, meas_struct)
% 计算 CRLB（使用 pinv 处理秩亏）
[~, J] = predict_and_jacobian(x_sensors, v_sensors, x, v, f0, c, meas_struct);
C_diag = [];
N = size(x_sensors, 2);
if meas_struct.use_aoa
C_diag = [C_diag; sigma_psi^2 * ones(N, 1)];
end
if meas_struct.use_tdoa
C_diag = [C_diag; sigma_r^2 * ones(N-1, 1)];
end
if meas_struct.use_fdoa
C_diag = [C_diag; sigma_f^2 * ones(N-1, 1)];
end
C_meas = diag(C_diag);
% Fisher 信息矩阵
I = J' * (C_meas \ J);
% 使用 pinv 处理秩亏（AOA-only/TDOA-only 时速度不可观测）
C = pinv(I);
end
function f_diff = fdoa_measurement(x1, v1, x2, v2, x_t, v_t, f0, c)
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 plot_hyperbola(x1, x2, r_diff, color, lw)
d = norm(x2 - x1);
a = r_diff / 2;
c_hyp = d / 2;
if abs(a) >= c_hyp, return; end
b = sqrt(c_hyp^2 - a^2);
xc = (x1(1) + x2(1)) / 2;
yc = (x1(2) + x2(2)) / 2;
theta = atan2(x2(2) - x1(2), x2(1) - x1(1));
R_mat = [cos(theta), -sin(theta); sin(theta), cos(theta)];
t_param = linspace(-2, 2, 300);
x_hyp = a * cosh(t_param);
y_hyp = b * sinh(t_param);
if r_diff < 0, x_hyp = -x_hyp; end
pts = R_mat * [x_hyp; y_hyp];
plot(xc + pts(1,:), yc + pts(2,:), '-', 'Color', color, 'LineWidth', lw);
end
function plot_isodoppler(x1, v1, x2, v2, f_target, f0, c, color, lw)
x_min = min([x1(1), x2(1)]) - 4;
x_max = max([x1(1), x2(1)]) + 6;
y_min = min([x1(2), x2(2)]) - 4;
y_max = max([x1(2), x2(2)]) + 8;
x_g = linspace(x_min, x_max, 200);
y_g = linspace(y_min, y_max, 200);
[XX, YY] = meshgrid(x_g, y_g);
FF = zeros(size(XX));
for i = 1:size(XX, 1)
for j = 1:size(XX, 2)
x_t = [XX(i,j); YY(i,j)];
FF(i,j) = fdoa_measurement(x1, v1, x2, v2, x_t, [0;0], f0, c);
end
end
contour(XX, YY, FF, [f_target, f_target], '-', 'Color', color, 'LineWidth', lw);
end四、代码讲解：为什么这么写？
4.1 统一的高斯-牛顿框架
代码中最核心的是 predict_and_jacobian 函数------它根据 meas_struct 中的开关，动态构建 测量向量 h \mathbf{h} h 和雅可比矩阵 J \mathbf{J} J：
matlab
if meas_struct.use_aoa
% 添加 AOA 的行
end
if meas_struct.use_tdoa
% 添加 TDOA 的行
end
if meas_struct.use_fdoa
% 添加 FDOA 的行
end为什么这样设计？ 因为混合定位的本质是"按需组合 "。同一份代码，通过修改三个布尔开关，就能实现 AOA-only、TDOA-only、AOA+TDOA、AOA+TDOA+FDOA 等任意组合------这就是混合定位的工程价值。
4.2 雅可比矩阵的稀疏结构
仔细看雅可比矩阵的构造：
matlab
J = [J; dpsi_dx, 0, 0]; % AOA 行：后两列为零
J = [J; dr_dx, 0, 0]; % TDOA 行：后两列为零
J = [J; df_dz]; % FDOA 行：4 列全非零- AOA 和 TDOA 只约束位置 → 对速度的导数为零；
- FDOA 同时约束位置和速度 → 4 列都有贡献。
这个结构直接导致了一个重要结论：没有 FDOA，速度列全为零，Fisher 信息矩阵奇异 → 无法估计速度。
4.3 数值雅可比 vs 解析雅可比
对于 AOA 和 TDOA，代码用了解析雅可比：
- AOA： ∂ ψ i / ∂ x = − Δ y i R i 2 , Δ x i R i 2 \partial\psi_i/\partial \mathbf{x} = \left-\\frac{\\Delta y_i}{R_i\^2}, \\frac{\\Delta x_i}{R_i\^2}\\right ∂ψi/∂x=−Ri2Δyi,Ri2Δxi
- TDOA： ∂ r i , 1 / ∂ x = u i − u 1 \partial r_{i,1}/\partial \mathbf{x} = \mathbf{u}_i - \mathbf{u}_1 ∂ri,1/∂x=ui−u1
但对于 FDOA，解析公式过于冗长 ，代码改用中心差分：
matlab
df_dz(k) = (fp - fm) / (2*eps_x);eps_x = 1e-6 是经验值------太小受浮点误差影响，太大会引入非线性误差。
4.4 收敛判据
matlab
if norm(delta) < tol, break; endtol = 1e-10 是相对严格的阈值。如果 30 次迭代内没收敛，可能说明：
- 初值离真值太远；
- 几何构型退化（Fisher 信息矩阵接近奇异）；
- 噪声过大导致迭代陷入局部极小。
工程实践 ：实际系统会同时记录 err_hist，如果发现振荡或发散，切换到更保守的初值或降级到单模态估计。
4.5 CRLB 的对角加载
matlab
C = inv(I + 1e-10*eye(4));加入微小的对角项，避免几何退化时 inv 报错。代价是 CRLB 略微乐观（因为加了信息），但实际中 Fisher 信息矩阵本身接近奇异，加一点正则化是必要且合理的。
4.6 三种配置的对比实验
图3是最关键的实验------定量比较四种配置：
- AOA only：精度最差（因为测向误差大）；
- TDOA only ：位置精度提升，速度不可估 （
NaN）； - AOA+TDOA：位置精度进一步提升；
- AOA+TDOA+FDOA ：位置精度最佳，速度也能估。
这个对比清晰地展示了"模态越多，精度越高；FDOA 是速度的必要条件"。
五、运行结果解读
运行代码后，你将看到 5 张图：
-
图1（几何示意） ：从两个传感器发出的 AOA 射线、TDOA 双曲线（橙色）、FDOA 等多普勒线（紫色）在真实目标附近交会。
-
图2（迭代收敛） ：高斯-牛顿迭代的残差随迭代次数快速下降------通常 5~10 次迭代即可收敛到机器精度。
-
图3（模态对比） ：
- 位置 RMSE：AOA-only > TDOA-only > AOA+TDOA > AOA+TDOA+FDOA；
- 速度 RMSE：只有 FDOA 参与的配置才能给出（其他三个为 NaN）。
-
图4（CRLB 热力图） ：
- AOA-only：在传感器连线方向有"谷"（几何退化）；
- TDOA-only：在双曲线模糊方向精度差；
- AOA+TDOA+FDOA ：全平面均匀，退化区域被显著抑制。
-
图5（噪声敏感度） ：
- AOA 误差从 0.5° 增到 10°，位置 RMSE 从 0.1 km 增到 0.8 km（近似线性）；
- TDOA 误差从 5 m 增到 100 m，位置 RMSE 增大约 20 倍；
- FDOA 误差从 0.5 Hz 增到 20 Hz，位置 RMSE 变化不大（FDOA 主要影响速度，对位置影响较小）。
六、工程实战的四条准则
| 准则 | 数学依据 | 工程做法 |
|---|---|---|
| 1. FDOA 是速度估计的必要条件 | 雅可比矩阵速度列非零 | 需要测速的场景必须配备 FDOA |
| 2. TDOA 提供位置主约束 | 高精度时间同步 | 定位精度的主要来源 |
| 3. AOA 提升几何鲁棒性 | 补充位置信息 | 传感器布站尽量"张开" |
| 4. 模态越多，计算量越大 | 雅可比维度增加 | 根据场景动态启用/禁用模态 |
七、延伸思考
- 传感器数量与模态的平衡 ： N N N 个传感器，3 模态的全部测量维度为 3 N − 2 3N-2 3N−2（AOA 有 N N N 个，TDOA 有 N − 1 N-1 N−1，FDOA 有 N − 1 N-1 N−1）。过约束程度决定了系统的鲁棒性。
- 异常测量剔除 ：实战中某个传感器的 AOA 可能因多径而严重偏差。鲁棒估计（如 RANSAC、Huber 损失）可以识别并剔除异常测量。
- 分布式融合 ：如果每个传感器只有部分模态，可以在融合中心做分布式卡尔曼滤波 或协方差交叉（CI） ------这是当前多传感器融合的研究热点。
- 动态跟踪 ：把混合定位嵌入扩展卡尔曼滤波（EKF） 或 无迹卡尔曼滤波（UKF） ，可以实现对运动目标的连续跟踪（原书第2本教材第7章）。
附录：系列博客完结
到此为止，电子战系列博客（共15篇） 完整地走过了从信号检测 到多模态融合定位的全流程：
| 篇号 | 主题 | 核心算法 |
|---|---|---|
| 01 | 噪声、门限与PFA/PD | 卡方分布、Neyman-Pearson |
| 02 | 能量检测器 vs 相关检测器 | 匹配滤波 |
| 03 | CFAR | CA-CFAR |
| 04 | 匹配滤波与脉冲压缩 | BPSK 编码 |
| 05 | Watson-Watt 测向 | 幅度比较 |
| 06 | 干涉仪测向 | 相位差、多基线解模糊 |
| 07 | 多普勒测向 | Bessel 函数、DFT |
| 08 | 多普勒测向的工程实现 | 批量FFT、盲估计 |
| 09 | 阵列信号处理 | Beamscan/MVDR/MUSIC |
| 10 | 测向精度极限 | CRLB |
| 11 | AOA 定位 | 三角测量、最小二乘 |
| 12 | TDOA 定位 | 双曲线、SX 闭式解 |
| 13 | FDOA 定位 | 等多普勒线、联合估计 |
| 14 | TOA 定位 | 圆交汇、未知 t₀ 消元 |
| 15 | 混合定位 | 多模态最大似然 |
从"有没有信号"到"在哪、多快、往哪飞"------这就是电子战信号处理的完整画卷。
希望这个系列能帮助你理解雷达、ESM、通信对抗等领域的核心技术，也欢迎你带着问题回来，我们在下一轮的博客中继续探讨：
- 第2本教材：跟踪滤波、卡尔曼滤波、3D 坐标系统（ENU/ECEF）、多传感器数据链；
- 进阶主题：压缩感知 DOA、深度学习定位、MIMO 雷达、认知电子战。
感谢你的阅读！🎯