rtklib一之帶你一步一步讀懂rtklib 單點定位代碼及算法

1 調用關係圖

下圖是rtklib的單點定位的調用關係圖，入口函數爲pntpos（位於pntpos.c）。

2 單點定位程序流程

從上圖中可以看出，pntpos這個函數做了3件事，基本上分別對應了三個函數。

2.1 計算衛星位置和速度

函數estpos完成了衛星位置的計算，這一部分代碼基本是對着公式敲代碼。

2.2 位置估計

函數estpos用來完成位置估計，使用的算法是加權最小二乘法，位置估計如果失敗會調用raim算法重新進行位置估計，前提是收星數滿足至少六顆衛星，並且對應的解算參數要設置opt->posopt[4]=1;。
位置估計的本質是解算下邊的方程組：
$P_1=\sqrt{(x-x_1^s)^2+(y-y_1^s)^2+(z-z_1^s)^2}+cdt_u+I_1+T_1\\ P_2=\sqrt{(x-x_2^s)^2+(y-y_2^s)^2+(z-z_2^s)^2}+cdt_u+I_2+T_2\\ ...\\ P_m=\sqrt{(x-x_m^s)^2+(y-y_m^s)^2+(z-z_m^s)^2}+cdt_u+I_m+T_m$
其中，
$P_i$是$i$衛星對應的僞距觀測量，從觀測數據中獲得，爲已知量，
$(x_i^s,y_i^s,z_i^s)$爲$i$衛星對應的位置，在2.1中已經計算得到，也是已知量，
$c$爲光速，
$I_i$爲$i$衛星對應的電離層延遲，通過建模得到，下邊的第3章中介紹，這裏看做已知量，
$T_i$爲$i$衛星對應的對流層延遲，通過建模得到，下邊的第3章中介紹，這裏看做已知量，
那麼方程組中只剩下$(x,y,z,dt_u)$四個未知數了（分別是接收機位置和接收機鐘差），解算四個未知數需要至少四個方程，所以要完成單點定位解算至少需要四顆衛星。

這裏特別說明一下，以上的方程和結論是建立在單系統（如單GPS，或者單北斗）解算的基礎上的，如果是多系統混合解算那麼，就不應該只有一個接收機鐘差。實際上每增加一個解算系統，就會增加一個未知數，相應的觀測衛星的數據也至少加1才能解算。

下邊看看estpos中是怎麼解算這個方程組的

2.2.1 模型線性化和觀測值補償

函數rescode幾乎是位置估算中最重要的一個函數，完成了解算模型的線性化，以及僞距補償，加權參數估計等工作，是最小二乘計算的基礎。可以這麼說，最小二乘中用到的所有參數（除衛星位置外）都是在這個函數裏計算的。

2.2.1.1 僞距修正

這個函數用於修正僞距，如雙拼僞距融合等，具體方法見3

if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue;

2.2.1.2 電離層修正

此處完成電離層修正dion，當然還有電離層修正的方差vion，vion會在計算權重時用到。

/* ionospheric corrections */
if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2,
                iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) continue;
        
/* GPS-L1 -> L1/B1 */
if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) {
    dion*=SQR(lam_L1/lam_carr[0]);
}

2.2.1.3 對流層修正

此處完成電離層修正dtrop，當然還有對流層修正的方差vtrp，vtrp會在計算權重時用到。

if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
             iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {
    continue;
}

另外的部分是模型線性化和觀測值方差計算，這個可以在3中介紹。

2.2.2 加權

下邊的代碼用來給觀測值對應的$H$陣相應的行乘一個權值，可以看到用的是var[j]這個變量做的加權，這個參數的計算見3

for (j=0;j<nv;j++) {
    sig=sqrt(var[j]);
    v[j]/=sig;
    for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}

2.2.3 最小二乘求解

下邊的代碼用來計算最小二乘解，
$x=(A*A')^{-1}*A*y$

if ((info=lsq(H,v,NX,nv,dx,Q))) {
    sprintf(msg,"lsq error info=%d",info);
    break;
}

2.2.4 結果檢驗valsol

解算結果是否合格要看以下兩個檢驗是不是通過

2.2.4.1 殘差檢驗

殘差檢驗用的是卡方檢驗，其實這個檢驗對低精度板卡來說有點嚴格，如果解算的對象是低精度板卡這個檢測應該禁掉，否則很多解算結果是通不過的。

if (nv>nx&&vv>chisqr[nv-nx-1]) {
    trace(3,"chi-square error nv=%d vv=%.1f cs=%.1f\n",nv,vv,chisqr[nv-nx-1;
    return 0;
}

2.2.4.2 dop值檢驗

下邊第一行代碼是dop值的計算，從這個函數的參數也可以看出，dop僅與衛星的幾何分佈有關係，azels爲衛星們的方位角仰角。

dops(ns,azels,opt->elmin,dop)
if (dop[0]<=0.0||dop[0]>opt->maxgdop) {
    sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]);
    return 0;
}

2.3 速度估計

函數estvel用來完成速度估計，前提是數據中要有多普勒頻移觀測值（即結構體obsd_t中的D），如果沒有則解算失敗，如果這種情況下仍然必須要獲取速度，只能通過位置進行估計了。

typedef struct {        /* observation data record */
...
    float  D[NFREQ+NEXOBS]; /* observation data doppler frequency (Hz) */
} obsd_t;

速度估計的本質是解算下邊的方程組：
$d_1=\vec{e_1}\cdot(\vec{v_{s1}}-\vec{v})+c\dot{b}\\ d_2=\vec{e_2}\cdot(\vec{v_{s2}}-\vec{v})+c\dot{b}\\ ...\\ d_m=\vec{e_m}\cdot(\vec{v_{sm}}-\vec{v})+c\dot{b}\\$
其中，
$d_i$是$i$衛星的多普勒觀測量，即obs_t中的D，
$\vec{v_{si}}$是衛星速度向量，
$\vec{v}$是接收機速度，爲未知量
$\dot{b}$爲接收機鍾漂，爲未知量
$\vec{e_i}$爲衛星$i$的方向餘弦矢量，由於衛星的位置和接收機位置已經在前邊解算出來了，那麼這個方向餘弦也是已知的
可以看出速度的求解是求解一組四元一次線性方程，這點與位置解算不同，無需迭代。

3 單點定位中的關鍵函數對應算法

3.1 求解非線性方程組

衛星位置解算的本質是求解一組非線性方程，沒有學過數值計算的人可能理解起來稍微有一點點費解，這裏從一個一維的例子擴展到高維。學過數值計算的可以跳過這一節了。
首先看下邊這麼一個非線性方程：
$x^2=4$
下邊我們通過線性化和迭代的方法解算一下這個方程，首先通過泰勒級數線性化，泰勒展開需要在某點展開，首先令$x_0=1$。
$x^2=x_0^2+2x_0(x-x_0)=1+2(x-1)=4$
方程變成了一個線性方程，解算得$x=2.5$
然後我們令$x=2.5$,
$2.5^2+2*2.5(x-2.5)=4$
解算得到，$x=2.05$,就這麼一直迭代解算下去，我們可以看到迭代次數和解算結果的關係，可以看到只需要經過四次迭代，幾乎得到正確結果了。

迭代次數	解算結果	殘差
1	2.5	-2.25
2	2.05	-0.2025
3	2.000609756	-0.00244
4	2.000000093	-3.7e-7

現在可以擴展到高維了，四個未知數的情形和上邊幾乎是一模一樣的，唯一不同的是線性化時，使用的是多元函數的泰勒展開而已,迭代多少次合適呢？是不是迭代次數越多越好呢？rtklib中使用以下方法：

for (i=0;i<MAXITR;i++) {
    ...
    if (norm(dx,NX)<1E-4) {
        ...

從上邊的代碼可以看出，rtklib通過限制迭代次數（最大十次）和殘差範數來結束迭代，當殘差範數小於1e-4時，結束迭代，一般6次左右的迭代就能結束迭代了。

3.2 RAIM算法

RAIM即Receiver Autonomous Integrity Monitoring，FDE即Fault Detection Exclusion,是接收機自主完好性監測和故障檢測與排除。rtklib中通過每次排除一顆衛星進行解算，然後選取殘差最小的一次解算結果作爲最終的解算結果，此次對應的衛星就是故障衛星，這個方法不適合有超過一個故障衛星的場合。
以下是RAIM算法的一個直觀解釋，
老師讓五個同學測量黑板的長度，下邊是測量結果：

姓名	小明	jack	李雷	韓梅梅	賈寶玉
測量值	2.01	1.98	2.00	2.12	2.4

老師一平均2.102米，這就是最小二乘法，老師覺得李雷學習成績好，做事認真應該更相信李雷，賈寶玉整天無所事事，不靠譜，不能信他，於是給他們加了信任度。

姓名	小明	jack	李雷	韓梅梅	賈寶玉
測量值	2.01	1.98	2.00	2.12	2.4
權	2	2	3	2	1

老師這次一平均，哦，2.062似乎結果更準了，這就叫加權最小二乘。
什麼是RAIM呢？我們每次剔除一個學生的結果，算一下加權平均。

姓名	小明	jack	李雷	韓梅梅	賈寶玉
測量值	2.01	1.98	2.00	2.12	2.4
權	2	2	3	2	1
剔除後估計值	2.075	2.0825	2.0886	2.0475	2.0244

表中可以看出raim的結果，賈寶玉對應的測量值2.4被剔除，估計值爲2.0244，
殘差的計算這裏省略。

3.3 僞距修正

以下是消電離層組合，

/* iono-free combination */
PC=(gamma*P1-P2)/(gamma-1.0);

$PC=\frac{\gamma P1-P2}{\gamma-1.0} \\ =\frac{f_1^2P1-f_2^2P2}{f_1^2-f_2^2}\\ =\frac{f_1^2 \rho-f_2^2\rho+f_1^2 ion_1-f_2^2ion_2+f_1^2 trop-f_2^2trop}{f_1^2-f_2^2}$
因爲gps的電離層延遲與頻率相關，其一階近似項爲，
$ion_i=\frac{A}{f_i^2}$
其中$A$爲常數，因此$f_1^2 ion_1-f_2^2ion_2=0$.
所以，
$PC =\frac{f_1^2 \rho-f_2^2\rho+f_1^2 ion_1-f_2^2ion_2+f_1^2 trop-f_2^2trop}{f_1^2-f_2^2}\\ =\frac{f_1^2 \rho-f_2^2\rho+f_1^2 trop-f_2^2trop}{f_1^2-f_2^2}\\ =\rho+trop$
從上邊的公式可以看出，電離層誤差被消除了，所以這個組合也叫消電離層組合。