【GNSS原理】【最小二乘法】Chapter.5 GNSS定位算法——LS和WLS方法 [2025年4月]
Chapter.5 GNSS定位算法——LS和WLS方法
作者：齐花Guyc(CAUC)
文章目录
- Chapter.5 GNSS定位算法——LS和WLS方法
-
- 一、引言
- 二、LS方法
- 三、WLS方法
- 四、GNSS PVT解算流程中的LS和WLS
一、引言
在GNSS定位中，最小二乘法是一种核心算法，用于根据接收机获取的观测数据（如伪距、载波相位等）估算用户的位置、速度和时间偏差（PVT解算）。
二、LS方法
最小二乘法的核心是最小化观测值与预测值的误差平方和，得到最优的状态量估计。
GNSS定位的数学模型通常线性表示为：
Y = A x + n Y = A x + n Y=Ax+n
其中，
Y Y Y是观测向量（伪距观测值）；
A A A是系统观测矩阵，表示状态量到观测量的转换关系；
x x x是待估计的状态向量（位置坐标和钟差）；
n n n是观测噪声向量。
给定实际观测值 Y ~ \tilde{Y} Y~,目标是找到状态估计 x ^ \hat{x} x^，使预测值 A x ^ A \hat{x} Ax^尽可能接近 Y ~ \tilde{Y} Y~。最小二乘法通过最小化误差的平方和来实现这一点。
用代价函数来衡量预测值与观测值的差异 J ( x ^ ) = ( Y ~ − A x ^ ) T ( Y ~ − A x ^ ) J(\hat{x}) = (\tilde{Y} - A \hat{x})^T (\tilde{Y} - A \hat{x}) J(x^)=(Y~−Ax^)T(Y~−Ax^) 即误差平方和
目标是找到 x ^ \hat{x} x^,使 J ( x ^ ) J(\hat{x}) J(x^)最小化： x ^ = arg min x ^ J ( x ^ ) \hat{x} = \arg\min_{\hat{x}} J(\hat{x}) x^=argx^minJ(x^)
为了求解 x ^ \hat{x} x^，将 J ( x ^ ) J(\hat{x}) J(x^)展开，得到
J ( x ^ ) = Y ~ T Y ~ − Y ~ T A x ^ − ( A x ^ ) T Y ~ + ( A x ^ ) T ( A x ^ ) J(\hat{x}) = \tilde{Y}^T \tilde{Y} - \tilde{Y}^T A \hat{x} - (A \hat{x})^T \tilde{Y} + (A \hat{x})^T (A \hat{x}) J(x^)=Y~TY~−Y~TAx^−(Ax^)TY~+(Ax^)T(Ax^)
中间两项同为标量且二者相等
Y ~ T A x ^ = ( Y ~ T A x ^ ) T = x ^ T A T Y ~ \tilde{Y}^T A \hat{x} = (\tilde{Y}^T A \hat{x})^T = \hat{x}^T A^T \tilde{Y} Y~TAx^=(Y~TAx^)T=x^TATY~
所以又有 J ( x ^ ) = Y ~ T Y ~ − 2 Y ~ T A x ^ + x ^ T A T A x ^ J(\hat{x}) = \tilde{Y}^T \tilde{Y} - 2 \tilde{Y}^T A \hat{x} + \hat{x}^T A^T A \hat{x} J(x^)=Y~TY~−2Y~TAx^+x^TATAx^
要最小化 J ( x ^ ) J(\hat{x}) J(x^)，即对 x ^ \hat{x} x^求导并令导数为零
注意到 J ( x ^ ) J(\hat{x}) J(x^)的第一项是常数项、第二项是一次项、第三项是二次项。
常数项对 x ^ \hat{x} x^求导为0；
一次项对 x ^ \hat{x} x^求导为 ∂ ∂ x ^ ( − 2 Y ~ T A x ^ ) = − 2 A T Y ~ \frac{\partial}{\partial \hat{x}} (-2 \tilde{Y}^T A \hat{x}) = -2 A^T \tilde{Y} ∂x^∂(−2Y~TAx^)=−2ATY~
二次项对 x ^ \hat{x} x^求导为 ∂ ∂ x ^ ( x ^ T A T A x ^ ) = 2 A T A x ^ \frac{\partial}{\partial \hat{x}} (\hat{x}^T A^T A \hat{x}) = 2 A^T A \hat{x} ∂x^∂(x^TATAx^)=2ATAx^
最终得到
∂ J ( x ^ ) ∂ x ^ = − 2 A T Y ~ + 2 A T A x ^ \frac{\partial J(\hat{x})}{\partial \hat{x}} = -2 A^T \tilde{Y} + 2 A^T A \hat{x} ∂x^∂J(x^)=−2ATY~+2ATAx^
令一阶导数等于零，找到极值点
− 2 A T Y ~ + 2 A T A x ^ = 0 -2 A^T \tilde{Y} + 2 A^T A \hat{x} = 0 −2ATY~+2ATAx^=0
得 A T A x ^ = A T Y ~ A^T A \hat{x} = A^T \tilde{Y} ATAx^=ATY~
若此时满足 A T A A^T A ATA可逆，即 A A A的列满秩， A A A的 m m m个行向量中至少有 k k k个是线性无关的， r a n k ( A ) = k rank(A)=k rank(A)=k。这就是为什么需要四颗卫星才能得到位置坐标和钟差的原因。
最终最小二乘估计的解析解为
x ^ = ( A T A ) − 1 A T Y ~ \hat{x} = (A^T A)^{-1} A^T \tilde{Y} x^=(ATA)−1ATY~
最小二乘的几何意义是将观测向量 Y ~ \tilde{Y} Y~投影到由矩阵 A A A的列向量张成的线性空间S(A)上。
![【GNSS原理】【最小二乘法】Chapter.5 GNSS定位算法——LS和WLS方法 [2025年4月]_第1张图片](http://img.e-com-net.com/image/info8/8df9abb0bbbc49c2818a20fba1380b5c.png)
三、WLS方法
LS方法假设所有观测值具有相同的可靠性，即噪声方差相等。意味着每个观测值对估计结果的贡献相同。
然而，在GNSS实际应用中，观测数据的噪声方差会因为卫星信号质量、卫星仰角、多径效应等因素而异，LS忽略这些差异，将不可靠的观测与高质量观测同等对待，可能导致精度下降。
WLS通过引入权重矩阵 W W W根据观测值的可靠性（噪声方差）调整各观测对估计结果的贡献，优化估计精度。
WLS的代价函数为 J ( x ^ ) = ( Y ~ − A x ^ ) T W ( Y ~ − A x ^ ) J(\hat{x}) = (\tilde{Y} - A \hat{x})^TW (\tilde{Y} - A \hat{x}) J(x^)=(Y~−Ax^)TW(Y~−Ax^)
W = d i a g ( w 1 , w 2 , . . . , w m ) W =diag(w_1,w_2,...,w_m) W=diag(w1,w2,...,wm)对角矩阵， w i w_i wi为第 i i i个观测的权重。
通常选择 W = R − 1 W=R^{-1} W=R−1, R R R为噪声协方差矩阵，如果噪声是白噪声，那么
R = [ σ 1 2 0 ⋯ 0 0 σ 2 2 ⋯ 0 ⋮ ⋮ ⋱ ⋮ 0 0 ⋯ σ m 2 ] R = \begin{bmatrix} \sigma_1^2 & 0 & \cdots & 0 \\ 0 & \sigma_2^2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & \sigma_m^2 \end{bmatrix} R= σ120⋮00σ22⋮0⋯⋯⋱⋯00⋮σm2
那么有
W = R − 1 = [ 1 σ 1 2 0 ⋯ 0 0 1 σ 2 2 ⋯ 0 ⋮ ⋮ ⋱ ⋮ 0 0 ⋯ 1 σ m 2 ] W =R^{-1}= \begin{bmatrix} \frac{1}{\sigma_1^2} & 0 & \cdots & 0 \\ 0 & \frac{1}{\sigma_2^2} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & \frac{1}{\sigma_m^2} \end{bmatrix} W=R−1= σ1210⋮00σ221⋮0⋯⋯⋱⋯00⋮σm21
噪声方差越大说明观测量越不可靠，那么对应的观测量分配较小的权重；反之，噪声方差越小说明观测量越可靠，那么对应的观测量分配较大的权重，整体估计可信度提高。
WLS的估计公式为 x ^ = ( A T W A ) − 1 A T W Y ~ \hat{x} = (A^T WA)^{-1} A^TW \tilde{Y} x^=(ATWA)−1ATWY~
GNSS观测数据的噪声方差差异显著:
高仰角卫星的伪距测量受大气延迟和多径影响小， σ 2 \sigma^2 σ2 较小，应分配较大权重；
低仰角卫星或受遮挡的卫星信号质量差， σ 2 \sigma^2 σ2 较大，应分配较小权重。
四、GNSS PVT解算流程中的LS和WLS
1.数据准备
通过接收机跟踪测距码和载波获取观测值：伪距和多普勒。
通过星历信息计算卫星位置及速度。
2.进行位置钟差计算
GNSS位置和钟差计算基于伪距观测。
伪距观测方程为
ρ ~ i ( x u ) = ( x u − x s i ) 2 + ( y u − y s i ) 2 + ( z u − z s i ) 2 + c b + n ρ i , i = 1 , 2 , … , m \tilde{\rho}_i(x_u) = \sqrt{(x_u - x_{s_i})^2 + (y_u - y_{s_i})^2 + (z_u - z_{s_i})^2} + c b + n_{\rho_i}, \quad i = 1, 2, \dots, m ρ~i(xu)=(xu−xsi)2+(yu−ysi)2+(zu−zsi)2+cb+nρi,i=1,2,…,m
目标是解算 x = [ x u , y u , z u , b ] T x = [x_u, y_u, z_u, b]^T x=[xu,yu,zu,b]T
伪距方程是非线性的，直接应用最小二乘法需将其线性化。
选择初始估计点 x 0 = [ x 0 , y 0 , z 0 , b 0 ] T x_0 = [x_0, y_0, z_0, b_0]^T x0=[x0,y0,z0,b0]T，在初始估计点进行一阶泰勒展开使其线性化。
得到 ρ ~ i ( x u ) ≈ ρ i ( x 0 ) + ∂ ρ i ∂ x u ∣ x 0 ( x u − x 0 ) + ∂ ρ i ∂ y u ∣ y 0 ( y u − y 0 ) + ∂ ρ i ∂ z u ∣ z 0 ( z u − z 0 ) + c ( b − b 0 ) + n ρ i \tilde{\rho}_i(x_u) \approx \rho_i(x_0) + \left. \frac{\partial \rho_i}{\partial x_u} \right|_{x_0} (x_u - x_0) + \left. \frac{\partial \rho_i}{\partial y_u} \right|_{y_0} (y_u - y_0) + \left. \frac{\partial \rho_i}{\partial z_u} \right|_{z_0} (z_u - z_0) + c (b - b_0) + n_{\rho_i} ρ~i(xu)≈ρi(x0)+∂xu∂ρi x0(xu−x0)+∂yu∂ρi y0(yu−y0)+∂zu∂ρi z0(zu−z0)+c(b−b0)+nρi
其中 ρ i ( x 0 ) = ( x 0 − x s i ) 2 + ( y 0 − y s i ) 2 + ( z 0 − z s i ) 2 + c b 0 \rho_i(x_0) = \sqrt{(x_0 - x_{s_i})^2 + (y_0 - y_{s_i})^2 + (z_0 - z_{s_i})^2} + c b_0 ρi(x0)=(x0−xsi)2+(y0−ysi)2+(z0−zsi)2+cb0是基于初始点的预测伪距。
为了方便，定义伪距残差和状态增量
伪距残差为
δ ρ i = ρ ~ i − ρ i ( x 0 ) \delta \rho_i = \tilde{\rho}_i - \rho_i(x_0) δρi=ρ~i−ρi(x0)
状态增量为
d x 0 = [ x u − x 0 , y u − y 0 , z u − z 0 , b − b 0 ] T \quad d x_0 = [x_u - x_0, y_u - y_0, z_u - z_0, b - b_0]^T dx0=[xu−x0,yu−y0,zu−z0,b−b0]T
则线性化方程表示为 δ ρ i = u i ⋅ d x 0 + n ρ i \delta \rho_i = u_i \cdot d x_0 + n_{\rho_i} δρi=ui⋅dx0+nρi
其中 u i = [ − x 0 − x s i ρ i ( x 0 ) , − y 0 − y s i ρ i ( x 0 ) , − z 0 − z s i ρ i ( x 0 ) , c ] u_i = \left[ -\frac{x_0 - x_{s_i}}{\rho_i(x_0)}, -\frac{y_0 - y_{s_i}}{\rho_i(x_0)}, -\frac{z_0 - z_{s_i}}{\rho_i(x_0)}, c \right] ui=[−ρi(x0)x0−xsi,−ρi(x0)y0−ysi,−ρi(x0)z0−zsi,c]为单位方向矢量。
对于 m m m颗卫星，伪距残差方程组写成矩阵形式为
δ ρ = H d x 0 + n ρ \delta \rho = H d x_0 + n_\rho δρ=Hdx0+nρ
观测矩阵为
H = [ u 1 T , u 2 T , … , u m T ] T H= [u_1^T, u_2^T, \dots, u_m^T]^T H=[u1T,u2T,…,umT]T
最小二乘法通过最小化代价函数求解 d x 0 d x_0 dx0
此时的代价函数为 J ( d x 0 ) = ( δ ρ − H d x 0 ) T ( δ ρ − H d x 0 ) J(d x_0) = (\delta \rho - H d x_0)^T (\delta \rho - H d x_0) J(dx0)=(δρ−Hdx0)T(δρ−Hdx0)
对其求导并令导数为0，得到极值点 ∂ J ∂ d x 0 = − 2 H T δ ρ + 2 H T H d x 0 = 0 \frac{\partial J}{\partial d x_0} = -2 H^T \delta \rho + 2 H^T H d x_0 = 0 ∂dx0∂J=−2HTδρ+2HTHdx0=0
d x 0 = ( H T H ) − 1 H T δ ρ d x_0 = (H^T H)^{-1} H^T \delta \rho dx0=(HTH)−1HTδρ
由于线性化是基于初始点的，解算得到的 d x 0 d x_0 dx0是状态增量，更新状态量
x u = x 0 + d x 0 x_u = x_0 + d x_0 xu=x0+dx0
将 x u x_u xu作为新的初始点，重复线性化、解算和更新过程，直到 d x 0 d x_0 dx0收敛。
这个过程又称为牛顿迭代法。
一般5到6次迭代后，位置和钟差收敛。
![【GNSS原理】【最小二乘法】Chapter.5 GNSS定位算法——LS和WLS方法 [2025年4月]_第2张图片](http://img.e-com-net.com/image/info8/f8786386e8094e64965bbd674ed2024f.png)
3.进行速度钟漂计算
GNSS速度和钟漂计算基于多普勒观测，反映卫星与用户之间的相对速度。
多普勒观测方程为
f d i = D C i ⋅ ( v s i − v u ) + c b ˙ + n d i , i = 1 , 2 , … , m f_{d_i} = DC_i \cdot (v_{s_i} - v_u) + c \dot{b} + n_{d_i}, \quad i = 1, 2, \dots, m fdi=DCi⋅(vsi−vu)+cb˙+ndi,i=1,2,…,m
其中 D C i = [ − x u − x s i ρ i , − y u − y s i ρ i , − z u − z s i ρ i ] DC_i = \left[ -\frac{x_u - x_{s_i}}{\rho_i}, -\frac{y_u - y_{s_i}}{\rho_i}, -\frac{z_u - z_{s_i}}{\rho_i} \right] DCi=[−ρixu−xsi,−ρiyu−ysi,−ρizu−zsi]为单位方向矢量。
目标是解算 x v = [ v x u , v y u , v z u , b ˙ ] T x_v = [v_{x_u}, v_{y_u}, v_{z_u}, \dot{b}]^T xv=[vxu,vyu,vzu,b˙]T
多普勒观测方程是线性形式，卫星的高速运动贡献多普勒频移的大部分，将这部分去掉，自然剩下的就是由用户运动引起的部分。
f d i − D C i ⋅ v s i = − D C i ⋅ v u + c b ˙ + n d i f_{d_i} - DC_i \cdot v_{s_i} = -DC_i \cdot v_u + c \dot{b} + n_{d_i} fdi−DCi⋅vsi=−DCi⋅vu+cb˙+ndi
定义 f d i ′ = f d i − D C i ⋅ v s i f_{d_i}' = f_{d_i} - DC_i \cdot v_{s_i} fdi′=fdi−DCi⋅vsi
方程整理为
f d i ′ = u i ⋅ x v + n d i f_{d_i}' = u_i \cdot x_v + n_{d_i} fdi′=ui⋅xv+ndi
其中 u i = [ − D C i , c ] = [ − x u − x s i ρ i , − y u − y s i ρ i , − z u − z s i ρ i , c ] u_i = [-DC_i, c] = \left[ -\frac{x_u - x_{s_i}}{\rho_i}, -\frac{y_u - y_{s_i}}{\rho_i}, -\frac{z_u - z_{s_i}}{\rho_i}, c \right] ui=[−DCi,c]=[−ρixu−xsi,−ρiyu−ysi,−ρizu−zsi,c]
对于 m m m颗卫星，多普勒观测方程组写成矩阵形式为
f d ′ = H x v + n d f_d' = H x_v + n_d fd′=Hxv+nd
观测矩阵为
H = [ u 1 T , u 2 T , … , u m T ] T H= [u_1^T, u_2^T, \dots, u_m^T]^T H=[u1T,u2T,…,umT]T
最小二乘法通过最小化代价函数求解 x v x_v xv
J ( x v ) = ( f d ′ − H x v ) T ( f d ′ − H x v ) J(x_v) = (f_d' - H x_v)^T (f_d' - H x_v) J(xv)=(fd′−Hxv)T(fd′−Hxv)
对 x v x_v xv求导并令导数为零
∂ J ∂ x v = − 2 H T f d ′ + 2 H T H x v = 0 \frac{\partial J}{\partial x_v} = -2 H^T f_d' + 2 H^T H x_v = 0 ∂xv∂J=−2HTfd′+2HTHxv=0
得到极值点
x v = ( H T H ) − 1 H T f d ′ x_v = (H^T H)^{-1} H^T f_d' xv=(HTH)−1HTfd′
多普勒观测方程为线性方程，无需迭代。