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

2024-06-11 00:20

本文主要是介绍让GNSSRTK不再难【第二天-第3部分】,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

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

11.1 定位方程重构

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

p i s = P i s − ( X s − X 0 ) 2 + ( Y s − Y 0 ) 2 + ( Z s − Z 0 ) 2 + c δ t r − I i s − T i s − ϵ P i s 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} pis=Pis(XsX0)2+(YsY0)2+(ZsZ0)2 +trIisTisϵPis

其中:

  • p i s p_i^s pis 为残差,是观测伪距与计算伪距之间的差值。
  • P i s P_i^s Pis 为卫星 s s s 到接收机 r r r 的伪距观测值。
  • ( X s , Y s , Z s ) (X^s, Y^s, Z^s) (Xs,Ys,Zs) 为卫星 s s s 在ECEF坐标系下的位置。
  • ( X 0 , Y 0 , Z 0 ) (X_0, Y_0, Z_0) (X0,Y0,Z0) 为接收机 r r r 在ECEF坐标系下的近似位置。
  • c δ t r c\delta t^r tr 为接收机钟差的影响, c c c 为光速。
  • I i s I_i^s Iis 为电离层延迟。
  • T i s T_i^s Tis 为对流层延迟。
  • ϵ P i s \epsilon_{P_i^s} ϵPis 为伪距观测值噪声。

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

对于P1频点的伪距:

p r , 1 s = P r , 1 s − ( ( X s − X 0 ) 2 + ( Y s − Y 0 ) 2 + ( Z s − Z 0 ) 2 − Δ ρ ) + ( c δ t s − T g d ) − I r s − T r s − ϵ P 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} pr,1s=Pr,1s((XsX0)2+(YsY0)2+(ZsZ0)2 Δρ)+(tsTgd)IrsTrsϵP

其中:

  • p r , 1 s p_{r,1}^s pr,1s 为P1频点残差。
  • P r , 1 s P_{r,1}^s Pr,1s 为P1频点的伪距观测值。
  • Δ ρ \Delta \rho Δρ 为地球自转引起的改正。
  • c δ t s c\delta t^s ts 为卫星钟差。
  • T g d T_{gd} Tgd 为码偏差改正值。
  • I r s I_r^s Irs 为电离层延迟。
  • T r s T_r^s Trs 为对流层延迟。
  • ϵ P \epsilon_{P} ϵP 为伪距观测值噪声。

对于P2频点:

p r , 2 s = P r , 2 s − ( ( X s − X 0 ) 2 + ( Y s − Y 0 ) 2 + ( Z s − Z 0 ) 2 − Δ ρ ) + ( c δ t s − γ T g d ) − I r s − T r s − ϵ P 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} pr,2s=Pr,2s((XsX0)2+(YsY0)2+(ZsZ0)2 Δρ)+(tsγTgd)IrsTrsϵP

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

γ = f 1 2 f 2 2 \gamma = \frac{f_1^2}{f_2^2} γ=f22f12

其中 f 1 f_1 f1 f 2 f_2 f2 分别为P1频点和P2频点的频率。例如,对于GPS系统, f 1 = 1575.42 M H z f_1 = 1575.42 \, MHz f1=1575.42MHz f 2 = 1227.60 M H z f_2 = 1227.60 \, MHz f2=1227.60MHz。计算得到的 γ \gamma γ 为:

γ = ( 1575.42 1227.60 ) 2 ≈ 1.64694 \gamma = \left(\frac{1575.42}{1227.60}\right)^2 \approx 1.64694 γ=(1227.601575.42)21.64694

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

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

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

其中:

  • V V V 为残差向量,每个元素 p i p^i pi 为第 i i i 个观测值的残差。

  • A A A 为设计矩阵或Jacobian矩阵,其元素为:
    A = [ l 1 m 1 n 1 − 1 l 2 m 2 n 2 − 1 l 3 m 3 n 3 − 1 ⋮ ⋮ ⋮ ⋮ l n m n n n − 1 ] A = \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] A= l1l2l3lnm1m2m3mnn1n2n3nn1111
    其中 l i , m i , n i l^i, m^i, n^i li,mi,ni 分别为第 i i i 个观测值在 X , Y , Z X, Y, Z X,Y,Z 方向的系数,-1 为钟差项的系数。

  • δ x \delta x δx 为待估状态量向量,包括接收机位置的改正值和钟差:
    δ x = [ d x d y d z c δ t ] \delta x = \left[ \begin{matrix} dx \\ dy \\ dz \\ c\delta t \\ \end{matrix} \right] δx= dxdydzt

即有以上公式:

V = − A δ x V = -A \delta x V=Aδx

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

11.2 观测值权重

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

P i s = ρ i s + c δ t s + c δ t r + I i s + T i s + ϵ P i s P_i^s = \rho_i^s + c\delta t^s + c\delta t^r + I_i^s + T_i^s + \epsilon_{P_i^s} Pis=ρis+ts+tr+Iis+Tis+ϵPis

其中:

  • P i s P_i^s Pis 为观测的伪距值。
  • ρ i s \rho_i^s ρis 为卫星 s s s 到接收机 i i i 的真实伪距。
  • c δ t s c\delta t^s ts 为卫星钟差, c c c 为光速。
  • c δ t r c\delta t^r tr 为接收机钟差。
  • I i s I_i^s Iis 为电离层延迟。
  • T i s T_i^s Tis 为对流层延迟。
  • ϵ P i s \epsilon_{P_i^s} ϵPis 为伪距观测噪声。

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

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

E ( ϵ P i s ) = 0 E(\epsilon_{P_i^s}) = 0 E(ϵPis)=0

σ ( ϵ P i s ) = σ 0 sin ⁡ ( E l e v a t i o n ) \sigma(\epsilon_{P_i^s}) = \frac{\sigma_0}{\sin(Elevation)} σ(ϵPis)=sin(Elevation)σ0

其中:

  • σ 0 \sigma_0 σ0 为标准的伪距噪声,一般设定为0.3m。
  • E l e v a t i o n Elevation Elevation 为卫星的高度角。

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

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

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

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

公式:

σ 2 = ( σ ( ϵ P i s ) ) 2 + σ 2 ( o r b ) + σ 2 ( i o n ) \sigma^2 = (\sigma(\epsilon_{P_i^s}))^2 + \sigma^2(orb) + \sigma^2(ion) σ2=(σ(ϵPis))2+σ2(orb)+σ2(ion)

解释:

  • σ 2 \sigma^2 σ2:总的观测值方差,是各个误差分量方差的和。

  • ( σ ( ϵ P i s ) ) 2 (\sigma(\epsilon_{P_i^s}))^2 (σ(ϵPis))2:伪距观测值的噪声方差, σ ( ϵ P i s ) \sigma(\epsilon_{P_i^s}) σ(ϵPis) 是伪距观测噪声的标准差,其计算方式为 σ 0 sin ⁡ ( E l e v a t i o n ) \frac{\sigma_0}{\sin(Elevation)} sin(Elevation)σ0,其中 σ 0 \sigma_0 σ0 为标准伪距噪声,通常设定为0.3米, E l e v a t i o n Elevation Elevation 为卫星的高度角。

  • σ 2 ( o r b ) \sigma^2(orb) σ2(orb):轨道误差的方差。轨道误差是由于卫星轨道的不确定性引起的,可以从广播星历中的卫星位置精度字段(SV accuracy)中获得。

  • σ 2 ( i o n ) \sigma^2(ion) σ2(ion):电离层误差的方差。电离层误差是由于电离层对信号的折射和延迟引起的,通常可以通过电离层模型进行估算。

总结:

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

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

P = [ 1 σ 1 2 0 0 ⋯ 0 0 1 σ 2 2 0 ⋯ 0 0 0 1 σ 3 2 ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ 0 0 0 ⋯ 1 σ n 2 ] P = \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] P= σ1210000σ2210000σ3210000σn21

其中:

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

11.3 最小二乘

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

V T ∗ P ∗ V = m i n V^T * P * V = min VTPV=min

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

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

δ x = − ( A T P A ) − 1 ∗ ( A T P V ) \delta x = -(A^T PA)^{-1} * (A^T PV) δx=(ATPA)1(ATPV)

通过上文中的公式

X r = X 0 + d x Y r = Y 0 + d y Z r = Z 0 + d z X_r = X_0 + dx \\ Y_r = Y_0 + dy \\ Z_r = Z_0 + dz Xr=X0+dxYr=Y0+dyZr=Z0+dz

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


元素解释:

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

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

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

推导过程

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

假设我们有 n n n个观测方程,每个观测方程表示为:

P i = f ( X ) + ϵ i P_i = f(X) + \epsilon_i Pi=f(X)+ϵi

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

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

min ⁡ ∑ i = 1 n ϵ i 2 \min \sum_{i=1}^n \epsilon_i^2 mini=1nϵi2

线性化观测方程

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

f ( X + δ X ) ≈ f ( X ) + ∂ f ∂ X δ X f(X + \delta X) \approx f(X) + \frac{\partial f}{\partial X} \delta X f(X+δX)f(X)+XfδX

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

P i ≈ f ( X ) + ∂ f ∂ X δ X + ϵ i P_i \approx f(X) + \frac{\partial f}{\partial X} \delta X + \epsilon_i Pif(X)+XfδX+ϵi

矩阵形式表示

引入设计矩阵 A A A,其元素为 ∂ f i ∂ X j \frac{\partial f_i}{\partial X_j} Xjfi,我们可以将所有观测方程写成矩阵形式:

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

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

目标函数

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

J = ϵ T ϵ = ( P − f ( X ) − A δ X ) T ( P − f ( X ) − A δ X ) J = \epsilon^T \epsilon = (\mathbf{P} - \mathbf{f}(X) - \mathbf{A} \delta X)^T (\mathbf{P} - \mathbf{f}(X) - \mathbf{A} \delta X) J=ϵTϵ=(Pf(X)AδX)T(Pf(X)AδX)

其中:

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

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


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

在数学和几何中, ϵ T ϵ \epsilon^T \epsilon ϵTϵ 具有以下含义:

  1. 数学上的含义:

    • ϵ \epsilon ϵ 是一个误差向量,它的每个元素表示一个观测值和理论值之间的差异。
    • ϵ T \epsilon^T ϵT 是误差向量 ϵ \epsilon ϵ 的转置向量。
    • ϵ T ϵ \epsilon^T \epsilon ϵTϵ 表示误差向量与其自身的内积(也叫点积)。
  2. 几何上的含义:

    • 在几何上, ϵ T ϵ \epsilon^T \epsilon ϵTϵ 表示误差向量的平方和。
    • 从向量的角度看, ϵ T ϵ \epsilon^T \epsilon ϵTϵ 实际上是误差向量的长度(范数)的平方。
  3. 通俗易懂的理解:

    • 假设你有几个测量值和对应的理论值, ϵ \epsilon ϵ 表示每个测量值与理论值的差异。
    • ϵ T ϵ \epsilon^T \epsilon ϵTϵ 就是将这些差异平方后相加的总和。
    • 这表示所有测量误差的总量,是衡量测量精度的一种方式。

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


求解目标函数

对目标函数 J J J δ X \delta X δX 的偏导数,并令其等于0,即:

∂ J ∂ δ X = − 2 A T ( P − f ( X ) − A δ X ) = 0 \frac{\partial J}{\partial \delta X} = -2 \mathbf{A}^T (\mathbf{P} - \mathbf{f}(X) - \mathbf{A} \delta X) = 0 δXJ=2AT(Pf(X)AδX)=0

解此方程,得到最小二乘解:

A T A δ X = A T ( P − f ( X ) ) \mathbf{A}^T \mathbf{A} \delta X = \mathbf{A}^T (\mathbf{P} - \mathbf{f}(X)) ATAδX=AT(Pf(X))

即:

δ X = ( A T A ) − 1 A T ( P − f ( X ) ) \delta X = (\mathbf{A}^T \mathbf{A})^{-1} \mathbf{A}^T (\mathbf{P} - \mathbf{f}(X)) δX=(ATA)1AT(Pf(X))

引入残差向量

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

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

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

δ X = ( A T A ) − 1 A T V \delta X = (\mathbf{A}^T \mathbf{A})^{-1} \mathbf{A}^T \mathbf{V} δX=(ATA)1ATV

考虑权矩阵

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

δ X = ( A T P A ) − 1 A T P V \delta X = (\mathbf{A}^T \mathbf{P} \mathbf{A})^{-1} \mathbf{A}^T \mathbf{P} \mathbf{V} δX=(ATPA)1ATPV

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

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

举例

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

观测方程为:
P 1 = X 1 + 2 X 2 + ϵ 1 P_1 = X_1 + 2X_2 + \epsilon_1 P1=X1+2X2+ϵ1

P 2 = 3 X 1 + 4 X 2 + ϵ 2 P_2 = 3X_1 + 4X_2 + \epsilon_2 P2=3X1+4X2+ϵ2

P 3 = 5 X 1 + 6 X 2 + ϵ 3 P_3 = 5X_1 + 6X_2 + \epsilon_3 P3=5X1+6X2+ϵ3

观测值为:
P = [ 2 8 12 ] \mathbf{P} = \begin{bmatrix} 2 \\ 8 \\ 12 \end{bmatrix} P= 2812

初始状态量为:
X = [ 1 1 ] \mathbf{X} = \begin{bmatrix} 1 \\ 1 \end{bmatrix} X=[11]

设计矩阵 A \mathbf{A} A 为:
A = [ 1 2 3 4 5 6 ] \mathbf{A} = \begin{bmatrix} 1 & 2 \\ 3 & 4 \\ 5 & 6 \end{bmatrix} A= 135246

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

使用初始状态量 X \mathbf{X} X 计算理论值向量 f ( X ) \mathbf{f}(X) f(X)
f ( X ) = A X = [ 1 2 3 4 5 6 ] [ 1 1 ] = [ 1 ∗ 1 + 2 ∗ 1 3 ∗ 1 + 4 ∗ 1 5 ∗ 1 + 6 ∗ 1 ] = [ 3 7 11 ] \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} f(X)=AX= 135246 [11]= 11+2131+4151+61 = 3711

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

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

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

A T A = [ 1 3 5 2 4 6 ] [ 1 2 3 4 5 6 ] = [ 1 ∗ 1 + 3 ∗ 3 + 5 ∗ 5 1 ∗ 2 + 3 ∗ 4 + 5 ∗ 6 2 ∗ 1 + 4 ∗ 3 + 6 ∗ 5 2 ∗ 2 + 4 ∗ 4 + 6 ∗ 6 ] = [ 35 44 44 56 ] \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} ATA=[123456] 135246 =[11+33+5521+43+6512+34+5622+44+66]=[35444456]

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

A T V = [ 1 3 5 2 4 6 ] [ − 1 1 1 ] = [ 1 ∗ − 1 + 3 ∗ 1 + 5 ∗ 1 2 ∗ − 1 + 4 ∗ 1 + 6 ∗ 1 ] = [ 7 8 ] \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} ATV=[123456] 111 =[11+31+5121+41+61]=[78]

求解 δ X \delta X δX

δ X = ( A T A ) − 1 A T V = [ 35 44 44 56 ] − 1 [ 7 8 ] \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} δX=(ATA)1ATV=[35444456]1[78]

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

( A T A ) − 1 = 1 ( 35 ) ( 56 ) − ( 44 ) 2 [ 56 − 44 − 44 35 ] = 1 196 [ 56 − 44 − 44 35 ] = [ 0.2857 − 0.2245 − 0.2245 0.1786 ] (\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} (ATA)1=(35)(56)(44)21[56444435]=1961[56444435]=[0.28570.22450.22450.1786]

最终计算 δ X \delta X δX

δ X = [ 0.2857 − 0.2245 − 0.2245 0.1786 ] [ 7 8 ] = [ 0.2857 ∗ 7 + ( − 0.2245 ) ∗ 8 − 0.2245 ∗ 7 + 0.1786 ∗ 8 ] = [ 2.0009 − 0.2862 ] \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} δX=[0.28570.22450.22450.1786][78]=[0.28577+(0.2245)80.22457+0.17868]=[2.00090.2862]

修正量 δ X = [ 2.0009 − 0.2862 ] \delta X = \begin{bmatrix} 2.0009 \\ -0.2862 \end{bmatrix} δX=[2.00090.2862]

最终修正后的状态量为:
X + δ X = [ 1 1 ] + [ 2.0009 − 0.2862 ] = [ 3.0009 0.7138 ] \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} X+δX=[11]+[2.00090.2862]=[3.00090.7138]

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

11.4 定位精度因子

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

如果我们假设所有卫星的URE是均相等为 σ U R E \sigma_{URE} σURE,那么给出定位的精度:

σ X , Y , Z , T = ( A T A ) − 1 σ U R E \sigma_{X,Y,Z,T} = (A^T A)^{-1} \sigma_{URE} σX,Y,Z,T=(ATA)1σURE

我们令

Q = ( A T A ) − 1 Q = (A^T A)^{-1} Q=(ATA)1

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

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

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

Q = [ q X X q X Y q X Z q X T q Y X q Y Y q Y Z q Y T q Z X q Z Y q Z Z q Z T q T X q T Y q T Z q T T ] Q = \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} Q= qXXqYXqZXqTXqXYqYYqZYqTYqXZqYZqZZqTZqXTqYTqZTqTT

精度因子:

  • G D O P = q X X + q Y Y + q Z Z + q T T GDOP = \sqrt{q_{XX} + q_{YY} + q_{ZZ} + q_{TT}} GDOP=qXX+qYY+qZZ+qTT 称为几何精度衰减因子。
  • P D O P = q X X + q Y Y + q Z Z PDOP = \sqrt{q_{XX} + q_{YY} + q_{ZZ}} PDOP=qXX+qYY+qZZ 称为空间精度衰减因子。
  • T D O P = q T T TDOP = \sqrt{q_{TT}} TDOP=qTT 称为钟差精度衰减因子。
  • H D O P = q X X + q Y Y HDOP = \sqrt{q_{XX} + q_{YY}} HDOP=qXX+qYY 称为水平精度衰减因子。
  • V D O P = q Z Z VDOP = \sqrt{q_{ZZ}} VDOP=qZZ 称为高度精度衰减因子。

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

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

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

UERE的计算

如果我们假设所有卫星的UERE均相等为 σ U E R E \sigma_{UERE} σUERE,那么给出定位精度的公式为:

σ X , Y , Z , T = ( A T A ) − 1 σ U E R E \sigma_{X,Y,Z,T} = (A^T A)^{-1} \sigma_{UERE} σX,Y,Z,T=(ATA)1σUERE

其中:

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

我们引入矩阵 Q Q Q

Q = ( A T A ) − 1 Q = (A^T A)^{-1} Q=(ATA)1

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

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

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

Q Q Q 矩阵的解释

Q Q Q 矩阵是一个 4 × 4 4 \times 4 4×4 的矩阵:

Q = [ q X X q X Y q X Z q X T q Y X q Y Y q Y Z q Y T q Z X q Z Y q Z Z q Z T q T X q T Y q T Z q T T ] Q = \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} Q= qXXqYXqZXqTXqXYqYYqZYqTYqXZqYZqZZqTZqXTqYTqZTqTT

这个矩阵的各个元素表示不同方向上的误差放大因子。

几种常见的定位精度因子
  1. GDOP(几何精度衰减因子)

G D O P = q X X + q Y Y + q Z Z + q T T GDOP = \sqrt{q_{XX} + q_{YY} + q_{ZZ} + q_{TT}} GDOP=qXX+qYY+qZZ+qTT

表示的是几何构型对所有测量误差放大的综合影响。

  1. PDOP(位置精度衰减因子)

P D O P = q X X + q Y Y + q Z Z PDOP = \sqrt{q_{XX} + q_{YY} + q_{ZZ}} PDOP=qXX+qYY+qZZ

表示的是几何构型对位置测量误差的影响。

  1. TDOP(时间精度衰减因子)

T D O P = q T T TDOP = \sqrt{q_{TT}} TDOP=qTT

表示的是几何构型对时间测量误差的影响。

  1. HDOP(水平精度衰减因子)

H D O P = q X X + q Y Y HDOP = \sqrt{q_{XX} + q_{YY}} HDOP=qXX+qYY

表示的是几何构型对水平位置测量误差的影响。

  1. VDOP(垂直精度衰减因子)

V D O P = q Z Z VDOP = \sqrt{q_{ZZ}} VDOP=qZZ

表示的是几何构型对垂直位置测量误差的影响。

实例解释

假设我们在某个时间点观测到了4颗卫星的信号,经过计算得到了设计矩阵 A A A,并求得了 Q = ( A T A ) − 1 Q = (A^T A)^{-1} Q=(ATA)1,其具体数值如下:

Q = [ 0.1 0.02 0.01 0.03 0.02 0.1 0.01 0.02 0.01 0.01 0.1 0.02 0.03 0.02 0.02 0.1 ] Q = \begin{bmatrix} 0.1 & 0.02 & 0.01 & 0.03 \\ 0.02 & 0.1 & 0.01 & 0.02 \\ 0.01 & 0.01 & 0.1 & 0.02 \\ 0.03 & 0.02 & 0.02 & 0.1 \end{bmatrix} Q= 0.10.020.010.030.020.10.010.020.010.010.10.020.030.020.020.1

根据这个矩阵,我们可以计算出各种定位精度因子:

  1. GDOP

G D O P = 0.1 + 0.1 + 0.1 + 0.1 = 0.4 ≈ 0.632 GDOP = \sqrt{0.1 + 0.1 + 0.1 + 0.1} = \sqrt{0.4} \approx 0.632 GDOP=0.1+0.1+0.1+0.1 =0.4 0.632

  1. PDOP

P D O P = 0.1 + 0.1 + 0.1 = 0.3 ≈ 0.548 PDOP = \sqrt{0.1 + 0.1 + 0.1} = \sqrt{0.3} \approx 0.548 PDOP=0.1+0.1+0.1 =0.3 0.548

  1. TDOP

T D O P = 0.1 = 0.316 TDOP = \sqrt{0.1} = 0.316 TDOP=0.1 =0.316

  1. HDOP

H D O P = 0.1 + 0.1 = 0.2 ≈ 0.447 HDOP = \sqrt{0.1 + 0.1} = \sqrt{0.2} \approx 0.447 HDOP=0.1+0.1 =0.2 0.447

  1. VDOP

V D O P = 0.1 = 0.316 VDOP = \sqrt{0.1} = 0.316 VDOP=0.1 =0.316

通过这些计算,我们可以看到,不同的精度因子反映了不同方向上的误差放大情况。GDOP表示的是总体的误差放大,而PDOP、TDOP、HDOP和VDOP分别表示位置、时间、水平和垂直方向上的误差放大。

11.5 算法定位流程

刚开始定位时,我们⽆法知道接收机的概略位置,所以可以给(0,0,0)作为初值。通过迭代,逐渐收敛到真实位置坐标。
在这里插入图片描述

这篇关于让GNSSRTK不再难【第二天-第3部分】的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/1049687

相关文章

poj 2976 分数规划二分贪心(部分对总体的贡献度) poj 3111

poj 2976: 题意: 在n场考试中,每场考试共有b题,答对的题目有a题。 允许去掉k场考试,求能达到的最高正确率是多少。 解析: 假设已知准确率为x,则每场考试对于准确率的贡献值为: a - b * x,将贡献值大的排序排在前面舍弃掉后k个。 然后二分x就行了。 代码: #include <iostream>#include <cstdio>#incl

负债不再是障碍?银行信贷“白名单“揭秘

谈及银行信贷产品,常闻有言称存在无需考量负债与查询记录之奇品,此等说法十有八九为中介诱人上钩之辞。轻信之下,恐将步入连环陷阱。除非个人资质出类拔萃,如就职于国央企或事业单位,工龄逾年,五险一金完备,还款能力卓越,或能偶遇线下产品对查询记录稍显宽容,然亦非全然无视。宣称全然不顾者,纯属无稽之谈。 银行非慈善机构,不轻易于困境中援手,更偏爱锦上添花之举。若无坚实资质,即便求助于银行亦难获青睐。反

笔记整理—内核!启动!—kernel部分(2)从汇编阶段到start_kernel

kernel起始与ENTRY(stext),和uboot一样,都是从汇编阶段开始的,因为对于kernel而言,还没进行栈的维护,所以无法使用c语言。_HEAD定义了后面代码属于段名为.head .text的段。         内核起始部分代码被解压代码调用,前面关于uboot的文章中有提到过(eg:zImage)。uboot启动是无条件的,只要代码的位置对,上电就工作,kern

Java基础回顾系列-第二天-面向对象编程

面向对象编程 Java类核心开发结构面向对象封装继承多态 抽象类abstract接口interface抽象类与接口的区别深入分析类与对象内存分析 继承extends重写(Override)与重载(Overload)重写(Override)重载(Overload)重写与重载之间的区别总结 this关键字static关键字static变量static方法static代码块 代码块String类特

项目实战系列三: 家居购项目 第四部分

购物车 🌳购物车🍆显示购物车🍆更改商品数量🍆清空购物车&&删除商品 🌳生成订单 🌳购物车 需求分析 1.会员登陆后, 可以添加家居到购物车 2.完成购物车的设计和实现 3.每添加一个家居,购物车的数量+1, 并显示 程序框架图 1.新建src/com/zzw/furns/entity/CartItem.java, CartItem-家居项模型 /***

码蹄集部分题目(2024OJ赛9.4-9.8;线段树+树状数组)

1🐋🐋配对最小值(王者;树状数组) 时间限制:1秒 占用内存:64M 🐟题目思路 MT3065 配对最小值_哔哩哔哩_bilibili 🐟代码 #include<bits/stdc++.h> using namespace std;const int N=1e5+7;int a[N],b[N],c[N],n,q;struct QUERY{int l,r,id;}que

关于断言的部分用法

1、带变量的断言  systemVerilog assertion 中variable delay的使用,##[variable],带变量的延时(可变延时)_assertion中的延时-CSDN博客 2、until 的使用 systemVerilog assertion 中until的使用_verilog until-CSDN博客 3、throughout的使用   常用于断言和假设中的

牛客小白月赛100部分题解

比赛地址:牛客小白月赛100_ACM/NOI/CSP/CCPC/ICPC算法编程高难度练习赛_牛客竞赛OJ A.ACM中的A题 #include<bits/stdc++.h>using namespace std;#define ll long long#define ull = unsigned long longvoid solve() {ll a,b,c;cin>>a>>b>

VB和51单片机串口通信讲解(只针对VB部分)

标记:该篇文章全部搬自如下网址:http://www.crystalradio.cn/thread-321839-1-1.html,谢谢啦            里面关于中文接收的部分,大家可以好好学习下,题主也在研究中................... Commport;设置或返回串口号。 SettingS:以字符串的形式设置或返回串口通信参数。 Portopen:设置或返回串口