让GNSSRTK不再难【第二天-第3部分】

第11讲定位方程构建以及最小二乘

11.1 定位方程重构

历史讲中我们已经初步构建了单点定位的先验残差：

$p_i^s = P_i^s - \sqrt{(X^s - X_0)^2 + (Y^s - Y_0)^2 + (Z^s - Z_0)^2} + c\delta t^r - I_i^s - T_i^s - \epsilon_{P_i^s}$

其中：

$p_i^s$ 为残差，是观测伪距与计算伪距之间的差值。
$P_i^s$ 为卫星 $s$ 到接收机 $r$ 的伪距观测值。
$X^s, Y^s, Z^s)$ 为卫星 $s$ 在ECEF坐标系下的位置。
$X_0, Y_0, Z_0)$ 为接收机 $r$ 在ECEF坐标系下的近似位置。
$c\delta t^r$ 为接收机钟差的影响， $c$ 为光速。
$I_i^s$ 为电离层延迟。
$T_i^s$ 为对流层延迟。
$\epsilon_{P_i^s}$ 为伪距观测值噪声。

但是在上一讲中，又有两项改正，即地球自转和伪距码偏差。所以如果下沉到频率层面，残差计算要再次更新。

对于P1频点的伪距：

$p_{r,1}^s = P_{r,1}^s - \left( \sqrt{(X^s - X_0)^2 + (Y^s - Y_0)^2 + (Z^s - Z_0)^2} - \Delta \rho \right) + (c\delta t^s - T_{gd}) - I_r^s - T_r^s - \epsilon_{P}$

其中：

$p_{r,1}^s$ 为P1频点残差。
$P_{r,1}^s$ 为P1频点的伪距观测值。
$\Delta \rho$ 为地球自转引起的改正。
$c\delta t^s$ 为卫星钟差。
$T_{gd}$ 为码偏差改正值。
$I_r^s$ 为电离层延迟。
$T_r^s$ 为对流层延迟。
$\epsilon_{P}$ 为伪距观测值噪声。

对于P2频点：

$p_{r,2}^s = P_{r,2}^s - \left( \sqrt{(X^s - X_0)^2 + (Y^s - Y_0)^2 + (Z^s - Z_0)^2} - \Delta \rho \right) + (c\delta t^s - \gamma T_{gd}) - I_r^s - T_r^s - \epsilon_{P}$

其中 $\gamma$ 是频率因子，用于将P1频点的码偏差转化为P2频点的码偏差。 $\gamma$ 的定义为：

$\gamma = \frac{f_1^2}{f_2^2}$

其中 $f_1$ 和 $f_2$ 分别为P1频点和P2频点的频率。例如，对于GPS系统， $f_1 = 1575.42 \, MHz$ ， $f_2 = 1227.60 \, MHz$ 。计算得到的 $\gamma$ 为：

$\gamma = \left(\frac{1575.42}{1227.60}\right)^2 \approx 1.64694$

我们初步仅使用单一频点进行单点定位。

将第9讲公式复制到此处：

$\left[ \begin{matrix} p^1 \\ p^2 \\ p^3 \\ \vdots \\ p^n \\ \end{matrix} \right] = A \delta x$

其中：

$V$ 为残差向量，每个元素 $p^i$ 为第 $i$ 个观测值的残差。
$A$ 为设计矩阵或Jacobian矩阵，其元素为：
$\left[ \begin{matrix} l^1 & m^1 & n^1 & -1 \\ l^2 & m^2 & n^2 & -1 \\ l^3 & m^3 & n^3 & -1 \\ \vdots & \vdots & \vdots & \vdots \\ l^n & m^n & n^n & -1 \\ \end{matrix} \right]$
其中 $l^i, m^i, n^i$ 分别为第 $i$ 个观测值在 $X, Y, Z$ 方向的系数，-1 为钟差项的系数。
$\delta x$ 为待估状态量向量，包括接收机位置的改正值和钟差：
$\delta x = \left[ \begin{matrix} dx \\ dy \\ dz \\ c\delta t \\ \end{matrix} \right]$

即有以上公式：

$\delta x$

我们将 $V$ 称为先验残差阵， $A$ 为设计矩阵或者Jacobian矩阵， $\delta x$ 为待估状态量。

11.2 观测值权重

每个卫星的钟精度以及电离层模型修正后的误差都有差异，所以我们不能简单的将各个观测值等权处理。理论上来说，我们头顶的卫星穿过电离层区域时较短，且不容易受地面建筑物的遮挡，理论上来说观测值精度更高。

$P_i^s = \rho_i^s + c\delta t^s + c\delta t^r + I_i^s + T_i^s + \epsilon_{P_i^s}$

其中：

$P_i^s$ 为观测的伪距值。
$\rho_i^s$ 为卫星 $s$ 到接收机 $i$ 的真实伪距。
$c\delta t^s$ 为卫星钟差， $c$ 为光速。
$c\delta t^r$ 为接收机钟差。
$I_i^s$ 为电离层延迟。
$T_i^s$ 为对流层延迟。
$\epsilon_{P_i^s}$ 为伪距观测噪声。

所以对于公式中的伪距观测值 $\epsilon_{P_i^s}$ ，我们一般将其建模为随着卫星高度角降低而精度变差。

一般我们认为观测值的噪声符号正态分布，并与高度角的正弦成负相关。

$E(\epsilon_{P_i^s}) = 0$

$\sigma(\epsilon_{P_i^s}) = \frac{\sigma_0}{\sin(Elevation)}$

其中：

$\sigma_0$ 为标准的伪距噪声，一般设定为0.3m。
$El e v a t i o n$ 为卫星的高度角。

除观测值的噪声，轨钟、电离层模型、对流层模型等都会引入误差。

其中轨钟的误差，可由广播星历中的SV accuracy字段计算得到。

而大气误差无法量化，可以一个经验值。

将所有误差依据误差传播律定律，即其方差之和作为观测值的方差 $\sigma^2$ 。

公式：

$\sigma^2 = (\sigma(\epsilon_{P_i^s}))^2 + \sigma^2(orb) + \sigma^2(ion)$

解释：

$\sigma^2$ ：总的观测值方差，是各个误差分量方差的和。
$(\sigma(\epsilon_{P_i^s}))^2$ ：伪距观测值的噪声方差， $\sigma(\epsilon_{P_i^s})$ 是伪距观测噪声的标准差，其计算方式为 $\frac{\sigma_0}{\sin(Elevation)}$ ，其中 $\sigma_0$ 为标准伪距噪声，通常设定为0.3米， $El e v a t i o n$ 为卫星的高度角。
$\sigma^2(orb)$ ：轨道误差的方差。轨道误差是由于卫星轨道的不确定性引起的，可以从广播星历中的卫星位置精度字段（SV accuracy）中获得。
$\sigma^2(ion)$ ：电离层误差的方差。电离层误差是由于电离层对信号的折射和延迟引起的，通常可以通过电离层模型进行估算。

总结：

总观测值方差 $\sigma^2$ 是伪距观测噪声、电离层误差和轨道误差方差的和。每个误差分量的方差分别考虑了不同的误差来源，通过将这些误差分量进行加和，可以得到一个综合的观测值方差，用于权重矩阵的构建，从而在定位计算中进行加权处理，提高定位精度。

我们一般认为各个观测值之间不相关，所以将方差的倒数作为该观测值的权重。

$\left[ \begin{matrix} \frac{1}{\sigma_1^2} & 0 & 0 & \cdots & 0 \\ 0 & \frac{1}{\sigma_2^2} & 0 & \cdots & 0 \\ 0 & 0 & \frac{1}{\sigma_3^2} & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & \frac{1}{\sigma_n^2} \\ \end{matrix} \right]$

其中：

$\sigma_i$ 为第 $i$ 个观测值的标准差。
$P$ 为权重矩阵，其对角线元素为各观测值方差的倒数。

11.3 最小二乘

最小二乘就是为了解决观测值数目大于状态量数目的问题。其目标公式为

$V^T * P * V = min$

其中 $V$ 为残差阵， $V^T$ 为其转置矩阵， $P$ 为其权矩阵。如果认为各残差等权，那么权矩阵即为单位矩阵。初步我们认为观测值等权。

下面我们不加证明，直接给出最小二乘的状态量解

$\delta x = -(A^T PA)^{-1} * (A^T PV)$

通过上文中的公式

$X_r = X_0 + dx \\ Y_r = Y_0 + dy \\ Z_r = Z_0 + dz$

即可以计算得到最后的结果。

元素解释：

$V$ ：残差阵，包含了每个观测值与理论值之间的差值。
$V^T$ ：残差阵的转置矩阵。
$P$ ：权矩阵，用于给每个观测值分配权重。在初步假设下，各观测值等权， $P$ 为单位矩阵。
$A$ ：设计矩阵或雅可比矩阵，表示观测值对状态量的一阶偏导数。
$\delta x$ ：状态量的调整量，表示通过最小二乘计算得到的状态量修正。
$X_r, Y_r, Z_r$ ：接收机最终的坐标。
$X_0, Y_0, Z_0$ ：接收机初始的坐标。

该过程的核心是利用观测值和理论值的差异，通过加权最小二乘方法，对状态量进行修正，从而得到更准确的定位结果。

附加A：最小二乘状态解公式推导与举例

推导过程

为了推导最小二乘状态解公式，我们从最小二乘法的基本原理开始。

假设我们有 $n$ 个观测方程，每个观测方程表示为：

$P_i = f(X) + \epsilon_i$

其中， $P_i$ 是第 $i$ 个观测值， $f (X)$ 是状态量 $X$ 的函数， $\epsilon_i$ 是观测误差。

我们希望找到一个状态量 $X$ ，使得观测值与理论值之间的误差平方和最小化，即：

$\min \sum_{i=1}^n \epsilon_i^2$

线性化观测方程

为了方便计算，我们通常线性化这些观测方程。假设 $X$ 是状态变量的初值， $\delta X$ 是状态变量的修正量，那么我们可以对 $f (X)$ 进行一阶泰勒展开：

$\delta X) \approx f(X) + \frac{\partial f}{\partial X} \delta X$

将上述公式代入观测方程，得到线性化的观测方程：

$P_i \approx f(X) + \frac{\partial f}{\partial X} \delta X + \epsilon_i$

矩阵形式表示

引入设计矩阵 $A$ ，其元素为 $\frac{\partial f_i}{\partial X_j}$ ，我们可以将所有观测方程写成矩阵形式：

$\mathbf{P} = \mathbf{f}(X) + \mathbf{A} \delta X + \epsilon$

其中， $\mathbf{P}$ 是观测值向量， $\mathbf{f}(X)$ 是理论值向量， $\mathbf{A}$ 是设计矩阵， $\epsilon$ 是误差向量。

目标函数

为了使误差平方和最小化，我们定义目标函数：

$\epsilon^T \epsilon = (\mathbf{P} - \mathbf{f}(X) - \mathbf{A} \delta X)^T (\mathbf{P} - \mathbf{f}(X) - \mathbf{A} \delta X)$

其中：

$J$ 是目标函数，表示误差平方和。
$\epsilon$ 是误差向量。
$\mathbf{P}$ 是观测值向量。
$\mathbf{f}(X)$ 是理论值向量。
$\mathbf{A}$ 是设计矩阵。
$\delta X$ 是状态变量的修正量。

目标函数 $J$ 表示的是观测值 $\mathbf{P}$ 和理论值 $\mathbf{f}(X) + \mathbf{A} \delta X$ 之间的误差的平方和。通过最小化 $J$ ，我们可以找到使得观测值与理论值之间的误差最小的状态量修正量 $\delta X$ 。这个过程是通过对 $J$ 进行偏导数求解，并令其等于零来实现的。

通俗解释 $\epsilon^T \epsilon$ 的含义

在数学和几何中， $\epsilon^T \epsilon$ 具有以下含义：

数学上的含义：
- $\epsilon$ 是一个误差向量，它的每个元素表示一个观测值和理论值之间的差异。
- $\epsilon^T$ 是误差向量 $\epsilon$ 的转置向量。
- $\epsilon^T \epsilon$ 表示误差向量与其自身的内积（也叫点积）。
几何上的含义：
- 在几何上， $\epsilon^T \epsilon$ 表示误差向量的平方和。
- 从向量的角度看， $\epsilon^T \epsilon$ 实际上是误差向量的长度（范数）的平方。
通俗易懂的理解：
- 假设你有几个测量值和对应的理论值， $\epsilon$ 表示每个测量值与理论值的差异。
- $\epsilon^T \epsilon$ 就是将这些差异平方后相加的总和。
- 这表示所有测量误差的总量，是衡量测量精度的一种方式。

通过最小化 $\epsilon^T \epsilon$ ，我们实际上是在寻找一种方法，使得所有测量误差的总量尽可能小，从而得到最准确的结果。

求解目标函数

对目标函数 $J$ 求 $\delta X$ 的偏导数，并令其等于0，即：

$\frac{\partial J}{\partial \delta X} = -2 \mathbf{A}^T (\mathbf{P} - \mathbf{f}(X) - \mathbf{A} \delta X) = 0$

解此方程，得到最小二乘解：

$\mathbf{A}^T \mathbf{A} \delta X = \mathbf{A}^T (\mathbf{P} - \mathbf{f}(X))$

即：

$\delta X = (\mathbf{A}^T \mathbf{A})^{-1} \mathbf{A}^T (\mathbf{P} - \mathbf{f}(X))$

引入残差向量

为了进一步简化，我们引入残差向量 $\mathbf{V}$ ，其定义为：

$\mathbf{V} = \mathbf{P} - \mathbf{f}(X)$

因此，状态量修正量的最小二乘解可以表示为：

$\delta X = (\mathbf{A}^T \mathbf{A})^{-1} \mathbf{A}^T \mathbf{V}$

考虑权矩阵

考虑权矩阵 $\mathbf{P}$ ，当观测值权重不同的时候，可以修正上述公式为：

$\delta X = (\mathbf{A}^T \mathbf{P} \mathbf{A})^{-1} \mathbf{A}^T \mathbf{P} \mathbf{V}$

通过计算 $\delta X$ ，我们可以修正初值 $X$ ，得到更精确的状态量。

公式元素解释：

$\mathbf{A}$ ：设计矩阵，包含观测方程对状态量的一阶偏导数。
$\mathbf{P}$ ：观测值向量，包含所有的观测值。
$\mathbf{f}(X)$ ：理论值向量，包含所有观测方程在初始状态量 $X$ 下的计算值。
$\mathbf{V}$ ：残差向量，观测值与理论值之间的差值。
$\delta X$ ：状态量的修正量，通过最小二乘法计算得到。
$\mathbf{P}$ ：权矩阵，用于给观测值分配权重。

举例

假设我们有三个观测值和两个状态变量：

观测方程为：
$P_1 = X_1 + 2X_2 + \epsilon_1$

$P_2 = 3X_1 + 4X_2 + \epsilon_2$

$P_3 = 5X_1 + 6X_2 + \epsilon_3$

观测值为：
$\mathbf{P} = \begin{bmatrix} 2 \\ 8 \\ 12 \end{bmatrix}$

初始状态量为：
$\mathbf{X} = \begin{bmatrix} 1 \\ 1 \end{bmatrix}$

设计矩阵 $\mathbf{A}$ 为：
$\mathbf{A} = \begin{bmatrix} 1 & 2 \\ 3 & 4 \\ 5 & 6 \end{bmatrix}$

计算理论值向量 $\mathbf{f}(X)$

使用初始状态量 $\mathbf{X}$ 计算理论值向量 $\mathbf{f}(X)$ ：
$\mathbf{f}(X) = \mathbf{A} \mathbf{X} = \begin{bmatrix} 1 & 2 \\ 3 & 4 \\ 5 & 6 \end{bmatrix} \begin{bmatrix} 1 \\ 1 \end{bmatrix} = \begin{bmatrix} 1*1 + 2*1 \\ 3*1 + 4*1 \\ 5*1 + 6*1 \end{bmatrix} = \begin{bmatrix} 3 \\ 7 \\ 11 \end{bmatrix}$

计算残差向量 $\mathbf{V}$

残差向量 $\mathbf{V}$ 的计算公式为：
$\mathbf{V} = \mathbf{P} - \mathbf{f}(X)$
将观测值 $\mathbf{P}$ 和理论值 $\mathbf{f}(X)$ 带入：
$\mathbf{V} = \begin{bmatrix} 2 \\ 8 \\ 12 \end{bmatrix} - \begin{bmatrix} 3 \\ 7 \\ 11 \end{bmatrix} = \begin{bmatrix} -1 \\ 1 \\ 1 \end{bmatrix}$

计算 $\mathbf{A}^T \mathbf{A}$

$\mathbf{A}^T \mathbf{A} = \begin{bmatrix} 1 & 3 & 5 \\ 2 & 4 & 6 \end{bmatrix} \begin{bmatrix} 1 & 2 \\ 3 & 4 \\ 5 & 6 \end{bmatrix} = \begin{bmatrix} 1*1 + 3*3 + 5*5 & 1*2 + 3*4 + 5*6 \\ 2*1 + 4*3 + 6*5 & 2*2 + 4*4 + 6*6 \end{bmatrix} = \begin{bmatrix} 35 & 44 \\ 44 & 56 \end{bmatrix}$

计算 $\mathbf{A}^T \mathbf{V}$

$\mathbf{A}^T \mathbf{V} = \begin{bmatrix} 1 & 3 & 5 \\ 2 & 4 & 6 \end{bmatrix} \begin{bmatrix} -1 \\ 1 \\ 1 \end{bmatrix} = \begin{bmatrix} 1*-1 + 3*1 + 5*1 \\ 2*-1 + 4*1 + 6*1 \end{bmatrix} = \begin{bmatrix} 7 \\ 8 \end{bmatrix}$

求解 $\delta X$

$\delta X = (\mathbf{A}^T \mathbf{A})^{-1} \mathbf{A}^T \mathbf{V} = \begin{bmatrix} 35 & 44 \\ 44 & 56 \end{bmatrix}^{-1} \begin{bmatrix} 7 \\ 8 \end{bmatrix}$

计算 $(\mathbf{A}^T \mathbf{A})^{-1}$

$(\mathbf{A}^T \mathbf{A})^{-1} = \frac{1}{(35)(56) - (44)^2} \begin{bmatrix} 56 & -44 \\ -44 & 35 \end{bmatrix} = \frac{1}{196} \begin{bmatrix} 56 & -44 \\ -44 & 35 \end{bmatrix} = \begin{bmatrix} 0.2857 & -0.2245 \\ -0.2245 & 0.1786 \end{bmatrix}$

最终计算 $\delta X$

$\delta X = \begin{bmatrix} 0.2857 & -0.2245 \\ -0.2245 & 0.1786 \end{bmatrix} \begin{bmatrix} 7 \\ 8 \end{bmatrix} = \begin{bmatrix} 0.2857*7 + (-0.2245)*8 \\ -0.2245*7 + 0.1786*8 \end{bmatrix} = \begin{bmatrix} 2.0009 \\ -0.2862 \end{bmatrix}$

修正量 $\delta X = \begin{bmatrix} 2.0009 \\ -0.2862 \end{bmatrix}$ 。

最终修正后的状态量为：
$\mathbf{X} + \delta X = \begin{bmatrix} 1 \\ 1 \end{bmatrix} + \begin{bmatrix} 2.0009 \\ -0.2862 \end{bmatrix} = \begin{bmatrix} 3.0009 \\ 0.7138 \end{bmatrix}$

通过最小二乘法，我们得到了状态量的修正值，使得初始状态量得到修正，达到更精确的解。

11.4 定位精度因子

通常把各个误差的影响投到到各卫星的距离上，用相应的距离误差表示，并称为等效距离误差URE（User Equivalent Range Error）。这是一种度量各项误差对最终影响大小的度量方式，即这个因素的影响相当于使测量精度误差多少距离。

如果我们假设所有卫星的URE是均相等为 $\sigma_{URE}$ ，那么给出定位的精度：

$\sigma_{X,Y,Z,T} = (A^T A)^{-1} \sigma_{URE}$

我们令

$Q = (A^T A)^{-1}$

$Q$ 通常称为权系数矩阵，它是一个4x4的对称矩阵。上式清晰地表明了测量误差的方差 $\sigma_{URE}$ 被权系数矩阵 $Q$ 放大后转变成定位误差的方差。因此，GNSS 定位精度与以下两方面因素有关：

测量误差：测量误差的方差 $\sigma_{URE}$ 越大，则定位误差的方差也就越大。
卫星的几何分布：几何矩阵A完全取决于可见卫星的个数及其相对于接收机的空间几何分布状况，与信号的强弱、测量值的好坏或者接收机的性能高低均无关。因此，权系数矩阵 $Q$ 中的元素值越小，则测量误差被放大成定位误差的程度就越低。

可见，为了提高GNSS定位精度，我们必须从降低卫星的测量误差和改善卫星的几何分布这两方面入手。

$\begin{bmatrix} q_{XX} & q_{XY} & q_{XZ} & q_{XT} \\ q_{YX} & q_{YY} & q_{YZ} & q_{YT} \\ q_{ZX} & q_{ZY} & q_{ZZ} & q_{ZT} \\ q_{TX} & q_{TY} & q_{TZ} & q_{TT} \end{bmatrix}$

精度因子：

$\sqrt{q_{XX} + q_{YY} + q_{ZZ} + q_{TT}}$ 称为几何精度衰减因子。
$\sqrt{q_{XX} + q_{YY} + q_{ZZ}}$ 称为空间精度衰减因子。
$\sqrt{q_{TT}}$ 称为钟差精度衰减因子。
$\sqrt{q_{XX} + q_{YY}}$ 称为水平精度衰减因子。
$\sqrt{q_{ZZ}}$ 称为高度精度衰减因子。

通过计算这些精度因子，可以评估卫星几何分布对定位精度的影响。

附加A：定位精度因子详细解释

定位精度因子是用来描述GPS系统定位精度的一个指标，反映了测量误差对最终定位精度的影响。通常将各种误差的影响投影到卫星-接收机的距离上，用相应的距离误差表示，并称为等效距离误差UERE（User Equivalent Range Error）。UERE是一种度量各种误差对最终定位结果影响的综合值，表示这个误差的影响相当于使测量精度误差多少距离。

UERE的计算

如果我们假设所有卫星的UERE均相等为 $\sigma_{UERE}$ ，那么给出定位精度的公式为：

$\sigma_{X,Y,Z,T} = (A^T A)^{-1} \sigma_{UERE}$

其中：

$\sigma_{X,Y,Z,T}$ 是位置和时间的精度。
$A$ 是设计矩阵。
$A^T A)^{-1}$ 是设计矩阵的逆矩阵。

我们引入矩阵 $Q$ ：

$Q = (A^T A)^{-1}$

$Q$ 通常称为权系数矩阵，它是一个 $\times 4$ 的对称矩阵。上式清晰地表明了测量误差的方差 $\sigma_{UERE}^2$ 被权系数矩阵 $Q$ 放大后转变成定位误差的方差。因此，GNSS定位精度与以下两方面因素有关：

测量误差：测量误差的方差 $\sigma_{UERE}$ 越大，则定位误差的方差也就越大。
卫星的几何分布：几何分布完全取决于可见卫星的个数及其相对于接收机的空间几何分布情况，与信号的强弱、测量值的好坏或者接收机的性能高低均无关。因此，权系数矩阵 $Q$ 也只与可见卫星的空间几何分布有关。权系数矩阵 $Q$ 中的元素值越小，则测量误差被放大成定位误差的程度就越低。

为了提高GNSS定位精度，我们必须从降低卫星的测量误差和改善卫星的几何分布这两方面入手。

$Q$ 矩阵的解释

$Q$ 矩阵是一个 $\times 4$ 的矩阵：

$\begin{bmatrix} q_{XX} & q_{XY} & q_{XZ} & q_{XT} \\ q_{YX} & q_{YY} & q_{YZ} & q_{YT} \\ q_{ZX} & q_{ZY} & q_{ZZ} & q_{ZT} \\ q_{TX} & q_{TY} & q_{TZ} & q_{TT} \end{bmatrix}$