1. 观测方程

设GNSS接收机和卫星的位矢分别为

$$ \vec{R_r} = \begin{bmatrix} x_r \\ y_r \\ z_r \\ \end{bmatrix} , \quad \vec{R_s^i} = \begin{bmatrix} x_s^i \\ y_s^i \\ z_s^i \\ \end{bmatrix} $$

其中的$i$表示第$i$颗卫星。

伪距观测量$\rho$是指从卫星到接收机的近似距离，因为它包含了大量的误差（接收机钟差、卫星钟差、电离层延迟误差、对流层延迟误差、相对论效应误差、固体潮海潮误差、多普勒效应误差等），所以它并不严格等于二者的几何距离。

$$ \rho_i = |R_{rs}| + (\delta t_r - \delta t_s^i )c\space + \delta_{ion} + \delta_{tro} + \delta_{rel} + \delta_{tide} + \delta\\_{dop} + \delta_{other} $$

有些误差（例如电离层、对流层等）能够进行数学物理建模然后计算出来，我们尽可能多地把误差都减掉，伪距就会变得更准确一点，为了方便我们把这个量定义为修正伪距$\hat \rho$ 。

$$ \hat \rho = |R_{rs}| + \delta t_r c + \delta_{other} $$

接收机和卫星$i$之间的距离为

$$ |R_{rs}^i| = \sqrt{ (x_r - x_s^i)^2 + (y_r - y_s^i)^2 + (z_r - z_s^i)^2 } $$

我们将修正伪距代入上式，忽略$\delta_{other}$，可得

$$ \sqrt{ (x_r - x_s^i)^2 + (y_r - y_s^i)^2 + (z_r - z_s^i)^2 } + \delta t_rc = \hat \rho_i $$

这个就是GNSS观测方程，其中的卫星坐标可根据广播星历计算出来，所以观测方程仅包含四个未知数：接收机三维坐标和接收机钟差。观测方程想要求解需要至少4个方程，换句话说，至少需要同时存在4颗可观测卫星，接收机才能完成单点定位。

2. 观测方程线性化求解

观测方程是非线性方程，很难求解，我们可以将观测方程左边在接收机位矢$\vec{R_{r_0}}$处进行线性化，换句话说，进行泰勒展开但仅保留一阶项。（$\vec{R_{r_0}}$是接收机初值，可以是任意值，只不过当初值越接近真实值，线性化后初值邻域内的函数值也越接近真实值）

$$ \frac {1}{2|R_{r_0s}^i|} ( 2(x_{r_0} - x_s^i)(x_r - x_{r_0}) + 2(y_{r_0} - y_s^i)(y_r - y_{r_0}) + 2(z_{r_0} - z_s^i)(z_r - z_{r_0}) ) + \delta t_r c = \hat \rho_i $$

以$x$项为例，我们注意到$(x_{r_0} - x_s^i) / |R_{r_0s}^i|$其实是以卫星$i$的位矢$\vec{R_s^i}$为起点，以接收机位矢$\vec{R_{r_0}}$为终点的向量$\vec{R_{r_0s}^i}$的方向向量$\vec{e_{r_0s}^i}$在$x$轴方向的分量，我们简单计作$e_x^i$。以及$(x_r - x_{r_0})$是接收机位矢变化量$\Delta \vec R_r$在$x$轴方向的分量。所以观测方程线性化后可写成如下形式

$$ e_x^i \Delta x + e_y^i \Delta y + e_z^i \Delta z + \delta t_r c = \hat \rho_i $$

多颗卫星的观测方程线性化后可组成方程组，写成矩阵形式

$$ \begin{bmatrix} e_x^1 & e_y^1 & e_z^1 & 1 \\ ... & ... & ... & ...\\ e_x^n & e_y^n & e_z^n & 1 \\ \end{bmatrix} \begin{bmatrix} \Delta x \\ \Delta y \\ \Delta z \\ \delta t_r c \end{bmatrix} = \begin{bmatrix} \hat \rho_1 \\ ... \\ \hat \rho_n \end{bmatrix} $$

$$ B \vec X = \vec L $$

为了方便，我们把$\vec X$叫做修正向量。

2.1 无冗余观测

若至少4颗卫星的分布良好，不存在过于靠近、共线等线性相关的情况，则方程的解存在

$$ \vec X = (B^TB)^{-1} B^T \vec L $$

这是比较少见的情况，为了泛用性，我们重点讨论下面的一般情况：有冗余观测。

2.2 有冗余观测（最小二乘）

若除了4颗良好卫星之外，还有冗余观测，则方程大概率无解。定义误差向量为

$$ \vec V = B \vec X - \vec L $$

那么我们的目标就是找到一个$\vec X$使$\vec V$的模长尽可能小，我们很自然地想到可以让$V^TV$的一阶导等于零（因为$V^TV$仅有一个极值点，且该点为最小值点）。

$$ \frac {\partial (V^TV)}{\partial X} = 0 $$

$$ \frac {\partial}{\partial X}(V^TV) = 2 \frac {\partial V}{\partial X}V = 2 \frac {\partial (BX)}{\partial X}V = 2 B^T (BX - L) = 2 (B^TBX - B^TL) = 0 $$

$$ \vec X = (B^TB)^{-1} B^T \vec L $$

这个算法就是最小二乘算法LS的核心步骤：误差平方和最小化。这个解$\vec X$的形式与无冗余观测是一样的。

需要注意的是，这里计算得到的解，是观测方程组线性化之后的解，而不是原观测方程组的解，因为线性化只保留了一阶项。所以直接把$\vec X$代入到原始的观测方程组，方程组一定是不成立的。

3. 迭代求解

在得到修正向量$\vec X$之后，我们就可以将其加到接收机初值上得到更准确的接收机向量

$$ \vec R_r = \vec R_{r_0} + \begin{bmatrix} \Delta x \\ \Delta y \\ \Delta z \end{bmatrix} $$

若进行接收机本地时钟校正，则接收机本地时钟也可以变得更准

$$ t_r = t_{r_0} + \delta t_r $$

把当前接收机向量作为初值，代入到观测方程线性化方程组继续求解，则可以迭代求解接收机向量，当误差小于用户指定的阈值，或达到最大迭代次数，迭代停止，我们就求出了接收机的坐标向量，接收机本地时钟也能够完成校正。

4. 加权最小二乘

观测方程线性化方程组

$$ B \vec X = \vec L $$

误差向量

$$ \vec V = B \vec X - \vec L $$

仅考虑有冗余观测的一般情况，我们在进行最小二乘算法的时候，会希望误差越大的观测对最终结果的贡献越小，反之误差越小对结果的贡献越大。我们可以定义一个权重矩阵（简称权阵）$P$

$$ P = \begin{bmatrix} p_1 & & & 0 \\ & p_2 & & \\ & & ... & \\ 0 & & & p_n \end{bmatrix} $$

权阵为对角矩阵，将$P$引入到最小二乘算法中。

$$ \frac {\partial(V^T P V)}{\partial X} = 0 $$

$$ \frac {\partial(V^T P V)}{\partial X} = \frac {\partial (BX)}{\partial X}PV + \frac {\partial (V^T P^T)}{\partial X}V = B^T PV + B^T P^T V $$

因为权阵$P$是对角矩阵，所以

$$ \frac {\partial(V^T P V)}{\partial X} = 2 B^T P (BX - L) = 0 $$

$$ X = (B^T P B)^{-1}B^T P L $$

4.1 权阵定义

权阵如何定义呢？根据“观测误差小的贡献大”的需求，我们可以选取伪距残差的倒数作为权重，为了只关注误差的相对水平，避免出现过大或过小的数值，我们再把伪距残差倒数进行归一化。

$$ p_i = \frac { \frac 1{|\hat \rho_i - |\vec R_{r_0} - \vec R_s^i||} } { \sum_{j = 1}^n \frac 1{|\hat \rho_j - |\vec R_{r_0} - \vec R_s^j||} } $$