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

其他 2020-05-25 14:35:28 阅读次数: 0

文章目录

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
今日推荐
NetBSD 禁止提交由 AI 生成的代码
Apache Doris 2.0.10 版本正式发布！
开源日报 | 大模型开战；大模型独角兽被曝卖身；周鸿祎建议谷歌开源所有产品；最大开源AI社区提供1000万美元共享GPU
开源日报 | Chrome内置Gemini的意义不在于Gemini；中国AI追随之路的五大误区；ECharts创始人“下海”养鱼；谷歌I/O开发者大会什么都有，只是没有惊喜
微软回应中国区AI团队“打包赴美”传闻
基于大语言模型的开源知识库问答系统 MaxKB GitHub Star 数量突破 5,000 个！
周排行
女程序员是这样被恶搞的
B/S 和 C/S 的优缺点
vector一直申请会怎样？
座头鲸识别比赛(Humpback Whale Identification)总结
Linux高性能服务器编程——I/O复用 select
Mysql连接数据库（当包使用）
通过URI获取的文件路径为null的解决方法
1022-Primes on Interval(素数筛选+二分查找) ZCMU
Python出现： TypeError: expected string or buffer
bzoj2434: [Noi2011]阿狸的打字机 ac自动机+树状数组
每日归档
更多
2024-05-18(4)
2024-05-17(34)
2024-05-16(6)
2024-05-15(24)
2024-05-14(0)
2024-05-13(18)
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)
2024-05-09(35)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022