由软件接收机的leastSquarePos.m实现。
原理解释
1.牛顿迭代法 (作用是非线性方程组线性化)
一元非线性方程
我们知道，伪距定位法列出的方程组是非线性的四元方程，所以接下来就要研究这类方程的求解方法。
首先考虑一元非线性的情况：
f
(
x
)
=
0
f(x)=0
f(x)=0
此方程没有求解的通法，于是考虑迭代。为了能够逼近最终的值，需要构建如下形式的迭代：
x
k
+
1
=
φ
(
x
k
)
x_{k+1}=\varphi(x_k)
xk+1=φ(xk)
这样有了一个初始值
x
k
x_k
xk，就能计算出
x
k
+
1
x_{k+1}
xk+1，以此类推，一直逼近到最后的值。
这种方法的精度由迭代次数决定，太少的话结果不够精确，但也不是越多越好，需要具体考虑。此外还要注意迭代收敛的问题，如果不能收敛，说明需要重新构建关系式，或选择其它的迭代方法。
然而，因为我们的原式是非线性的，因此无法通过直接变形将
x
k
+
1
x_{k+1}
xk+1剥离出来，所以自然想到利用泰勒级数展开，得到在某点近似的线性方程
具体操作时，由根的初始估计值
x
k
x_k
xk，将
f
(
x
)
f(x)
f(x)在
x
k
x_k
xk处进行泰勒展开，并忽略高阶余项，得到
f
(
x
)
≈
f
(
x
k
)
+
f
′
(
x
k
)
⋅
(
x
−
x
k
)
f(x)\approx f(x_{k})+f^{\prime}(x_{k})\cdot(x-x_{k})
f(x)≈f(xk)+f′(xk)⋅(x−xk)
再根据
f
(
x
)
=
0
f(x)=0
f(x)=0，稍加变形，得到如下的线性方程
f
(
x
k
)
+
f
′
(
x
k
)
⋅
(
x
−
x
k
)
=
0
f(x_{k})+f^{\prime}(x_{k})\cdot(x-x_{k})=0
f(xk)+f′(xk)⋅(x−xk)=0
将此方程的解作为
x
x
x的下一个值，也就是
x
k
+
1
x_{k+1}
xk+1，故有
x
k
+
1
=
x
k
−
f
(
x
k
)
f
′
(
x
k
)
x_{k+1}=x_{k}-\frac{f(x_{k})}{f^{\prime}(x_{k})}
xk+1=xk−f′(xk)f(xk)
这就是我们构建出的迭代式子
x
k
+
1
=
φ
(
x
k
)
x_{k+1}=\varphi(x_k)
xk+1=φ(xk)，接下来只需要重复以上步骤，直到求解精度满足要求即可。
多元非线性方程
将上面的思路进行推广。例如，对于下式
v
=
f
(
x
,
y
,
z
,
u
)
v=f(x,y,z,u)
v=f(x,y,z,u)
其中，
u
u
u是系统输入，
v
v
v是系统输出，
x
x
x、
y
y
y、
z
z
z是待求的参数，为确定它们的值，我们对系统进行了
n
n
n次测试，得到多元非线性方程组
{
v
1
=
f
(
x
,
y
,
z
,
u
1
)
v
2
=
f
(
x
,
y
,
z
,
u
2
)
⋯
v
N
=
f
(
x
,
y
,
z
,
u
N
)
\begin{cases}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{cases}
⎩
⎨
⎧v1=f(x,y,z,u1)v2=f(x,y,z,u2)⋯vN=f(x,y,z,uN)
仍沿用上述牛顿迭代的思想，并有初始值
(
x
k
,
y
k
,
z
k
)
(x_k,y_k,z_k)
(xk,yk,zk)，因此，可在该点处通过泰勒展开对上述方程组进行线性化。对于第
n
n
n个方程，有
ν
n
≈
f
(
x
k
,
y
k
,
z
k
,
u
n
)
+
∂
f
(
x
k
,
y
k
,
z
k
,
u
n
)
∂
x
(
x
−
x
k
)
+
∂
f
(
x
k
,
y
k
,
z
k
,
u
n
)
∂
y
(
y
−
y
k
)
+
∂
f
(
x
k
,
y
k
,
z
k
,
u
n
)
∂
z
(
z
−
z
k
)
\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})
νn≈f(xk,yk,zk,un)+∂x∂f(xk,yk,zk,un)(x−xk)+∂y∂f(xk,yk,zk,un)(y−yk)+∂z∂f(xk,yk,zk,un)(z−zk)
变形得
ν
n
−
f
(
x
k
,
y
k
,
z
k
,
u
n
)
=
∂
f
(
x
k
,
y
k
,
z
k
,
u
n
)
∂
x
(
x
−
x
k
)
+
∂
f
(
x
k
,
y
k
,
z
k
,
u
n
)
∂
y
(
y
−
y
k
)
+
∂
f
(
x
k
,
y
k
,
z
k
,
u
n
)
∂
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})
νn−f(xk,yk,zk,un)=∂x∂f(xk,yk,zk,un)(x−xk)+∂y∂f(xk,yk,zk,un)(y−yk)+∂z∂f(xk,yk,zk,un)(z−zk)
考虑方程组整体，就可以用矩阵形式来表达：
G
⋅
Δ
x
=
b
\boldsymbol{G}\cdot\Delta \boldsymbol{x}=\boldsymbol{b}
G⋅Δx=b
其中，
G
=
[
∂
f
(
x
k
,
y
k
,
z
k
,
u
1
)
∂
x
∂
f
(
x
k
,
y
k
,
z
k
,
u
1
)
∂
y
∂
f
(
x
k
,
y
k
,
z
k
,
u
1
)
∂
z
∂
f
(
x
k
,
y
k
,
z
k
,
u
2
)
∂
x
∂
f
(
x
k
,
y
k
,
z
k
,
u
2
)
∂
y
∂
f
(
x
k
,
y
k
,
z
k
,
u
2
)
∂
z
⋯
⋯
⋯
∂
f
(
x
k
,
y
k
,
z
k
,
u
N
)
∂
x
∂
f
(
x
k
,
y
k
,
z
k
,
u
N
)
∂
y
∂
f
(
x
k
,
y
k
,
z
k
,
u
N
)
∂
z
]
G=\begin{bmatrix}\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{bmatrix}
G=
∂x∂f(xk,yk,zk,u1)∂x∂f(xk,yk,zk,u2)⋯∂x∂f(xk,yk,zk,uN)∂y∂f(xk,yk,zk,u1)∂y∂f(xk,yk,zk,u2)⋯∂y∂f(xk,yk,zk,uN)∂z∂f(xk,yk,zk,u1)∂z∂f(xk,yk,zk,u2)⋯∂z∂f(xk,yk,zk,uN)
称为雅可比矩阵.
b = [ ν 1 − f ( x k , y k , z k , u 1 ) ν 2 − f ( x k , y k , z k , u 2 ) . . . ν N − f ( x k , y k , z k , u N ) ] \left.\boldsymbol{b}=\left[\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}\right.\right] b= ν1−f(xk,yk,zk,u1)ν2−f(xk,yk,zk,u2)...νN−f(xk,yk,zk,uN)
Δ
x
=
x
−
x
k
=
[
x
y
z
]
−
[
x
k
y
k
z
k
]
\left.\Delta x=x-x_{k}=\left[\begin{array}{c}x\\y\\z\end{array}\right.\right]-\left[\begin{array}{c}x_{k}\\y_{k}\\z_{k}\end{array}\right]
Δx=x−xk=
xyz
−
xkykzk
所以迭代式为
x
k
+
1
=
x
k
+
Δ
x
x_{k+1}=x_k+\Delta x
xk+1=xk+Δx
也就是说，因为
x
k
x_k
xk已知，只要再求出
Δ
x
\Delta x
Δx，就可以得到更新后的
x
k
+
1
x_{k+1}
xk+1.
顺便一提，除了牛顿法以外，还有其它常用的迭代方法，如梯度下降法，它需要构建的形式是
x
k
+
1
=
x
k
−
Δ
(
x
k
)
x_{k+1}=x_k-\Delta(x_k)
xk+1=xk−Δ(xk)
2.最小二乘法 （作用是对超定线性方程组求近似解）
在得到线性方程组之后，考虑GNSS定位的实际情况。显然在绝大多数时间内可见星都大于4颗（这也是接收机最希望的工作状态）。此时伪距定位方程组是超定的，也就是有效方程的个数>未知数的个数，在线性代数的体系中是无解的，所以需要使用最小二乘法，得到方程组的近似解
Δ
x
\Delta{x}
Δx，从而更新定位结果。
所谓最小二乘解，也就是说此时的
Δ
x
\Delta{x}
Δx会使计算值
f
(
x
,
y
,
z
,
u
)
f(x,y,z,u)
f(x,y,z,u)与实际观测的输出
v
v
v之间的差的平方和最小。
代码实现
因为matlab中有求解矩阵方程的运算符，因此就不需要再另外写最小二乘的代码。
调用格式为：x=A\B ; 要求矩阵A、B必须具有相同的行数。
对矩阵A可以分成如下三种情况：
- 如果A是标量，那么A\B就等同于A.\B
- 如果A是n×n方阵，B是n行矩阵，那么A\B就是方程A*X=B的解
- 如果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定位场景，就能得到以下：
G
⋅
Δ
x
=
b
\boldsymbol{G}\cdot\Delta \boldsymbol{x}=\boldsymbol{b}
G⋅Δx=b
①对这个方程，首先算b：
b
=
[
ν
1
−
f
(
x
k
,
y
k
,
z
k
,
u
1
)
ν
2
−
f
(
x
k
,
y
k
,
z
k
,
u
2
)
.
.
.
ν
N
−
f
(
x
k
,
y
k
,
z
k
,
u
N
)
]
\left.\boldsymbol{b}=\left[\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}\right.\right]
b=
ν1−f(xk,yk,zk,u1)ν2−f(xk,yk,zk,u2)...νN−f(xk,yk,zk,uN)
v
v
v是观测到的伪距，即输入的obs；
f
f
f是计算得到的距离，其中还包含钟差和对流层延迟修正。
故相应代码为：
omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop);
% norm( 'fro')：意思是求矩阵二范数
% pos(4)是钟差
% trop是对流层延迟修正
②然后算G，在程序里是矩阵A：
以第一列元素为例，
分母：观测矢量
r
r
r在
x
x
x方向上的分量再反向，而
r
r
r是由接收机指向卫星，故
r
r
r = 卫星的
x
x
x位置-用户的
x
x
x位置 = Rot_X(1) - pos(1)
分子：观测矢量
r
r
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=(A^T A)^{-1}
Q=(ATA)−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：时间精度因子；只与钟差有关
4316
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
