如需帮助，或有导航、定位滤波相关的代码定制需求，请点击文末卡片联系作者

程序详解

本程序实现了三维 TOA（Time of Arrival）定位场景下的 GDOP（Geometric Dilution of Precision）和 CRLB（Cramér–Rao Lower Bound）分析，用于评估四锚点配置下的几何可观测性与理论定位精度极限。程序首先设定四个已知三维坐标的锚点以及一个目标真实位置，并引入 TOA 测量误差模型。为了分析定位性能，本代码对指定高度$z=z_{\text{fix}}$处的平面进行二维扫描，通过计算各网格位置相对于锚点的距离，构建三维差分几何矩阵$H$，再由费舍尔信息矩阵
$$\text{FIM}=\frac{H^{\top }H}{({\sigma }_{TOA}c{)}^2}$$
推得克拉美罗下界
$$\text{CRLB}={\text{FIM}}^{-1},$$
并进一步得到 GDOP
$$\text{GDOP}=\sqrt{\mathrm{trace}(\text{CRLB})}。$$

程序首先生成 GDOP 等值图，用于展示几何布局对定位精度的影响，并标记锚点投影及目标位置。随后，对目标真实位置计算理论 CRLB 的三轴标准差，反映 TOA 测距在当前几何配置下可达到的最小定位误差。最后，代码给出 X、Y、Z 三个方向的 CRLB 空间分布图，用于观察不同方向的敏感度差异，为定位系统设计、锚点布置优化与实验场景规划提供定量依据。

运行结果

GDOP等高线图：

CRLB等高线图：

MATLAB源代码

程序结构：

部分代码如下：

% 三维定位GDOP与CRLB计算（单目标，四锚点），TOA
% 作者: matlabfilter
% 2025-11-17/Ver1

%% 参数设置
clear; clc; close all;
rng(0); % 固定随机种子

% 锚点配置（三维坐标，可修改）
anchorPos = [0, 0, 0;      % 锚点1
    100, 0, 0;     % 锚点2
    50, 100, 0;   % 锚点3
    50, 50, 100];  % 锚点4

% 目标真实位置（三维坐标）
truePos = [60, 40, 50];    % 目标位置，可修改

% 测量误差参数
sigma_TOA = 1e-9;     % 到达时间误差标准差(秒)
c = 3e8;              % 光速(m/s)

%% GDOP计算(网格扫描法)
xRange = -50:10:150;   % X轴扫描范围
yRange = -50:10:150;   % Y轴扫描范围
zFix = 80;     % 固定Z轴高度进行切片分析（可修改）

完整代码：
https://download.csdn.net/download/callmeup/92334241

或：