rtklib中載波相位差分算法詳解

原創 奔跑的橘子 2020-04-04 07:53

文章目錄

1 準備知識

rtklib中相對定位部分使用擴展卡爾曼濾波實現。所以，要真正搞懂rtklib中載波相位差分定位的部分，最好先看一下kalman濾波的知識（當然點開這篇文章，想必對GNSS領域的domain knowledge是已經很熟悉的了_）。不需要很精通，以筆者10幾年斷斷續續卡爾曼濾波相關的工作經驗來看，我覺得卡爾曼濾波最好能理解以下內容
：

  • 會應用卡爾曼濾波五公式解決基本問題
  • 感性的認識到卡爾曼濾波的本質其實是模型與量測的加權
  • 能知道什麼是時間更新，什麼是量測更新
  • 給定狀態與量測能自己建立模型，即求AA陣和CC陣（有的書上叫FF陣和 HH 陣）

關於最後這一點，其實每個行業或者業務領域需要的domain knowledge千差萬別，有些很簡單比如估計個溫度，電量等等，稍微複雜一點的比如本篇文章中的ekf，當然這個複雜是相對的，其與組合導航領域的數據融合來說真是小巫見大巫了。

2 狀態向量與觀測向量的選取

卡爾曼濾波中第一步也是最重要的便是選擇狀態向量和觀測向量。只要這兩組向量確定出來了，那麼卡爾曼濾波模型就“基本”確定了。

2.1 狀態向量

量測向量xx的定義如下：
x=[rr,vr,B1,B2,B5] x=[\bold{r_r},\bold{v_r},\bold{B_1},\bold{B_2},\bold{B_5}]
其中，rr,vr\bold{r_r},\bold{v_r}分別爲三維位置向量和速度向量。Bi\bold{B_i}是m維單差模糊度，m是觀測到的衛星個數。即：
Bi=[Brb,i1,Brb,i2,...,Brb,im] \bold{B_i}=[B_{rb,i}^1,B_{rb,i}^2,...,B_{rb,i}^m]
使用單差模糊度是爲了規避曆元間參考衛星可能改變的問題。

2.2 量測向量

量測向量yy的定義如下：
y=[ϕ1,ϕ2,ϕ5,P1,P2,P5] y=[\bold{\phi_{1},\phi_{2},\phi_{5},P_1,P_2,P_5}]
其中，ϕi,Pi\phi_{i},P_i分別爲雙差載波相位和雙差僞距。

3 定義卡爾曼濾波模型

3.1 時間更新模型

以下爲運動學方程,比較簡單，就是位置是上一時刻的位置加速度乘以時間間隔，速度認爲不變，單差模糊度恆定不變。
rr(k+1)=rr(k)+vr(k)∗dt\bold{r_r}(k+1) = \bold{r_r}(k)+\bold{v_r}(k)*dt
vr(k+1)=vr(k)\bold{v_r}(k+1) = \bold{v_r}(k)
Bi(k+1)=Bi(k)\bold{B_i}(k+1) = \bold{B_i}(k)

其中dtdt爲曆元間隔，通過以上關係我們很容易得到狀態轉移矩陣AA
A=[I3,3I3,3∗dt03,3m03,3I3,303,3m03m,303m,3I3m,3m] 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是一個常值矩陣，這個是很不錯的，這不僅省去了線性化的工作。更有意思的是如果量測矩陣也是一個常值矩陣，那麼這個系統就是一個線性定常系統，對於線性定常系統卡爾曼濾波的增益矩陣（通常記爲KK陣）無需在線計算，可以提前計算出來，系統中直接應用即可。

線性定常系統的KK陣最後收斂爲一個常值矩陣，不熟悉卡爾曼濾波的可以通過卡爾曼濾波五公式自己體會。

3.2 量測更新模型

量測模型略複雜，從下邊的式子可以容易看出是非線性的，所以系統並不是線性定常系統。
ϕrbjk=ρrbjk+λ(Brbj−Brbk)\phi_{rb}^{jk}=\rho_{rb}^{jk}+\lambda(B_{rb}^j-B_{rb}^k)
Prbjk=ρrbjkP_{rb}^{jk}=\rho_{rb}^{jk}
若記y=h(x)y=h(x),則我們需要求解h的雅克比矩陣。
觀測模型可以通過matlab或者python的sympy推導出來，以下以四顆衛星爲例給出結果。
[rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2000l1−l10000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2000l10−l1000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2000l100−l100000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)20000000l2−l2000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)20000000l20−l200000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)20000000l200−l20000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)200000000000l5−l500rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)200000000000l50−l50rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)200000000000l500−l5rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2000000000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2000000000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2000000000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2000000000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2000000000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2000000000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs2(rx−xs2)2+(ry−ys2)2+(rz−zs2)2000000000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs3(rx−xs3)2+(ry−ys3)2+(rz−zs3)2000000000000000rx−xs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rx−xs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2ry−ys1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−ry−ys4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2rz−zs1(rx−xs1)2+(ry−ys1)2+(rz−zs1)2−rz−zs4(rx−xs4)2+(ry−ys4)2+(rz−zs4)2000000000000000] \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=
[−DE0λ1D00−DE00λ2D0−DE000λ5D−DE0000−DE0000−DE0000] \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−10...010−1...0...............100...−1] \begin{bmatrix} 1 & -1 & 0 & ... & 0 \\ 1 & 0 & -1 & ... & 0 \\ ... & ... & ... & ... & ... \\ 1 & 0 & 0 & ... & -1 \end{bmatrix}

4 小結

到此，想必您對rtkpos的關鍵算法已經“很”瞭解了，當然其中還有很多細節，比如共視星選取，周跳探測，不同動態模型下的F陣或者A陣選取，Q陣和R陣的確定，整週模糊度的確定算法等，這些細節將會接下來結合代碼詳細講解。

發表評論
所有評論
還沒有人評論，想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章

智慧家庭場景的推薦系統的發展歷程和方向 | InfoQ《公開課》

直播概要： 隨着計算機的蓬勃發展，互聯網進入大數據和人工智能時代，爲了解決信息過載和長尾商品，推薦系統成爲唯一選擇，而面對不同的業務場景，爲了解決業務痛點，會根據不同的場景特點尋找不同的方法和手段來解決推薦中實際遇到的問題。在智慧家庭領域，

InfoQ 中文站 2021-12-21 10:54:01

Alexa 全球排名網站將關閉，排名曾引爭議

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null},"content":[{"typ

辛晓亮 2021-12-14 14:53:55

Thinking Above Code：TLA+思維概述

{"type":"doc","content":[{"type":"blockquote","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null

李明昊 2021-12-07 17:23:58

你的2.6朵雲裏，會有火山引擎嗎？

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null},"content":[{"typ

张俊宝 2021-12-07 10:28:54

數字化轉型這麼火，你真的看懂了嗎？

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null},"content":[{"typ

张俊宝 2021-12-02 21:08:57

基於圖像的機器學習技術將數十億的電子商務產品分爲數千個類別

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null},"content":[{"typ

Celian Gossec 2021-11-29 16:28:50

如何用 PyTorch 構建 GAN？

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null},"content":[{"typ

a-ying Cheng 2021-11-23 11:18:54

繞過硬件瓶頸，成倍提升芯片算力，軟件層面深挖芯片性能可行嗎？

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null},"content":[{"typ

张俊宝 2021-11-23 11:18:54

App Annie發佈預測：TikTok 將達 15 億活躍用戶，遙遙領先 Instagram

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null},"content":[{"typ

闫园园 2021-11-19 19:53:55

不是隻有數字化水平高，纔可以落地知識圖譜

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null}},{"type":"blockq

罗燕珊 2021-11-11 15:23:53

科大訊飛在AI源頭技術上的突破，實現系統性創新

{"type":"doc","content":[{"type":"blockquote","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null

Lucien 2021-11-08 15:13:57

不滿被辭退，一程序員寫爬蟲程序侵入公司後臺刪庫泄憤，造成經濟損失10餘萬元

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null}},{"type":"blockq

刘燕 2021-11-08 14:03:51

“Trojan Source”算法漏洞幾乎影響所有代碼的安全

{"type":"doc","content":[{"type":"blockquote","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null

Brian Krebs 2021-11-05 18:33:59

谷歌前CEO發出警告：元宇宙對人類未必是好事，AI技術是“僞神”

{"type":"doc","content":[{"type":"blockquote","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null

凌敏 2021-11-02 14:03:53

騰訊發佈超大預訓練系統派大星，聚焦解決BERT等超大模型訓練時的“GPU內存牆”問題

{"type":"doc","content":[{"type":"paragraph","attrs":{"indent":0,"number":0,"align":null,"origin":null},"content":[{"typ

InfoQ编辑部 2021-11-02 13:38:53