rtklib三之relpos rtkpo庖丁解牛

寫在前面

爲什麼relpos看起來這麼晦澀

relpos這個函數是rtklib高精度相對定位的核心函數，完成了相對定位的絕大部分工作。雖說其核心算法不外乎用於解算浮點解的EKF和固定模糊度的lambda算法，但真正的c代碼實現卻真的讓人卻步。之所以看起來有點兒費勁，個人認爲有以下幾點原因：

• 首先relpos代碼長度250行左右，絕對是一個大函數。

總之，十行以內是整潔的函數比較合適的長度，若沒有特殊情況，我們最好將單個函數控制在十行以內.《代碼整潔之道》

• C語言寫的，很多很多的變量寫在函數開始，當然這些在讀函數的時候可以先不關注
• relpos中調用的子函數也很長，如ddres
• relpos的子函數中包含了GNSS領域的domain knowledge，如無差觀測生成，雙差觀測量和雅克比矩陣的計算,也包含一些比較專業的算法如EKF算法和用於解決混合整數最小二乘的算法，如果不懂這些知識強看代碼也會很頭痛。
• 函數似乎沒有遵循講故事原則，即代碼都在一個邏輯層次。個人愚見，按邏輯層次分層寫出來的relpos可能長下邊這個模樣：

void relpos() {
    ekf();    // 計算浮點解
    lambda(); // 計算固定解
}
void ekf(){
    temporal_update(); // 卡爾曼濾波的時間更新
    meas_update();     // 卡爾曼濾波的量測更新
}
...

怎麼讀懂

一個原則：從上到下的閱讀策略。具體說來就是無論是relpos還是其子函數，閱讀當前函數時，對於子函數只關注（且讀懂）其輸入輸出，其內容不做深究。
只有當輸入輸出無法從上下文和註釋推測時，才進行最小限度的子函數展開閱讀。

Top邏輯層

按照從上向下的原則，首先看relpos的輸入輸出，觀測數據obs和星曆nav當然是必須的，nu和nr分別是移動站和基準站的觀測值條數，rtk這個結構有多種用途，作爲輸出結果的承載結構，rtk->opt中含有解算參數，另外其中還包含了過程參數如浮點解，固定解，及其對應的P陣，這些既是輸入又是輸出。

/* relpos()relative positioning ------------------------------------------------------
 *  args:  rtk      IO      gps solution structure
           obs      I       satellite observations
           nu       I       # of user observations (rover)
           nr       I       # of ref observations  (base)
           nav      I       satellite navigation data
 */

還是先上張圖，不過個人覺得作用不是很大~

衛星位置解算

/* compute satellite positions, velocities and clocks */
    satposs(time,obs,n,nav,opt->sateph,rs,dts,var,svh);

返回值rs是衛星的位置速度數組，rs=mat(6,n),其返回的數據結構如下：

字段	類型	數據描述	單位
$x^1$	double	衛星位置 $x$	$m$
$y^1$	double	衛星位置 $y$	$m$
$z^1$	double	衛星位置 $z$	$m$
$v_x^1$	double	衛星速度$v_x$	$m/s$
$v_y^1$	double	衛星速度$v_y$	$m/s$
$v_z^1$	double	衛星速度$v_z$	$m/s$
…	double[6]	下一衛星的位置速度

返回值dts是衛星的鐘差鍾漂，dts=mat(2,n),其返回的數據結構如下：

字段	類型	數據描述	單位
$dt_1$	double	衛星位置鐘差	$s$
$\dot{dt_1}$	double	衛星鐘漂	$s/s$
…	double[2]	下一衛星的鐘差鍾漂

基準站無差觀測

我們知道rtklib中浮點解是用ekf算法計算的，ekf的量測更新要有量測向量和線性化的量測矩陣，從無差觀測值開始，量測向量的計算開始了。

if (!zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,var+nu,svh+nu,nav,rtk->rb,opt,1,
               y+nu*nf*2,e+nu*3,azel+nu*2)) {
    ...
}

返回值y+nu*nf*2是基準站的無差觀測向量，y=mat(nf*2,n);,nf在iono-free模型下是1，我們就以這個模型爲例，其返回的數據結構如下：

字段	類型	數據描述	單位
$\lambda \phi_1-r_b^1$	double	phase-r	$m$
$P_1-r_b^1$	double	code-r	$m$
…	double[2]	下一衛星對應的無差觀測

$\lambda$爲消電離層組合觀測的波長

返回值e+nu*3是基準站的視線向量，e=mat(3,n);,其返回的數據結構如下：

字段	類型	數據描述	單位
$e_x$	double	$\frac{x_s^1-x_b}{r}$	NA
$e_y$	double	$\frac{y_s^1-y_b}{r}$	NA
$e_z$	double	$\frac{z_s^1-z_b}{r}$	NA
…	double[3]	下一衛星對應的視線向量

其中，$(x_s^i,y_s^i,z_s^i)$爲衛星$i$的位置，$(x_b,y_b,z_b)$爲基準站座標，均爲ecef座標系。$r_i=\sqrt{(x_s^i-x_b)^2+(y_s^i-y_b)^2+(z_s^i-z_b)^2}$。

返回值azel+nu*2是基準站的方位角仰角向量，azel=zeros(2,n);,其返回的數據結構如下：

字段	類型	數據描述	單位
$az$	double	方位角	$[0,2\pi)$
$el$	double	仰角	$[\frac{-\pi}{2},\frac{\pi}{2}]$
…	double[2]	下一衛星對應的方位角仰角

共視星

共視星是基準站和移動站能同時“看到”的衛星，只有基準站或者移動站能看到的衛星不參與高精度相對定位。

if ((ns=selsat(obs,azel,nu,nr,opt,sat,iu,ir))<=0) {

用以例子來表示selsat輸入與輸出之間的關係，
obs中sat依次爲[1,2,3,4,5,7,9,23, 2,3,4,6,23,32]
這個例子中nu爲8，nr爲6，函數輸出爲，

sat	iu	ir
2	1	8
3	2	9
4	3	10
23	7	12

卡爾曼濾波的時間更新

udstate(rtk,obs,sat,iu,ir,ns,nav);

從時間更新函數udstate下邊的函數聲明可以看出，此函數只有一個輸出rtk，

時間更新中的函數都以ud開頭,表示update

static void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat,
                    const int *iu, const int *ir, int ns, const nav_t *nav)

這部分的算法部分在 rtklib相對定位算法中已有介紹，因此在top邏輯層中不再詳細介紹。
輸出爲rtk，x, P根據卡爾曼濾波的時間更新方程，做了一步預測。

計算移動站的無差觀測

if (!zdres(0,obs,nu,rs,dts,var,svh,nav,xp,opt,0,y,e,azel)) {

此函數與上邊計算基站的無差觀測爲同一個函數，只不過總體說來有兩點不同：

• 站點座標使用的是xp,這個是kalman濾波時間更新中得到的新座標，
• 所有的輸出，如y,e,azel，起始存儲位置爲0，而上邊的基準站無差觀測時爲nu*size

雙差觀測及觀測矩陣

if ((nv=ddres(rtk,nav,obs,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,H,R,vflg))<1)

上邊函數最重要的三個輸出v,H,R都是卡爾曼濾波量測更新所必須的。
這裏多說一句，rtklib相對定位算法中我們從算法上介紹過，ekf的量測向量是雙差載波和雙差僞距，這裏爲什麼觀測變成雙差殘差了呢？對ekf熟悉的可以略過下邊的解釋了，如果有疑惑，那麼請看

量測方程線性化
熟悉ekf的此節可以略過，不要浪費時間
例子，存在非線性量測方程，$y=h(x)=x^2$，爲了應用kalman濾波，我們使用泰勒展開將她線性化
$y=x_0^2+2x_0(x-x_0)$
即，$2x_0(x-x_0)=H(x-x0)=y-x_0^2$
就是這樣， $h$變成了$H$，$y$變成了$y-x_0^2$

v[nv]=(y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])-
                      (y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2]);

這個式子就是求雙差殘差，帶入前邊的無差觀測即可。
本函數詳細內容不在Top邏輯層介紹了，下邊專門介紹一下。

量測更新

到這裏，量測更新的觀測向量，雅可比矩陣，噪聲矩陣都有了，萬事俱備只欠東風，下邊這個函數完成的就是kalman濾波的量測更新。

if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) {

上述函數執行之後，
$xp=x+K*v\\ Pp=(I-K*H')*P$
其實到這裏，相對定位的浮點解已經出來了！！！

固定解

下邊纔是重頭戲，單差整週模糊度的固定，只有固定了模糊度，才能得到固定解，也就是真正意義上的高精度解。

/* resolve integer ambiguity by LAMBDA */
    else if (stat!=SOLQ_NONE&&resamb_LAMBDA(rtk,bias,xa)>1) {

rtklib中的整週固定解通過lambda算法求解，整週固定解的求解過程其數學本質是一個混合整數最小二乘問題，即待求解的變量中有一部分是浮點數，一部分是整數（單差模糊度）。如果不考慮計算效率，那麼我們通過估計誤差設置一個搜索區間，帶入所有模糊度的組合，取殘差最小的那一組就是我們要求解的整週模糊度。但是，這個計算量無疑是一個天文數字！想象一下，我們有14*3=42個單差模糊度要求解，每個浮點周圍我們取10個待算整數，那麼組合數量就是$10^{42}$，即便是後處理應用，這個量級也挺嚇人的。
所以，lambda算法本質上是解決計算速度的問題，relpos的核心算法或者之一，後邊單獨介紹一下。
這裏我們需要知道，通過這個函數之後固定解rtk->xa得到了。

總結

現在不結，感覺有點長了。如果對照代碼看會發現還有地方沒提到。是的，總體來說從top level來說 relpos這個函數已經講完了。但是有的細節確實是沒有提及，從後向前說：

• hold and fix: 在取得固定解之後再做一次kalman濾波的量測更新
• 固定解獲取後，計算雙差殘差，進行有效性驗證
• 浮點解解算完成後，計算雙差殘差，進行有效性驗證
• 對於每個函數的實現細節都沒有提及

這些將在後續章節陸續寫出來。