rtklib二之载波相位差分算法详解

1 准备知识

rtklib中相对定位部分使用扩展卡尔曼滤波实现。所以,要真正搞懂rtklib中载波相位差分定位的部分,最好先看一下kalman滤波的知识(当然点开这篇文章,想必对GNSS领域的domain knowledge是已经很熟悉的了_)。不需要很精通,以笔者10几年断断续续卡尔曼滤波相关的工作经验来看,我觉得卡尔曼滤波最好能理解以下内容

  • 会应用卡尔曼滤波五公式解决基本问题
  • 感性的认识到卡尔曼滤波的本质其实是模型与量测的加权
  • 能知道什么是时间更新,什么是量测更新
  • 给定状态与量测能自己建立模型,即求 A A 阵和 C C 阵(有的书上叫 F F 阵和 H H 阵)

关于最后这一点,其实每个行业或者业务领域需要的domain knowledge千差万别,有些很简单比如估计个温度,电量等等,稍微复杂一点的比如本篇文章中的ekf,当然这个复杂是相对的,其与组合导航领域的数据融合来说真是小巫见大巫了。

2 状态向量与观测向量的选取

卡尔曼滤波中第一步也是最重要的便是选择状态向量和观测向量。只要这两组向量确定出来了,那么卡尔曼滤波模型就“基本”确定了。

2.1 状态向量

量测向量 x x 的定义如下:
x = [ r r , v r , B 1 , B 2 , B 5 ] x=[\bold{r_r},\bold{v_r},\bold{B_1},\bold{B_2},\bold{B_5}]
其中, r r , v r \bold{r_r},\bold{v_r} 分别为三维位置向量和速度向量 B i \bold{B_i} 是m维单差模糊度,m是观测到的卫星个数。即:
B i = [ B r b , i 1 , B r b , i 2 , . . . , B r b , i m ] \bold{B_i}=[B_{rb,i}^1,B_{rb,i}^2,...,B_{rb,i}^m]
使用单差模糊度是为了规避历元间参考卫星可能改变的问题。

2.2 量测向量

量测向量 y y 的定义如下:
y = [ ϕ 1 , ϕ 2 , ϕ 5 , P 1 , P 2 , P 5 ] y=[\bold{\phi_{1},\phi_{2},\phi_{5},P_1,P_2,P_5}]
其中, ϕ i , P i \phi_{i},P_i 分别为双差载波相位和双差伪距。

3 定义卡尔曼滤波模型

3.1 时间更新模型

以下为运动学方程,比较简单,就是位置是上一时刻的位置加速度乘以时间间隔,速度认为不变,单差模糊度恒定不变。
r r ( k + 1 ) = r r ( k ) + v r ( k ) d t \bold{r_r}(k+1) = \bold{r_r}(k)+\bold{v_r}(k)*dt
v r ( k + 1 ) = v r ( k ) \bold{v_r}(k+1) = \bold{v_r}(k)
B i ( k + 1 ) = B i ( k ) \bold{B_i}(k+1) = \bold{B_i}(k)

其中 d t dt 为历元间隔,通过以上关系我们很容易得到状态转移矩阵 A A
A = [ I 3 , 3 I 3 , 3 d t 0 3 , 3 m 0 3 , 3 I 3 , 3 0 3 , 3 m 0 3 m , 3 0 3 m , 3 I 3 m , 3 m ] A=\begin{bmatrix} I_{3,3} & I_{3,3}*dt & 0_{3,3m} \\ 0_{3,3} & I_{3,3} & 0_{3,3m} \\ 0_{3m,3} & 0_{3m,3} & I_{3m,3m} \end{bmatrix}
从上面的结果可以看出A是一个常值矩阵,这个是很不错的,这不仅省去了线性化的工作。更有意思的是如果量测矩阵也是一个常值矩阵,那么这个系统就是一个线性定常系统,对于线性定常系统卡尔曼滤波的增益矩阵(通常记为 K K 阵)无需在线计算,可以提前计算出来,系统中直接应用即可。

线性定常系统的 K K 阵最后收敛为一个常值矩阵,不熟悉卡尔曼滤波的可以通过卡尔曼滤波五公式自己体会。

3.2 量测更新模型

量测模型略复杂,从下边的式子可以容易看出是非线性的,所以系统并不是线性定常系统。
ϕ r b j k = ρ r b j k + λ ( B r b j B r b k ) \phi_{rb}^{jk}=\rho_{rb}^{jk}+\lambda(B_{rb}^j-B_{rb}^k)
P r b j k = ρ r b j k P_{rb}^{jk}=\rho_{rb}^{jk}
若记 y = h ( x ) y=h(x) ,则我们需要求解h的雅克比矩阵。
观测模型可以通过matlab或者python的sympy推导出来,以下以四颗卫星为例给出结果。
[ r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 0 0 0 l 1 l 1 0 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 0 0 0 l 1 0 l 1 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 0 0 0 l 1 0 0 l 1 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 0 0 0 0 0 0 0 l 2 l 2 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 0 0 0 0 0 0 0 l 2 0 l 2 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 0 0 0 0 0 0 0 l 2 0 0 l 2 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 0 0 0 0 0 0 0 0 0 0 0 l 5 l 5 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 0 0 0 0 0 0 0 0 0 0 0 l 5 0 l 5 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 0 0 0 0 0 0 0 0 0 0 0 l 5 0 0 l 5 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 2 ( r x x s 2 ) 2 + ( r y y s 2 ) 2 + ( r z z s 2 ) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 3 ( r x x s 3 ) 2 + ( r y y s 3 ) 2 + ( r z z s 3 ) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 r x x s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r x x s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r y y s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r y y s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 r z z s 1 ( r x x s 1 ) 2 + ( r y y s 1 ) 2 + ( r z z s 1 ) 2 r z z s 4 ( r x x s 4 ) 2 + ( r y y s 4 ) 2 + ( r z z s 4 ) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] \left[\begin{array}{cccccccccccccccccc}\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & 0 & 0 & 0 & l_{1} & - l_{1} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & 0 & 0 & 0 & l_{1} & 0 & - l_{1} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & 0 & 0 & 0 & l_{1} & 0 & 0 & - l_{1} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & l_{2} & - l_{2} & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & l_{2} & 0 & - l_{2} & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & l_{2} & 0 & 0 & - l_{2} & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & l_{5} & - l_{5} & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & l_{5} & 0 & - l_{5} & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & l_{5} & 0 & 0 & - l_{5}\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{2}}{\sqrt{\left(rx - xs_{2}\right)^{2} + \left(ry - ys_{2}\right)^{2} + \left(rz - zs_{2}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{3}}{\sqrt{\left(rx - xs_{3}\right)^{2} + \left(ry - ys_{3}\right)^{2} + \left(rz - zs_{3}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\frac{rx - xs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rx - xs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{ry - ys_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{ry - ys_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & \frac{rz - zs_{1}}{\sqrt{\left(rx - xs_{1}\right)^{2} + \left(ry - ys_{1}\right)^{2} + \left(rz - zs_{1}\right)^{2}}} - \frac{rz - zs_{4}}{\sqrt{\left(rx - xs_{4}\right)^{2} + \left(ry - ys_{4}\right)^{2} + \left(rz - zs_{4}\right)^{2}}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]

可以抽象成以下形式,
C = C=
[ D E 0 λ 1 D 0 0 D E 0 0 λ 2 D 0 D E 0 0 0 λ 5 D D E 0 0 0 0 D E 0 0 0 0 D E 0 0 0 0 ] \begin{bmatrix} -DE & 0 & \lambda_1D & 0 & 0 \\ -DE & 0 & 0 & \lambda_2D & 0 \\ -DE & 0 & 0 & 0 & \lambda_5D \\ -DE & 0 & 0 & 0 & 0 \\ -DE & 0 & 0 & 0 & 0 \\ -DE & 0 & 0 & 0 & 0 \end{bmatrix}
其中,
D = D=
[ 1 1 0 . . . 0 1 0 1 . . . 0 . . . . . . . . . . . . . . . 1 0 0 . . . 1 ] \begin{bmatrix} 1 & -1 & 0 & ... & 0 \\ 1 & 0 & -1 & ... & 0 \\ ... & ... & ... & ... & ... \\ 1 & 0 & 0 & ... & -1 \end{bmatrix}

4 小结

到此,想必您对rtkpos的关键算法已经“很”了解了,当然其中还有很多细节,比如共视星选取,周跳探测,不同动态模型下的F阵或者A阵选取,Q阵和R阵的确定,整周模糊度的确定算法等,这些细节将会接下来结合代码详细讲解。

猜你喜欢

转载自blog.csdn.net/iceboy314159/article/details/105302464