由软件接收机的leastSquarePos.m实现。

原理解释

1.牛顿迭代法 (作用是非线性方程组线性化)

一元非线性方程

我们知道，伪距定位法列出的方程组是非线性的四元方程，所以接下来就要研究这类方程的求解方法。
首先考虑一元非线性的情况：
$$f(x)=0$$
此方程没有求解的通法，于是考虑迭代。为了能够逼近最终的值，需要构建如下形式的迭代：
$$x_{k+1}=\phi (x_k)$$
这样有了一个初始值$x_k$，就能计算出$x_{k+1}$，以此类推，一直逼近到最后的值。
这种方法的精度由迭代次数决定，太少的话结果不够精确，但也不是越多越好，需要具体考虑。此外还要注意迭代收敛的问题，如果不能收敛，说明需要重新构建关系式，或选择其它的迭代方法。
然而，因为我们的原式是非线性的，因此无法通过直接变形将$x_{k+1}$剥离出来，所以自然想到利用泰勒级数展开，得到在某点近似的线性方程
具体操作时，由根的初始估计值$x_k$，将$f(x)$在$x_k$处进行泰勒展开，并忽略高阶余项，得到
$$f(x)\approx f(x_k)+f^{\prime }(x_k)\cdot (x-x_k)$$
再根据$f(x)=0$，稍加变形，得到如下的线性方程
$$f(x_k)+f^{\prime }(x_k)\cdot (x-x_k)=0$$
将此方程的解作为$x$的下一个值，也就是$x_{k+1}$，故有
$$x_{k+1}=x_k-\frac{f(x_k)}{f^{\prime }(x_k)}$$
这就是我们构建出的迭代式子$x_{k+1}=\phi (x_k)$，接下来只需要重复以上步骤，直到求解精度满足要求即可。

多元非线性方程

将上面的思路进行推广。例如，对于下式
$$v=f(x,y,z,u)$$
其中，$u$是系统输入，$v$是系统输出，$x$、$y$、$z$是待求的参数，为确定它们的值，我们对系统进行了$n$次测试，得到多元非线性方程组
$$\{\begin{array}{l}{v}_1=f(x,y,z,u_1)\\ v_2=f(x,y,z,u_2)\\ \cdots \\ v_N=f(x,y,z,u_N)\end{array}$$
仍沿用上述牛顿迭代的思想，并有初始值$(x_k,y_k,z_k)$，因此，可在该点处通过泰勒展开对上述方程组进行线性化。对于第$n$个方程，有
$${\nu }_n\approx f(x_k,y_k,z_k,u_n)+\frac{\partial f(x_k,y_k,z_k,u_n)}{\partial x}(x-x_k)+\frac{\partial f(x_k,y_k,z_k,u_n)}{\partial y}(y-y_k)+\frac{\partial f(x_k,y_k,z_k,u_n)}{\partial z}(z-z_k)$$
变形得
$${\nu }_n-f(x_k,y_k,z_k,u_n)=\frac{\partial f(x_k,y_k,z_k,u_n)}{\partial x}(x-x_k)+\frac{\partial f(x_k,y_k,z_k,u_n)}{\partial y}(y-y_k)+\frac{\partial f(x_k,y_k,z_k,u_n)}{\partial z}(z-z_k)$$
考虑方程组整体，就可以用矩阵形式来表达：
$$\mathbf{G}\cdot \Delta \mathbf{x}=\mathbf{b}$$

其中，
$$G=\left[\begin{array}{ccc}\frac{\partial f(x_k,y_k,z_k,u_1)}{\partial x}& \frac{\partial f(x_k,y_k,z_k,u_1)}{\partial y}& \frac{\partial f(x_k,y_k,z_k,u_1)}{\partial z}\\ \frac{\partial f(x_k,y_k,z_k,u_2)}{\partial x}& \frac{\partial f(x_k,y_k,z_k,u_2)}{\partial y}& \frac{\partial f(x_k,y_k,z_k,u_2)}{\partial z}\\ \cdots & \cdots & \cdots \\ \frac{\partial f(x_k,y_k,z_k,u_N)}{\partial x}& \frac{\partial f(x_k,y_k,z_k,u_N)}{\partial y}& \frac{\partial f(x_k,y_k,z_k,u_N)}{\partial z}\end{array}\right]$$
称为雅可比矩阵.

$$\mathbf{b}=[\begin{array}{c}{\nu }_1-f(x_k,y_k,z_k,u_1)\\ {\nu }_2-f(x_k,y_k,z_k,u_2)\\ ...\\ \\ {\nu }_N-f(x_k,y_k,z_k,u_N)\end{array}]$$

$$\Delta x=x-x_k=[\begin{array}{c}x\\ y\\ z\end{array}]-\left[\begin{array}{c}{x}_k\\ y_k\\ z_k\end{array}\right]$$
所以迭代式为
$$x_{k+1}=x_k+\Delta x$$
也就是说，因为$x_k$已知，只要再求出$\Delta x$，就可以得到更新后的$x_{k+1}$.
顺便一提，除了牛顿法以外，还有其它常用的迭代方法，如梯度下降法，它需要构建的形式是
$$x_{k+1}=x_k-\Delta (x_k)$$

2.最小二乘法 （作用是对超定线性方程组求近似解）

在得到线性方程组之后，考虑GNSS定位的实际情况。显然在绝大多数时间内可见星都大于4颗（这也是接收机最希望的工作状态）。此时伪距定位方程组是超定的，也就是有效方程的个数>未知数的个数，在线性代数的体系中是无解的，所以需要使用最小二乘法，得到方程组的近似解$\Delta x$，从而更新定位结果。
所谓最小二乘解，也就是说此时的$\Delta x$会使计算值$f(x,y,z,u)$与实际观测的输出$v$之间的差的平方和最小。

代码实现

因为matlab中有求解矩阵方程的运算符，因此就不需要再另外写最小二乘的代码。
调用格式为：x=A\B ; 要求矩阵A、B必须具有相同的行数。

对矩阵A可以分成如下三种情况：

1. 如果A是标量，那么A\B就等同于A.\B
2. 如果A是n×n方阵，B是n行矩阵，那么A\B就是方程A*X=B的解
3. 如果A是m×n矩阵（m≠n），B是m行矩阵，那么A\B返回方程组A*X=B的最小二乘解

在下面代码中，显然符合第三种情况，所以会直接返回最小二乘解，十分方便。

function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings)
%接收机位置的最小二乘解

%=== Initialization =======================================================
nmbOfIterations = 7;  %迭代次数=7

dtr     = pi/180;        %角度和弧度的转换因子
pos     = zeros(4, 1);   %初始化用户位置结果结构体，包括三维位置和钟差
X       = satpos;        %X存入卫星位置
nmbOfSatellites = size(satpos, 2);  %因为satpos每一列存的是一颗卫星的数据，所以计算列数就得到卫星数量

A       = zeros(nmbOfSatellites, 4);
omc     = zeros(nmbOfSatellites, 1);    %残差
az      = zeros(1, nmbOfSatellites);    %方位角
el      = az;   %仰角

%=== 开始迭代 ===================================
for iter = 1:nmbOfIterations

    for i = 1:nmbOfSatellites
        if iter == 1
            Rot_X = X(:, i);    %第一次迭代时，Rot_X记录卫星初始位置
            trop = 2;   %初始化对流层延迟修正
        else
            % 如果不是第一次迭代...
            rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ...
                   (X(3, i) - pos(3))^2;   
            %rho2就是伪距的平方，pos123分别代表XYZ
            traveltime = sqrt(rho2) / settings.c ;  % 传播时间=伪距/光速

            % 调用e_r_corr，根据卫星信号传播时间和地球自转速度，对卫星位置进行修正
            Rot_X = e_r_corr(traveltime, X(:, i));

            % ---调用topocent函数，计算卫星的方位角、仰角和距离
            [az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
					 % 至此，四个输出结果中的el、az就求完了
					 
            if (settings.useTropCorr == 1)   %如果使用对流层延迟修正：
                
                trop = tropo(sin(el(i) * dtr), ...
                             0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
            else
                % 否则就令=0，不修正
                trop = 0;
            end
        end 

下面就是进行矩阵方程的求解，把第一部分原理解释中的牛顿迭代法应用到GNSS定位场景，就能得到以下：
$$\mathbf{G}\cdot \Delta \mathbf{x}=\mathbf{b}$$
①对这个方程，首先算b：
$$\mathbf{b}=[\begin{array}{c}{\nu }_1-f(x_k,y_k,z_k,u_1)\\ {\nu }_2-f(x_k,y_k,z_k,u_2)\\ ...\\ \\ {\nu }_N-f(x_k,y_k,z_k,u_N)\end{array}]$$
$v$是观测到的伪距，即输入的obs；$f$是计算得到的距离，其中还包含钟差和对流层延迟修正。
故相应代码为：

omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop);
% norm( 'fro')：意思是求矩阵二范数
% pos(4)是钟差
% trop是对流层延迟修正

②然后算G，在程序里是矩阵A：
以第一列元素为例，
分母：观测矢量$r$在$x$方向上的分量再反向，而$r$是由接收机指向卫星，故
$r$ = 卫星的$x$位置-用户的$x$位置 = Rot_X(1) - pos(1)
分子：观测矢量$r$的模，也就是伪距obs(i)
因此A的代码为：

 A(i, :) =  [ (-(Rot_X(1) - pos(1))) / obs(i) ...
                     (-(Rot_X(2) - pos(2))) / obs(i) ...
                     (-(Rot_X(3) - pos(3))) / obs(i) ...
                     1 ];
    end

③求完之后，需要判断A的情况
若A不满秩，则有无穷多解，我们对这种情况不感兴趣，所以pos直接返回0矩阵。否则可往下进行。

    if rank(A) ~= 4         %若矩阵A不满秩
        pos     = zeros(1, 4);
        return
    end

至此，G、b都已得到，因此矩阵方程已经建立，下面就是求解。

    x   = A \ omc;          %计算位置更新量x  

    pos = pos + x;          %更新位置
    
end % for iter = 1:nmbOfIterations

pos = pos';

到这里，第三个输出pos也得到了。最后一个输出是精度因子DOP。

先求出权系数阵Q：
$$Q=(A^{T}A{)}^{-1}$$

原本，定位误差就是测量误差，但是在最小二乘法求解的过程中，测量误差的方差被矩阵Q放大了，成为新的定位误差。而DOP表征的正是误差的放大倍数，所以它由Q阵求得，代码如下：

%=== 计算DOP ======================================
if nargout  == 4
    %--- Initialize output ------------------------------------------------
    dop     = zeros(1, 5);
    
    %--- Calculate DOP ----------------------------------------------------
    Q       = inv(A'*A);     % Q=(A*A转置)求逆
    
    dop(1)  = sqrt(trace(Q));                       % GDOP    
    dop(2)  = sqrt(Q(1,1) + Q(2,2) + Q(3,3));       % PDOP
    dop(3)  = sqrt(Q(1,1) + Q(2,2));                % HDOP
    dop(4)  = sqrt(Q(3,3));                         % VDOP
    dop(5)  = sqrt(Q(4,4));                         % TDOP
end

其中，
GDOP：三维坐标与时间精度因子；所以所有对角线元素都要用到，也就是矩阵的迹
PDOP：位置精度因子；只与三维位置坐标有关
HDOP：水平精度因子；只与xy位置有关
VDOP：垂直精度因子；只与z有关
TDOP：时间精度因子；只与钟差有关