伪距单点定位推导+DOP值分析（附matlab仿真代码）

伪距单点定位公式推导、DOP值分析与matlab仿真分析

shannanshouyin

1144人浏览 · 2025-03-18 09:59:37

shannanshouyin · 2025-03-18 09:59:37 发布

一、伪距单点定位介绍

伪距单点定位是卫星导航定位系统中一种简单直接的定位方式。利用卫星发射的测距码信号，接收机通过测量信号到达的传播时间乘以光速得到伪距观测值。由于卫星钟、接收机钟的误差以及信号在电离层、对流层中的延迟等影响，该距离不等于卫星到接收机的真实几何距离，故称为伪距。根据卫星星历以及一台 GNSS 接收机在某一时刻测得四颗以上卫星的伪距及已知的卫星位置，采用空间后方交会的方法求定接收机天线所在点的三维坐标，如图1所示。

伪距观测方程含有四个未知量，分别是接收机位置三维坐标 $(x,y,z)$ 以及接收机钟差。由于伪距观测方程为非线性方程，通常需要对其通过泰勒级数展开进行线性化，然后在有n≥4颗可见卫星,或者累计可见卫星n≥4的条件下，根据最小二乘法进行求解。一般通过不断迭代，直至坐标变化值小于阈值，得到接收机的精确坐标，后续将对伪距单点定位进行详细的公式推导。

伪距单点定位的优点是定位速度快，无多值性问题，一台接收机即可完成独立定位，外业观测组织方便，数据处理简单，还可作为载波相位测量中解算整周模糊度的辅助资料；缺点是精度不高，一般只能达到十几米或几十米甚至更差的精度。其常用于对定位精度要求不高的场景，如普通的导航应用，为用户提供大致的位置信息。在一些需要快速获取位置但精度要求相对较低的领域，如车辆导航、个人位置服务等方面有广泛应用。也可作为其他高精度定位方法的初始定位或辅助定位手段。

二、伪距观测模型建立

在已知信号从卫星发出的时刻和被接收机接收的时刻，以及信号传播速度的情况下，可列出如下基本伪距方程：

$\rho=c\bullet(t_u-t^s)\, \, \, \, \, \, \, \, (1)$

然而在实际测量中， $t_u$ 和 $t^s$ 存在误差，故 $\rho$ 并非卫星与接收机之间的真实几何距离，故称为伪距。假设卫星和接收机之间的真实几何距离为 $r$ ，接收机位置矢量为 $\mathbf{x}_\mathbf{u}=\ \left[x_u,y_u,\left.z_u\right]\right.^T$ ，卫星位置矢量为 $\mathbf{x}^\mathbf{s}=\left[x^s,y^s,\left.z^s\right]\right.^T$ 接收机钟差为 $\delta_{t_u}$ ,卫星的钟差为 $\delta_{t^s}$ ， $I$ 和 $T$ 分别为电离层和对流层延时，其他误差为 $\varepsilon$ ，有以下方程：

$r=c\bullet[\left(t_u-\delta_{t_u}\right)-\left(t^s-\delta_{t^s}\right)-I-T-\varepsilon]\, \, \, \, \, \, \, \, (2)$

$r=\sqrt{{{(x}_u-x^s)}^2+{{(y}_u-y^s)}^2+{{(z}_u-z^s)}^2}\, \, \, \, \, \, \, \, (3)$

由（1）（2）（3）联立得：

$\sqrt{{{(x}_u-x^s)}^2+{{(y}_u-y^s)}^2+{{(z}_u-z^s)}^2}+c\bullet\delta_{t_u}=\\\rho+c\bullet\delta_{t^s}-c\bullet I-c\bullet T-c\bullet\varepsilon\, \, \, \, \, \, \, \, (4)$

式（4）即为伪距测量的完整观测方程。

在三维空间内，以卫星为原点，卫星到地面接收机的距离为半径构成的球面上处处伪距值相等，此球面称为等伪距球面，其与地球面相交可获得等伪距圆。如图2所示。

在 $t_1$ 时刻,设卫星位置为 $\mathbf{x}^\mathbf{s}=\left[x^s,y^s,\left.z^s\right]\right.^T$ ，接收机位置为 $\mathbf{x}_\mathbf{u}=\left[x,y,\left.z\right]\right.^T$ ，二者相对位置矢量为 $\mathbf{x}_\mathbf{u}^\mathbf{s}=\mathbf{x}^\mathbf{s}-\mathbf{x}_\mathbf{u}=\left[x^s\right.-x,y^s-y,z^s-\left.z\right]$ ， $\delta_{t_u}$ 为接收机的钟差，将伪距观测值、卫星钟差、电离层延迟、对流层延迟等已知信息统一写作 $\rho^s=\rho+c\bullet\delta_{t^s}-c\bullet I-c\bullet T-c\bullet\varepsilon$ ，由（4）式可知,卫星的伪距观测方程可简化为:

$\rho^s\left(t_1\right)=\sqrt{{(x^s-x)}^2+{(y^s-y)}^2+{(z^s-z)}^2}+\delta_{t_u}\, \, \, \, \, \, \, \, (5)$

对于地面接收机，卫星位置可由星历提供，伪距测量值可由接收机测量获得，此时方程（5）仅存在接收机位置 $\mathbf{x}_\mathbf{u}=\left[x,y,\left.z\right]\right.^T$ 和接收机钟差 $\delta_{t_u}$ 四个未知数,可组成解向量 $\mathbf{u}=[x,y,z,\delta_{t_u}]^T$ ，当可同时观测到三颗卫星时，三颗卫星的地面等伪距圆交汇点即可确定接收机的唯一位置，如图3所示。由于存在四个未知量，在同时可观测四颗及以上卫星时即可求解方程组全部未知数。

在有n≥4颗可见卫星,或者累计可见卫星n≥4的条件下，可列出如下伪距定位方程：

$\left\{\begin{matrix}\rho^1\left(\mathbf{u}\right)=\sqrt{\left(x^1-x\right)^2+\left(y^1-y\right)^2+\left(z^1-z\right)^2}+\delta_{t_u} \\ \rho^2\left(\mathbf{u}\right)=\sqrt{\left(x^2-x\right)^2+\left(y^2-y\right)^2+\left(z^2-z\right)^2}+\delta_{t_u} \\ \cdots \\ \rho^n\left(\mathbf{u}\right)=\sqrt{\left(x^n-x\right)^2+\left(y^n-y\right)^2+\left(z^n-z\right)^2}+\delta_{t_u} \end{matrix}\right.\, \, \, \, \, \, \, \, (6)$

三、伪距定位方程组解算

对于伪距定位方程，假设用户初始坐标为 $\mathbf{x}_\mathbf{0}=\left[x_0,y_0,z_0\right]^{T}$ ,接收机初始钟差为 $\delta _{t_0}$ ,组成初始解为 $\mathbf{u}_\mathbf{0}=\left[x_0,y_0,z_0,\delta_{t_0}\right]^{T}$ ，可将式（5）进行泰勒一阶展开线性化为下式：

$\rho^s\left(\mathbf{u}\right)=\rho^s\left(\mathbf{u}_\mathbf{0}\right)+\left.\frac{\partial f}{\partial x}\right|_{x=x_0}\left(x-x_0\right)+\left.\frac{\partial f}{\partial y}\right|_{y=y_0}\left(y-y_0\right)\\+\left.\frac{\partial f}{\partial z}\right|_{z=z_0}\left(z-z_0\right)+\delta_{t_u}-\delta_{t_0}\, \, \, \, \, \, \, \, (7)$

其中，

$\left.\frac{\partial f}{\partial x}\right|_{x=x_0}=\frac{x_0-x^s}{\sqrt{\left(x^s-x_0\right)^2+\left(y^s-y_0\right)^2+\left(z^s-z_0\right)^2}}\\\left.\frac{\partial f}{\partial y}\right|_{y=y_0}=\frac{y_0-y^s}{\sqrt{\left(x^s-x_0\right)^2+\left(y^s-y_0\right)^2+\left(z^s-z_0\right)^2}}\\\left.\frac{\partial f}{\partial z}\right|_{z=z_0}=\frac{z_0-z^s}{\sqrt{\left(x^s-x_0\right)^2+\left(y^s-y_0\right)^2+\left(z^s-z_0\right)^2}}\, \, \, \, \, \, \, \, (8)$

将式（6）每一个方程进行如式（7）的线性变化，可转化为矩阵表达式：

$\mathbf{G}\cdot \ \Delta \mathbf{x_{k-1}}=\Delta\boldsymbol{\rho}\, \, \, \, \, \, \, \, (9)$

其中，

$\mathbf{G}=\left[\begin{matrix}I_x^{\left(1\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)&I_y^{\left(1\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)&I_z^{\left(1\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)&1\\I_x^{\left(2\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)&I_y^{\left(2\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)&I_z^{\left(2\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)&1\\\cdots&\cdots&\cdots&\cdots\\I_x^{\left(n\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)&I_y^{\left(n\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)&I_z^{\left(n\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)&1\\\end{matrix}\right]\, \, \, \, \, \, \, \, (10)$

$\Delta \mathbf{x_{k-1}}=\begin{bmatrix}x_{k}-x_{k-1} \\ y_{k}-y_{k-1} \\ z_{k}-z_{k-1} \\ \delta _{t_k}-\delta _{t_{k-1}} \end{bmatrix}\, \, \, \, \, \, \, \, (11)$

$\Delta\boldsymbol{\rho} = \begin{bmatrix} \rho^{1}(\boldsymbol{u}) - \rho^{1}(\boldsymbol{u}_{k - 1}) \\ \rho^{2}(\boldsymbol{u}) - \rho^{2}(\boldsymbol{u}_{k - 1}) \\ \cdots \\ \rho^{n}(\boldsymbol{u}) - \rho^{n}(\boldsymbol{u}_{k - 1}) \end{bmatrix}\, \, \, \, \, \, \, \, (12)$

其中， $k$ 为迭代次数， $\mathbf{u}_{\mathbf{k}-\mathbf{1}}$ 为第 $k-1$ 次迭代解算 $\mathbf{u}$ 的估计值， $\Delta\boldsymbol{\rho}$ 为接收机接收卫星信号的伪距值与第 $k-1$ 次迭代伪距值的差值向量， $\mathbf{G}$ 为雅可比矩阵,由式（8）可知，其内部参数如下式：

$I_x^{\left(s\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)=\frac{x_{k-1}-x_k^s}{\sqrt{{(x_k^s-x_{k-1})}^2+{(y_k^s-y_{k-1})}^2+{(z_k^s-z_{k-1})}^2}}\\ I_y^{\left(s\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)=\frac{y_{k-1}-y_k^s}{\sqrt{{(x_k^s-x_{k-1})}^2+{(y_k^s-y_{k-1})}^2+{(z_k^s-z_{k-1})}^2}}\\ I_z^{\left(s\right)}\left(\mathbf{x}_{\mathbf{k}-\mathbf{1}}\right)=\frac{z_{k-1}-z_k^s}{\sqrt{{(x_k^s-x_{k-1})}^2+{(y_k^s-y_{k-1})}^2+{(z_k^s-z_{k-1})}^2}}\, \, \, \, \, \, \, \, (13)$

由式（13）可知，矩阵 $\mathbf{G}$ 只与各颗卫星相对于接收机的几何位置相关。

对于式（9），可根据最小二乘法原理求解 $\Delta \mathbf{x}$ ：

$\Delta\boldsymbol{x}_{k - 1} = (\boldsymbol{G}^{T}\boldsymbol{G})^{-1}\boldsymbol{G}^{T} \cdot \Delta\boldsymbol{\rho}\, \, \, \, \, \, \, \, (14)$

关于线性方程组的最小二乘法解算方法可以看博主的另一篇文章：

最小二乘法求解线性方程组推导（行空间、列空间、投影矩阵）-CSDN博客

将解算出的 $\Delta\boldsymbol{x}_{k - 1}$ 对上一次迭代的估计解 $\boldsymbol{x}_{k - 1}$ 进行修正，则更新后的估计解为 $\mathbf{x_k}=\mathbf{x_{k-1}}+\Delta \mathbf{x_{k-1}}$ ，并带入下一次迭代运算，直到 $\Delta \mathbf{x_{k-1}}$ 小于设定的阈值，则退出迭代，将此时的 $\mathbf{x_k}$ 作为最终的用户三维坐标解及接收机的钟差解，算法流程图如图4所示。

四、伪距雅可比矩阵dop值分析

考虑测量误差后的线性化矩阵，将（9）改写为下式：

$\mathbf{G}\bullet\begin{bmatrix}\Delta x+\varepsilon _x \\ \Delta y+\varepsilon _y \\ \Delta z+\varepsilon _z \\ \Delta \delta _t+\varepsilon _{\delta _t} \end{bmatrix}=\Delta \rho +\varepsilon _\rho \, \, \, \, \, \, \, \, (15)$

提去式（15）中的误差项目得下式：

$\mathbf{G}\bullet\begin{bmatrix}\varepsilon _x \\ \varepsilon _y \\ \varepsilon _z \\ \varepsilon _{\delta _t} \end{bmatrix}=\varepsilon _\rho \, \, \, \, \, \, \, \, (15)$

通过最小二乘法求解（15）得：

$\left[\begin{matrix}\varepsilon_x\\\varepsilon_y\\\varepsilon_z\\\varepsilon_{\delta_t}\\\end{matrix}\right]={(\mathbf{G}^\mathbf{T}\mathbf{G})}^{-1}\mathbf{G}^\mathbf{T}\bullet\mathbf{\varepsilon}_\mathbf{\rho }\, \, \, \, \, \, \, \, (16)$

假设各卫星测量误差为正态分布，且各误差互不相关，则均值 $E\left[\mathbf{\varepsilon}_\mathbf{\rho }\right]=0$ ,方差 $Var\left[\mathbf{\varepsilon}_\mathbf{\rho }\right]=\sigma_{URE}^2$ ，可得误差的协方差矩阵为:

$Cov\left(\left[\begin{matrix}\varepsilon_x\\\varepsilon_y\\\varepsilon_z\\\varepsilon_{\delta_t}\\\end{matrix}\right]\right)=\left(\mathbf{G}^\mathbf{T}\mathbf{G}\right)^{-1}\sigma_{URE}^2=\mathbf{H}{\bullet\sigma}_{URE}^2=\\ \left[\begin{matrix}q_{xx}&\cdots&\cdots&\cdots\\\cdots&q_{yy}&\cdots&\cdots\\\cdots&\cdots&q_{zz}&\cdots\\\cdots&\cdots&\cdots&q_{tt}\\\end{matrix}\right]{\bullet\sigma}_{URE}^2=\left[\begin{matrix}\sigma_{xx}^2&\cdots&\cdots&\cdots\\\cdots&\sigma_{yy}^2&\cdots&\cdots\\\cdots&\cdots&\sigma_{zz}^2&\cdots\\\cdots&\cdots&\cdots&\sigma_{tt}^2\\\end{matrix}\right]\, \, \, \, \, \, \, \, (17)$

其中， $\mathbf{H}={(\mathbf{G}^\mathbf{T}\mathbf{G})}^{-1}$ 为对称的权系数矩阵。

由（17）可知，定位误差是测量误差被权系数矩阵放大的结果，而权系数矩阵只与卫星的几何构型分布有关，故定位结果误差的大小取决于测量误差和卫星几何构型分布。精度因子DOP值可看作某历元卫星几何构型对接收机误差的放大系数，在地心地固坐标系（ECEF）下，几何位置精度因子（Geometric Dilution of Precision，GDOP）和空间位置精度因子（Position Dilution of Precision，PDOP）如下式：

$GDOP=\sqrt{q_{xx}+q_{yy}+q_{zz}+q_{tt}},\\PDOP=\sqrt{q_{xx}+q_{yy}+q_{zz}}\, \, \, \, \, \, \, \, (18)$

在相同观测条件下，DOP值越小则定位的精度越高，DOP值的大小也可以用来评估卫星星座的定位性能。

五、伪距单点定位matlab仿真

假设在某历元使用一台北斗接收机同时观测5颗卫星，卫星的瞬时空间直角坐标如下表所示，接收机天线概略坐标为(-2441267.123, 4790213.231, 3419994.321)，其中综合改正包括卫星钟差、对流层延迟误差、电离层延迟误差等，利用以下实例计算修正后的接收机坐标。

伪距单点定位函数

%% 伪距单点定位函数
function [User_Position_Range,G] = Least_square_Position_Range...
(Satellite_Position,Range,Initial_Position,Error_Correction)
% Input:
% Satellite_Position卫星坐标
% Range伪距观测值
% Initial_Position初值
% Error_Correction误差修正数
% Output:
% User_Position_Range接收机坐标
% G雅可比矩阵
%% 初始化
Number_of_Iterations = 20;%迭代次数
convergence_threshold = 1e-6;%收敛阈值
P = Satellite_Position;
Number_of_Satellites = size(Satellite_Position, 2);
User_Position_Range = [Initial_Position;0];
G = zeros(Number_of_Satellites,4);%伪距雅可比矩阵
delta_Range=zeros(Number_of_Satellites,1);
for j=1:Number_of_Iterations
for i=1:Number_of_Satellites
%---------------- 第一次迭代初始化变量 -------------------------
Satellite_Rot=P(:,i);
%--------------------应用更正----------------------------------
range = norm(Satellite_Rot-User_Position_Range(1:3),'fro');
delta_Range(i)=Range(i) + Error_Correction(i) - range - User_Position_Range(4);
%--------------- 构造雅可比矩阵 --------------------------------
G(i,:)=[(User_Position_Range(1)-Satellite_Rot(1))/range,...
(User_Position_Range(2)-Satellite_Rot(2))/range,...
(User_Position_Range(3)-Satellite_Rot(3))/range,1];
end
%--- Find position update ---------------------------------------------
delta_X_Range = (G'*G)\(G'* delta_Range);
%--- Apply position update --------------------------------------------
User_Position_Range = User_Position_Range + delta_X_Range;

if norm(delta_X_Range) < convergence_threshold
disp(['Converged after ', num2str(j), ' iterations.']);
break;
end
end
end

DOP值计算函数

function [GDOP, PDOP] = DOP_calculate(G)
% 计算 G 矩阵的伪逆
Q = inv(G' * G);
GDOP = sqrt(trace(Q));
PDOP = sqrt(trace(Q(1:3, 1:3)));
end

主函数

% 初始解
Initial_Position = [-2441267.123; 4790213.231; 3419994.321];
% 卫星钟差、对流层延迟误差、电离层延迟误差等
Error_Correction = [-24286.492 -43822.577 -116967.755 -7834.145 30330.506];
% 伪距观测值
Range = [23244182.861 23934824.154 24181945.803 22957572.28 22385541.968];
% 卫星坐标（按列排列）
Satellite_Position = [13550285.883 -9754080.537 13615174.294 -20328139.067 -21641940.073;
18190574.884 21155869.384 7856984.053 508157.447 15217052.005;
13721537.673 -12506802.672 21398396.517 17172573.272 -1309766.601];

% 伪距单点定位求解
[User_Position_Range,G] = Least_square_Position_Range...
(Satellite_Position,Range,Initial_Position,Error_Correction);

% Dop值计算
[GDOP,PDOP] = DOP_calculate(G);

disp("伪距单点定位坐标：");
disp(User_Position_Range(1:3));
disp("GDOP值：");
disp(GDOP);
disp("PDOP值：");
disp(PDOP);

2048 AI社区

有“AI”的1024 = 2048，欢迎大家加入2048 AI社区

更多推荐

机器学习决策树-分类

2048 AI社区

软考中级-软件设计师 UML图详解（类图，对象图，用例图，序列图，通信图，状态图，活动图，构件图，部署图）

2048 AI社区

不懂编程本科毕业生手搓三个APP，用ChatGPT两年狂赚千万美金！

而现在更厉害的是，有了多模态能力的ChatGPT，直接上传设计图片、截图，并告诉它「我要做成这个样子，这些按钮要实现xxx功能，帮我把代码写出来」。ChatGPT发布那天，Walter Isaacson在课上也是表现的异常热情激动，他的眼睛闪烁着兴奋的光芒，就像孩子看到了心爱的玩具一样。也就是说，假设你能做对80%的决策，另一个人也能做对80%，并且你们的专长领域不重叠，那么当你们联手的时候，正确