GPS 导航实验报告：伪距定位与算法实现

该研究报告的目标是根据卫星/接收器之间的伪距离测量来计算GPS系统用户的轨迹。问题的难点在于这些测量值与待估参数之间的非线性关系，并且还受到附加测量噪声的干扰。为了克服这些困难，采用了线性化的最小二乘算法版本。在研究算法在不利导航条件下的鲁棒性后，将通过计算几何精度因子 (Dilution of Precision, DOP) 来评估卫星星座的几何结构对GPS性能的影响。

1. 研究报告背景

在本研究中，我们关注一辆配备了GPS的汽车，该车在塔朗斯市(Talence)沿图1所示的路线行驶。在导航中，必须明确指出车辆运动所处的参考系。特别地，两个参考系在本研究中是有用的：

参考系1：

此参考系是GPS导航的地球中心地固(ECEF)参考系，卫星的坐标通常在该参考系中表示。其原点是地球中心O，x轴位于黄道平面(赤道)和格林尼治子午线的交点，z轴与地球极轴重合并指向北极，y轴则定义为与x轴和z轴形成右手坐标系。

参考系2：

此参考系是本地的北东地( NED)坐标系，用于可视化车辆的运动。该坐标系以地面上的某个点P0为中心，其轴分别指向局部垂直方向、北方和东方。

这两个参考系之间的关系在图2中给出。连接两个参考系中向量坐标的公式为：

X^1 = MX^2 + X^1_0\\

其中 X^1_0 = (x_0, y_0, z_0)^T 表示点 P_0 在参考系1中的坐标,变换矩阵 M 为:\

M = \begin{pmatrix} - \sin \lambda \cos \phi & - \sin \phi & - \cos \lambda \cos \phi \\ - \sin \lambda \sin \phi & \cos \phi & - \cos \lambda \sin \phi \\ \cos \lambda & 0 & - \sin \lambda \end{pmatrix}

其中 λ 和 φ 分别是参考点 P_0 的经度和纬度。

2. 回顾：利用最小二乘法计算位置

2.1 最小二乘法算法

最小二乘法算法用于从带噪声的观测值中估计一组未知参数，这些观测值与待估参数线性相关。该问题可通过以下数学方式表述：

Z = H X + w

其中：

X 是包含未知参数的状态向量，
Z 是观测值向量，
w 是观测噪声向量。

最小二乘法的技术是计算X的估计值，记作\hat{X}，它使得二次误差J(\xi) = \|Z - H \xi\|^2最小。

其估计值满足：

\hat{X} = (H^T H)^{-1} H^T Z

其中(H^T H)^{-1} H^T称为伪逆矩阵。

2.2 GPS中的应用

在GPS导航问题中，每时每刻待估参数包括车辆在三维空间中的位置 (x, y, z) 以及接收器的时钟偏移 b。它们组成了状态向量 X = (x, y, z, b)^T，并通过伪距离测量与GPS信号的关系表示为：

Y(t) = h_t(x, y, z, b) + w(t)

其中向量 Y(t) = (Y_1, Y_2, ..., Y_n)^T 是在时刻 t 可用的 n 个GPS测量值的集合，向量 w(t) 是零均值、方差为 \sigma^2 的高斯白噪声，表示GPS测量噪声。

函数 h_t(x, y, z, b) 的形式为：

h_t(x, y, z, b) = \begin{pmatrix} \sqrt{(x - x^1(t))^2 + (y - y^1(t))^2 + (z - z^1(t))^2} + b \\ \vdots \\ \sqrt{(x - x^n(t))^2 + (y - y^n(t))^2 + (z - z^n(t))^2} + b \end{pmatrix}

其中 (x^i(t), y^i(t), z^i(t)) 是第 i 个卫星在时刻 t 的坐标。

最小二乘法算法是为线性系统开发的，因此需要对公式 (4) 进行线性化以便应用。通过在参考点 X_r 处进行泰勒展开，我们得到：

Y(t) - h_t(x_r, y_r, z_r, b_r) = H_t (X - X_r) + w(t)

其中 H_t = \nabla h_t，包含了 h_t 对状态向量分量的偏导数。

令

Z(t) = Y(t) - h_t(x_r, y_r, z_r, b_r) + H_t X_r

估计问题就可以表示为 (4) 形式。

该方法的有效性取决于选择的线性化参考点，不应偏离真实状态向量 X 太远。在GPS导航中，通常选择前一个位置和时钟偏移的估计值作为线性化参考点。

H_t 矩阵的系数取决于GPS卫星相对于接收器的位置。该矩阵反映了卫星几何结构对位置估计误差的影响。

3. 精度稀释因子 (DOP)

如前所述，定位误差既依赖于测量的不确定性，也依赖于卫星星座的几何结构。对于同样的测量误差向量，由于卫星相对于接收器的相对位置不同，位置估计误差可能大小不同。卫星星座的影响通过一个称为精度稀释因子 (DOP, Dilution of Precision) 的量化因子来衡量，它定义如下：

RMSE = DOP × σ

其中 σ 是测量噪声的标准差，RMSE 是位置估计的均方根误差:

\text{RMSE} = \sqrt{\mathbb{E} \left[ \|X - \hat{X}\|^2 \right]}

DOP 的表达式可以从最小二乘法的定位问题解中推导。我们回顾如下公式：

\hat{X} = (H_t^T H_t)^{-1} H_t^T Z

因此，位置估计误差满足：

\hat{X} - X = (H_t^T H_t)^{-1} H_t^T w(t)

其中 w(t) 是测量噪声。通过观察：

\mathbb{E} \left[ \|X - \hat{X}\|^2 \right] = \text{Trace}(\Sigma_{\Delta X \Delta X})\ \ \ \ \ \ \ \ \text{avec} \ \Sigma_{\Delta X \Delta X} = \mathbb{E} \left[ (X - \hat{X})(X - \hat{X})^T \right]

我们可以证明：

\text{DOP} = \sqrt{\text{Trace}\left( (H_t^T H_t)^{-1} \right)}

这个因子用于计算位置和时钟偏移估计误差的量级，因此可以衡量GPS解的可信度。如果只考虑位置误差（不考虑时钟偏移估计），则可以计算位置精度稀释因子 (PDOP, Position Dilution of Precision)：

PDOP = \sqrt{\tilde{H}_t(1,1) + \tilde{H}_t(2,2) + \tilde{H}_t(3,3)} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \text{其中} \ \tilde{H}_t = (H_t^T H_t)^{-1}

根据位置所在的参考系，还可以计算垂直精度稀释因子 (VDOP, Vertical DOP) 和水平精度稀释因子 (HDOP, Horizontal DOP)。

4. 提供的数据描述

为了解决定位问题，GPS数据保存在文件 donnees GPS TP.mat 中。这些数据包括：

每个时刻车辆轨迹点的GPS伪距离。数据每秒记录一次，记录N秒。所考虑的接收器具有8个追踪通道，因此最多可以同时处理8个测量。
卫星相对于接收器的坐标，以WGS84坐标系表示。

此外，为了评估导航算法的性能，车辆的真实轨迹以本地坐标系表示，并保存在文件 "trajectoire TP.mat" 中。

数据格式

数据类型	变量名	维度
GPS伪距离	PRN	8 × T
卫星坐标	XYZsat	8 × 3 × T
车辆轨迹	Xloc	3 × T

当GPS数据不可用时，PRN 和XYZsat表格包含NaN值。最后，提供了程序 llh2xyz.m，该程序用于根据椭球坐标（以弧度表示的纬度和经度、高度）计算点的笛卡尔坐标。

附录

考虑从参考系 R'（定义为轴 (x', y', z')）到参考系 R（定义为轴 (x, y, z)）的变换矩阵 M，满足：

X = M X'

其中 X' 和 X 分别是点在参考系 R' 和 R 中的坐标向量。矩阵 M 的列是参考系 R' 中单位向量在参考系 R 中的坐标。因此，如果两个参考系之间的变换是绕 z 轴旋转 \theta 角，如下所示，则 M 满足：

M = \begin{pmatrix} \cos θ & -\sin θ & 0 \\ \sin θ & \cos θ & 0 \\ 0 & 0 & 1 \end{pmatrix}

类似的推理也适用于绕x轴或y轴的旋转。

参阅附录，通过将参考系1到参考系2的变换分解为两次旋转，推导矩阵M的表达式(2)。

旋转1:

从地球中心的地心地固坐标系（ECEF）进行绕 Z 轴 的旋转，旋转角度为 \phi，变换公式为：

M_z(\phi) = \begin{pmatrix} \cos \phi & -\sin \phi & 0 \\ \sin \phi & \cos \phi & 0 \\ 0 & 0 & 1 \end{pmatrix}

旋转2:

将坐标系绕新的 Y 轴 旋转，旋转角度为 -\left(\tfrac{\pi}{2} + \lambda\right)，变换公式为：

M_{y}(-\left(\tfrac{\pi}{2} + \lambda\right)) \;=\; \begin{pmatrix} -\sin\lambda & 0 & -\cos\lambda\\[3pt] 0 & 1 & 0\\ \cos\lambda & 0 & -\sin\lambda \end{pmatrix}

最终变换:

将两次旋转合并，变换矩阵 M 表达式为：

M = M_{z}(\phi)\, M_{y}\bigl(-(\tfrac{\pi}{2}+\lambda)\bigr)

通过矩阵相乘：

M = \begin{pmatrix} \cos \phi & -\sin \phi & 0 \\ \sin \phi & \cos \phi & 0 \\ 0 & 0 & 1 \end{pmatrix} \cdot \begin{pmatrix} -\sin\lambda & 0 & -\cos\lambda\\[3pt] 0 & 1 & 0\\ \cos\lambda & 0 & -\sin\lambda \end{pmatrix}

计算结果为：

使用程序 llh2xyz 计算参考点P0（参考系2的原点）在参考系1中的坐标。对于所研究的轨迹，参考点的高度为零，纬度为北纬44°48'，经度为西经0°35'（注意：经度向东为正）。提示：1' = 1/60°。

参考点 P_0 的椭球坐标：

纬度 \phi = 44^\circ48'\,\text{N}
经度 \lambda = 0^\circ35'\,\text{W} （注意西经为负）
高度 h=0

将分分秒转成小数度

因为 1 分 = \frac{1}{60} 度

\phi = 44 + \frac{48}{60} = 44.8^\circ, \quad \lambda = -\left(0 + \frac{35}{60}\right) = -0.5833^\circ

转弧度

\phi_{\text{rad}} = 44.8^\circ \times \frac{\pi}{180} \approx 0.7819, \quad \lambda_{\text{rad}} = -0.5833^\circ \times \frac{\pi}{180} \approx -0.01018

在代码中，我们看到WGS84 椭球常数：

a = 6378137\,\text{m}, \quad b = 6356732\,\text{m}, \quad e = \sqrt{1 - \left(\frac{b}{a}\right)^2} \approx 0.0818192

代码调用

%--------------------------------------
% 1) 定义椭球坐标（弧度）
phi_rad    = deg2rad(44.8);         % 44°48'
lambda_rad = deg2rad(-0.5833333);   % -0°35'
h          = 0;

% 2) 打包为 (lat, lon, alt)
llh = [phi_rad; lambda_rad; h];

% 3) 调用函数
xyzECEF = llh2xyz(llh);

% 查看结果
disp('P0 在 ECEF 中的坐标为 [X; Y; Z] = ');
disp(xyzECEF);

或者如下代码:

% 定义经纬度和高度
latitude_deg = 44 + 48/60; % 纬度 44°48' 北
longitude_deg = -(0 + 35/60); % 经度 0°35' 西 (转换为负值)
altitude = 0; % 海拔高度，假设为 0 米

% 将经纬度从度转换为弧度
latitude_rad = deg2rad(latitude_deg); % 纬度转换为弧度
longitude_rad = deg2rad(longitude_deg); % 经度转换为弧度

% 创建包含纬度、经度和高度的向量
llh = [latitude_rad, longitude_rad, altitude];

% 调用 llh2xyz 函数计算 ECEF 坐标
xyz = llh2xyz(llh);

% 输出结果
disp('ECEF 坐标:');
disp(	['X: ', num2str(xyz(1)), ' m']);
disp(['Y: ', num2str(xyz(2)), ' m']);
disp(['Z: ', num2str(xyz(3)), ' m']);

得到结果ECEF 坐标:
X: 4533051.770408139 m
Y: -46152.987854075 m
Z: 4471583.372126530 m

理论计算过程为：

已知弧度

\phi = 44.8^\circ \approx 0.7819\,(\mathrm{rad}), \quad \lambda = -0.5833^\circ \approx -0.01018\,(\mathrm{rad})

并且

\sin\phi \approx 0.7040

因此可以计算得到卯酉圈半径

N(\phi) = \frac{a}{\sqrt{1 - e^2 \sin^2\phi}} \approx \frac{6378137}{\sqrt{\,1 - 0.0066944 \times (0.7040)^2}} \approx 6{,}388{,}995\,\mathrm{m}

在 WGS84 大地坐标 (\phi,\,\lambda,\,h) 转到 ECEF 笛卡尔坐标 (X,Y,Z)的公式是

\begin{cases} X = \left[N(\phi) + h\right]\cos\phi\cos\lambda\\ Y = \left[N(\phi) + h\right]\cos\phi\sin\lambda\\ Z = \left[N(\phi)(1-e^2) + h\right]\sin\phi \end{cases}

最终得到 ECEF 坐标：

X \approx 6{,}388{,}995 \times 0.7102 \times 0.99995 \approx 4{,}536{,}900 \,\mathrm{m}

Y \approx 6{,}388{,}995 \times 0.7102 \times (-0.01018) \approx -46{,}200 \,\mathrm{m}

Z \approx 6{,}388{,}995 \times (1 - 0.0066944) \times 0.7040 \approx 4{,}469{,}000 \,\mathrm{m}

可见代码计算结果和理论计算结果非常近似

通过应用最小二乘法算法，使用提供的GPS数据找出车辆在塔朗斯市的轨迹（在参考系1和参考系2中）

%% Question 3 
load('donnees_GPS_TP.mat');  % 加载GPS卫星数据文件 

cla = cos(latitude); % 计算纬度的余弦
sla = sin(latitude); % 计算纬度的正弦

clon = cos(longitude); % 计算经度的余弦
slon = sin(longitude); % 计算经度的正弦

M = [-sla*clon, -slon, -cla*clon;  % 构建旋转矩阵M，用于从ECEF转换到局部NED坐标系
     -slon*sla, clon, -cla*slon; 
     cla, 0, -sla];

T = size(XYZsat,3);

for t = 1:T  % 遍历每个时间点
    ind = find(~isnan(PRN(:, t)));  % 找出当前时刻 t 所有非 NaN 测量的卫星索引
    Y = PRN(ind, t);  % 取出对应的伪距测量值
    nb_mes = length(ind); % 当前测量值

    h = zeros(nb_mes, 1);  % 存放每条测量对应的模型值 h(i)
    H = zeros(nb_mes, 4);  % 初始化测量矩阵H，用于后续线性化

    for i = 1:nb_mes
        Xref = [xyz_P0(1); xyz_P0(2); xyz_P0(3); 0];  % (x0,y0,z0,b0)
        %----(1) 计算当前卫星到参考点的几何距离-----
        dx = Xref(1) - XYZsat(ind(i),1,t);
        dy = Xref(2) - XYZsat(ind(i),2,t);
        dz = Xref(3) - XYZsat(ind(i),3,t);
        range_i = sqrt(dx^2 + dy^2 + dz^2);
        
        %----(2) 模型值 h^i = range_i + b_est ----
        h(i) = range_i + Xref(4);
        
        %----(3) H 矩阵的第 i 行(对x,y,z,b的偏导)----
        % partial wrt x
        H(i,1) = dx / range_i;
        % partial wrt y
        H(i,2) = dy / range_i;
        % partial wrt z
        H(i,3) = dz / range_i;
        % partial wrt b
        H(i,4) = 1;
    end

    DOP(t) = sqrt(trace(inv(H' * H)));  % 计算精度稀释因子DOP

    % 构造观测残差z，将观测值与预测值和初始估计值差计算出来
    z = Y - h + H * [xyz_P0'; 0];

    % Application de la méthode des moindres carrés
    % 使用最小二乘法估计位置和偏差，结果存储在Xest中
    Xest(:, t) = pinv(H) * z;
    
    % 将估计的ECEF坐标转换为局部NED坐标
    Xloc_est(:, t) = inv(M) * (Xest(1:3, t) - xyz_P0');
end

% 绘制3D估计轨迹
figure, plot3(Xest(1, :), Xest(2, :), Xest(3, :));
title('车辆在1号坐标系（ECEF）中估计轨迹的3D图形')

% 绘制局部NED坐标系下的真实轨迹和估计轨迹
figure
plot(Xloc(1, :), Xloc(2, :), 'r'), hold on  % 绘制真实轨迹
plot(Xloc_est(1, :), Xloc_est(2, :), 'b')  % 绘制估计轨迹
title('车辆在2号坐标系（本地NED）中的实际轨迹和估计轨迹。')
legend('真实轨迹', '估计轨迹')  % 添加图例

% 绘制DOP随时间的变化
figure
plot(DOP)
title('随时间变化的精度稀释（DOP）图')
xlabel('时间')
ylabel('精度稀释（DOP）')

我们下面对这部分进行详细解释

算法流程

读入数据、去除无效测量：

对每个历元 t，用 find(~isnan(PRN(:, t))) 筛选出当前有效卫星测量。

选取线性化参考点 X_{ref}

\mathbf{X}_{\text{ref}} \;=\; \bigl( x_{0},\, y_{0},\, z_{0},\, b_{0} \bigr)^T = \bigl( \texttt{xyz\_P0(1)},\, \texttt{xyz\_P0(2)},\, \texttt{xyz\_P0(3)},\,0 \bigr)^T

其中 (x_0, y_0, z_0) 是已知的参考坐标，b_0 初值设为 0。

构建模型值 h^i 与梯度(雅可比) H(i,\cdot)

先计算 h^i

对第 i 颗可见卫星，先计算

\texttt{dx} = x_{\text{ref}} - x_{\mathrm{sat}}^i\quad \texttt{dy} = y_{\text{ref}} - y_{\mathrm{sat}}^i\quad \texttt{dz} = z_{\text{ref}} - z_{\mathrm{sat}}^i

\texttt{range\_i} = \sqrt{\texttt{dx}^2 + \texttt{dy}^2 + \texttt{dz}^2}

因此伪距为

h^i(x,y,z,b) \;=\; \sqrt{(x - x_{\mathrm{sat}}^i)^2 + (y - y_{\mathrm{sat}}^i)^2 + (z - z_{\mathrm{sat}}^i)^2} \;+\; b

对应代码

h(i) = range_i + Xref(4);

接下来我们的任务是找到测量矩阵 H(i,1\!:\!4)

偏导数的计算过程

每个卫星的理论伪距可以写成：

h^i(x, y, z, b) \;=\; \sqrt{(x - x_{\mathrm{sat}}^i)^2 + (y - y_{\mathrm{sat}}^i)^2 + (z - z_{\mathrm{sat}}^i)^2} \;+\; b

其中 x, y, z 是接收机的位置坐标，b 是接收机时钟偏移，\bigl(x_{\mathrm{sat}}^i,\; y_{\mathrm{sat}}^i,\; z_{\mathrm{sat}}^i\bigr) 表示第 i 颗卫星的坐标。

令

r^i \;=\; \sqrt{(x - x_{\mathrm{sat}}^i)^2 \;+\; (y - y_{\mathrm{sat}}^i)^2 \;+\; (z - z_{\mathrm{sat}}^i)^2}

则有

h^i(x, y, z, b) \;=\; r^i \;+\; b

测量矩阵的定义如下

H = \begin{pmatrix} \frac{\partial h^1}{\partial x} & \frac{\partial h^1}{\partial y} & \frac{\partial h^1}{\partial z} &1 \\ \frac{\partial h^2}{\partial x} & \frac{\partial h^2}{\partial y} & \frac{\partial h^2}{\partial z} &1\\ \vdots & \vdots & \vdots & \vdots\\ \end{pmatrix}

我们对这些分量依次求偏导

\frac{\partial h^i}{\partial x} = \frac{\partial}{\partial x}(r^i + b) = \frac{\partial r^i}{\partial x} = \frac{x - x_{\mathrm{sat}}^i}{r^i}

\frac{\partial h^i}{\partial y}= \frac{\partial}{\partial y}(r^i + b) = \frac{y - y_{\mathrm{sat}}^i}{r^i}

\frac{\partial h^i}{\partial z}= \frac{\partial}{\partial z}(r^i + b) = \frac{z - z_{\mathrm{sat}}^i}{r^i}

\frac{\partial h^i}{\partial b}= \frac{\partial}{\partial b}(r^i + b) = 1

综上所述，偏导在代码中的计算公式为:

\frac{\partial h^i}{\partial x} = \frac{\texttt{dx}}{\texttt{range\_i}} \quad \frac{\partial h^i}{\partial y} = \frac{\texttt{dy}}{\texttt{range\_i}} \quad \frac{\partial h^i}{\partial z} = \frac{\texttt{dz}}{\texttt{range\_i}} \quad \frac{\partial h^i}{\partial b} = 1

对应代码如下

%----(3) H 矩阵的第 i 行(对x,y,z,b的偏导)----
% partial wrt x
H(i,1) = dx / range_i;
% partial wrt y
H(i,2) = dy / range_i;
% partial wrt z
H(i,3) = dz / range_i;
% partial wrt b
H(i,4) = 1;

要注意这里 dx 并不代表数学导数公式中的 dx ，而是在 x 坐标轴上的差

构造残差向量 \mathbf{z}：

如果问题是线性的，那么很简单直接如下所示

Z = H X + \varepsilon

但是我们的情况是非线性的，因此有:

y = h(x) + \varepsilon

y = h(x_0) + H(x - x_0) + \varepsilon

\underbrace{y - h(x_0) + H x_0}_{Z} = H x + \varepsilon

其中 x_0 是城市中心点，y 是GPS测量值，是PRN的一个列向量

在代码里等效实现为：

z = Y - h + H * [xyz_P0'; 0];

最小二乘解算 \hat{\mathbf{X}}：

\hat{\mathbf{X}} = \bigl(H^T H\bigr)^{-1} H^T \,\mathbf{z}

在代码里就是

Xest(:, t) = pinv(H) * z;

这得到估计的接收机位置 (\hat{x}, \hat{y}, \hat{z}) 和时钟偏移 \hat{b}。

计算精度稀释因子 DOP：

\text{DOP} = \sqrt{\mathrm{trace}\!\Bigl(\bigl(H^T H\bigr)^{-1}\Bigr)}

在代码中：

DOP(t) = sqrt(trace(inv(H'*H)));

结果显示：

修改提供的GPS数据以模拟干扰（增加测量噪声的方差）和多路径效应（在一个或多个GPS测量中引入偏差）。研究算法对这些扰动的鲁棒性，特别是可以研究均方误差随误差幅度的变化。
从公式(7)和(8)推导公式(7)和(9)的关系。

位置估计误差 (7) ：

\Delta X \;=\; \hat{X} - X \;=\; (H_t^T H_t)^{-1} H_t^T \,w(t) \tag{7}

误差协方差矩阵及其迹 (8) ：

\Sigma_{\Delta X \Delta X} \;=\; \mathbb{E}\!\bigl[(X - \hat{X})(X - \hat{X})^T\bigr] \quad

\mathbb{E}\!\bigl[\|X - \hat{X}\|^2\bigr] \;=\; \mathrm{Trace}\!\bigl(\Sigma_{\Delta X \Delta X}\bigr) \tag{8}

几何精度因子 DOP (9) ：

\mathrm{DOP} \;=\; \sqrt{\, \mathrm{Trace}\!\bigl( (H_t^T H_t)^{-1} \bigr) } \tag{9}

从 (7) 推导位置误差协方差矩阵 \Sigma_{\Delta X \Delta X}

由 (7) 可知，位置估计误差向量满足

\Delta X \;=\; \hat{X} - X \;=\; (H_t^T H_t)^{-1} \,H_t^T \; w(t)

估计误差的协方差矩阵为:

\Sigma_{\Delta X \Delta X} = \mathbb{E}\bigl[ (\hat{X} - X)(\hat{X} - X)^T \bigr]

在噪声 w(t) 服从零均值、协方差为 \sigma^2 I 的假设下，估计误差的协方差矩阵满足：

\Sigma_{\Delta X \Delta X} \;=\; \mathbb{E}\bigl[\Delta X \,\Delta X^T\bigr] \;=\; \bigl(H_t^T H_t\bigr)^{-1} H_t^T \;\mathbb{E}[\,w\,w^T\,]\;H_t \,\bigl(H_t^T H_t\bigr)^{-1}

由于 \mathbb{E}[\,w\,w^T\,] = \sigma^2 I，可进一步简化得到：

\Sigma_{\Delta X \Delta X} = (H_t^T H_t)^{-1}\,H_t^T\; (\sigma^2 I)\; H_t\,(H_t^T H_t)^{-1}

上式化简为

\Sigma_{\Delta X \Delta X} = \sigma^2\, (H_t^T H_t)^{-1}

由 (8) 知

\mathbb{E}\bigl[\|X - \hat{X}\|^2\bigr] \;=\; \mathrm{Trace}\bigl(\Sigma_{\Delta X \Delta X}\bigr)

将

\Sigma_{\Delta X \Delta X} = \sigma^2\,(H_t^T H_t)^{-1}

代入：

\mathbb{E}\bigl[\|X - \hat{X}\|^2\bigr] = \mathrm{Trace}\Bigl( \sigma^2\,(H_t^T H_t)^{-1} \Bigr) = \sigma^2 \; \mathrm{Trace}\bigl((H_t^T H_t)^{-1}\bigr)

因此，位置误差的均方根误差（RMSE, root mean square error）为

\sqrt{ \mathbb{E}\bigl[\|X - \hat{X}\|^2\bigr] } = \sqrt{\, \sigma^2 \,\mathrm{Trace}\bigl((H_t^T H_t)^{-1}\bigr) } = \sigma \;\sqrt{ \mathrm{Trace}\bigl((H_t^T H_t)^{-1}\bigr) }

由 (9) 定义可知

\mathrm{DOP} = \sqrt{ \mathrm{Trace}\bigl((H_t^T H_t)^{-1}\bigr) }

替换到上式中，于是立刻得到如下关系式：

\underbrace{ \sqrt{ \mathbb{E}\bigl[\|X - \hat{X}\|^2\bigr] } }_{\text{RMSE}} = \underbrace{\sigma}_{ \text{测量噪声标准差} } \times \underbrace{\mathrm{DOP}}_{ \sqrt{\mathrm{Trace}((H_t^T H_t)^{-1})} }

这说明，对于给定的噪声标准差 \sigma，最终的定位均方误差 (RMSE) 会受到卫星星座与接收机几何关系的放大/缩小，该几何放大系数正是 \mathrm{DOP}（精度稀释因子）。因此有结论：RMSE = DOP × 测量噪声标准差

GPS 定位误差不仅取决于测量噪声 \sigma ，还会受到几何结构的影响。如果某个时刻卫星分布的位置不正确，即 DOP 较大，则即使测量噪声比较小，最终RMSE也会很大，定位结果差

计算车辆轨迹上的DOP因子，并验证其与位置估计结果的合理性。还可以计算PDOP、VDOP和HDOP。对于后两个术语的计算，需要在本地参考系中进行分析。

DOP 定义如下:

\text{DOP}(t) \;=\; \sqrt{\, \mathrm{Trace} \Bigl( \bigl(H_t^T\,H_t\bigr)^{-1} \Bigr) }

对于 H_t 的计算我们在问题3的详细流程中已经指出，计算偏导即可

在时间序列上绘图

对 t = 1,2,\dots,T 做相同计算，得到 \{\text{DOP}(1),\text{DOP}(2),\dots\text{DOP}(T)\}。
绘制 DOP 随时间变化的曲线，观察雷达位置好坏（雷达位置都很好时 DOP 较小，有某个雷达位置很差时对应位置的 DOP 很大）。
将 DOP 曲线与估计轨迹偏差对比，可以发现二者呈正相关关系。

在代码中：

DOP(t) = sqrt(trace(inv(H' * H)));

然后 plot(DOP) 即可得到 DOP 随时间演变的图。

计算 PDOP, HDOP 和 VDOP

Position DOP 强调三维位置，即不包含时钟偏移
Horizontal DOP 强调水平面上的定位
Vertical DOP 强调垂直方向上的定位

他们都可以由矩阵 \bigl(H_t^T H_t\bigr)^{-1} 的相应行/列分块提取后得到。

PDOP 只考虑 位置坐标 (x, y, z) 对噪声的放大作用，忽略时钟偏移 b 。在四维矩阵 \widetilde{H}_t 中，我们对其左上 3 \times 3 部分(对应于 x,\,y,\,z)取迹，然后开方，即可得到：

PDOP(t) = \sqrt{ \widetilde{H}_t(1,1) + \widetilde{H}_t(2,2) + \widetilde{H}_t(3,3) }

在本地 NED 坐标系中的处理

想要得到更直观的 HDOP（水平）和 VDOP（垂直），会在 本地坐标系 下进行计算：

转换测量矩阵
- 将卫星和接收机的 ECEF 坐标转换到 NED 坐标系，然后线性化得到 H_t^\text{(NED)}
逆协方差矩阵

\tilde{H}_t^\text{(NED)} = \bigl( (H_t^\text{(NED)})^T \,H_t^\text{(NED)} \bigr)^{-1}
计算 HDOP, VDOP
- HDOP：水平平面 (N,E)：
  
  \mathrm{HDOP} \;=\; \sqrt{ \tilde{H}_t^\text{(NED)}(1,1) \;+\; \tilde{H}_t^\text{(NED)}(2,2) }
- VDOP：垂直方向 (D)：
  
  \mathrm{VDOP} \;=\; \sqrt{ \tilde{H}_t^\text{(NED)}(3,3) }

菜单

分享

GPS 导航实验报告：伪距定位与算法实现

1. 研究报告背景

2. 回顾：利用最小二乘法计算位置

2.1 最小二乘法算法

2.2 GPS中的应用

3. 精度稀释因子 (DOP)

4. 提供的数据描述

附录

算法流程

构建模型值 h^i 与梯度(雅可比) H(i,\cdot)

计算 PDOP, HDOP 和 VDOP

评论

优化理论（一）：导论、希尔伯特空间与凸性

Hello Halo

延迟系统稳定化的谱方法（研讨会）

置信函数理论在图像处理与多源融合中的应用

信念函数理论导论（研讨会报告）

心血管模型比较与 HRV 相干性分析（Master ATSI 团队项目）

医学信号处理 TP：相干函数性质证明与脑连接性实验

医学信号处理 TP：第 2 部分问题解答（LMS 自适应噪声消除）

医学信号处理 TP：自适应滤波去眼部伪差、ICA 与脑连接性（完整解答）

医学信号处理 TP（ENSEIRB-MATMECA 2023–2024）：实践工作题目