rtklib二之载波相位差分算法详解
文章目录
- 1 准备知识
- 2 状态向量与观测向量的选取
- 2.1 状态向量
- 2.2 量测向量
- 3 定义卡尔曼滤波模型
- 3.1 时间更新模型
- 3.2 量测更新模型
- 4 小结
1 准备知识
rtklib中相对定位部分使用扩展卡尔曼滤波实现。所以，要真正搞懂rtklib中载波相位差分定位的部分，最好先看一下kalman滤波的知识（当然点开这篇文章，想必对GNSS领域的domain knowledge是已经很熟悉的了_）。不需要很精通，以笔者10几年断断续续卡尔曼滤波相关的工作经验来看，我觉得卡尔曼滤波最好能理解以下内容
：
- 会应用卡尔曼滤波五公式解决基本问题
- 感性的认识到卡尔曼滤波的本质其实是模型与量测的加权
- 能知道什么是时间更新，什么是量测更新
- 给定状态与量测能自己建立模型，即求 A A A阵和 C C C阵（有的书上叫 F F F阵和 H H H 阵）
关于最后这一点，其实每个行业或者业务领域需要的domain knowledge千差万别，有些很简单比如估计个温度，电量等等，稍微复杂一点的比如本篇文章中的ekf，当然这个复杂是相对的，其与组合导航领域的数据融合来说真是小巫见大巫了。
2 状态向量与观测向量的选取
卡尔曼滤波中第一步也是最重要的便是选择状态向量和观测向量。只要这两组向量确定出来了，那么卡尔曼滤波模型就“基本”确定了。
2.1 状态向量
量测向量 x 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}] x=[rr,vr,B1,B2,B5]
其中， r r , v r \bold{r_r},\bold{v_r} rr,vr分别为三维位置向量和速度向量。 B i \bold{B_i} Bi是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] Bi=[Brb,i1,Brb,i2,...,Brb,im]
使用单差模糊度是为了规避历元间参考卫星可能改变的问题。
2.2 量测向量
量测向量 y 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}] y=[ϕ1,ϕ2,ϕ5,P1,P2,P5]
其中， ϕ i , P i \phi_{i},P_i ϕi,Pi分别为双差载波相位和双差伪距。
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 rr(k+1)=rr(k)+vr(k)∗dt
v r ( k + 1 ) = v r ( k ) \bold{v_r}(k+1) = \bold{v_r}(k) vr(k+1)=vr(k)
B i ( k + 1 ) = B i ( k ) \bold{B_i}(k+1) = \bold{B_i}(k) Bi(k+1)=Bi(k)
其中 d t dt dt为历元间隔，通过以上关系我们很容易得到状态转移矩阵 A 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=⎣⎡I3,303,303m,3I3,3∗dtI3,303m,303,3m03,3mI3m,3m⎦⎤
从上面的结果可以看出A是一个常值矩阵，这个是很不错的，这不仅省去了线性化的工作。更有意思的是如果量测矩阵也是一个常值矩阵，那么这个系统就是一个线性定常系统，对于线性定常系统卡尔曼滤波的增益矩阵（通常记为 K K 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) ϕrbjk=ρrbjk+λ(Brbj−Brbk)
P r b j k = ρ r b j k P_{rb}^{jk}=\rho_{rb}^{jk} Prbjk=ρrbjk
若记 y = h ( x ) 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] ⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rx−xs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rx−xs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rx−xs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rx−xs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2ry−ys1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rz−zs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rz−zs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rz−zs4(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs2(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs3(rx−xs1)2+(ry−ys1)2+(rz−zs1)2rz−zs1−(rx−xs4)2+(ry−ys4)2+(rz−zs4)2