RTKPPP定位算法流程

1、1 基礎(chǔ)知識(shí)1.1 GPS精密單點(diǎn)定位的基本原理GPS精密單點(diǎn)定位一般采用單臺(tái)雙頻GPS接收機(jī)，利用IGS提供的精密星歷和衛(wèi)星鐘差，基于載波相位觀測(cè)值進(jìn)行的高精度定位。觀測(cè)值中的電離層延遲誤差通過雙頻信號(hào)組合消除，對(duì)流層延遲誤差通過引入未知參數(shù)進(jìn)行估計(jì)。1.2時(shí)間系統(tǒng)RTKLIB內(nèi)部使用GPST（GPST時(shí)間）用于GNSS的數(shù)據(jù)處理和定位算法。數(shù)據(jù)在 RTKLIB內(nèi)部處 理之前，需要轉(zhuǎn)換成 GPST時(shí)間。使用GPST的原因是避免處理潤(rùn)秒。RTKLIB使用以下結(jié)構(gòu)體表示時(shí)間：typedef structtime_t time; /* time（s） expressed by standard t

2、ime_t */double sec; /* fraction of second under 1 s */ gtime_t;1.2.1 GPST 和 UTC（Universal Time Coordinated）關(guān)系參考【圖1】，參考【圖2】：圖1轉(zhuǎn)換關(guān)系公式圖2通過使用GPS導(dǎo)航信息中的UTC參數(shù)，GPST到 UTC或者UTC到GPST之前的轉(zhuǎn)換可以用更準(zhǔn)確 的表達(dá)方式，如【圖3】。圖3心 也£怙皿are衍耳這些參數(shù)是由GPS導(dǎo)航消息提供的。1.2.2 BDT （北斗導(dǎo)航衛(wèi)星系統(tǒng)時(shí)間）BDT （北斗導(dǎo)航衛(wèi)星系統(tǒng)時(shí)間）是一個(gè)連續(xù)的時(shí)間系統(tǒng)，沒有潤(rùn)秒。開始?xì)v元的時(shí)間是【UTC2006

3、年 1 月 1 號(hào) 00:00:00 】。北斗時(shí)間計(jì)算公式【圖4】：UTC和GPST時(shí)間轉(zhuǎn)換同上面的GPS樣，只不過UTC參數(shù)來自與北斗導(dǎo)航信息中。1.3坐標(biāo)系統(tǒng)接收機(jī)和衛(wèi)星的位置在 RTKLIB中表示為在ECEF(地心地固坐標(biāo)系)坐標(biāo)系統(tǒng)中的X, Y, Z組件。1.3.1大地坐標(biāo)到ECEF坐標(biāo)的轉(zhuǎn)換轉(zhuǎn)換公式如【圖5】。第三個(gè)公式最后一行有錯(cuò)，應(yīng)該為：(v(1 -e2)+h)si n圖5參數(shù)說明：a :地球參考橢球的長(zhǎng)半徑f:地球參考橢球的扁平率h:橢球高度':緯度:經(jīng)度當(dāng)前版本的RTKLIB使用的值為【圖6】：圖6圖7參考橢球體1.3.2 ECEF坐系到大地坐標(biāo)的轉(zhuǎn)換轉(zhuǎn)換公式如【圖8

4、】圖81.3.3本地坐標(biāo)到ECEF坐標(biāo)的轉(zhuǎn)換在接收機(jī)位置的本地坐標(biāo)， 也被稱為ENU坐標(biāo)，通常使用在GNSS導(dǎo)航處理。ECEF坐標(biāo)到本地 坐標(biāo)轉(zhuǎn)換的旋轉(zhuǎn)矩陣表示為【圖 9】。圖9 Er旋轉(zhuǎn)矩陣參數(shù)說明：.：接收機(jī)位置的緯度:接收機(jī)位置的經(jīng)度通過使用Er和接收機(jī)的坐標(biāo)r【ECEF】,坐標(biāo)ecef【ECEF】可以被轉(zhuǎn)換到本地坐標(biāo)的坐標(biāo)rgai，公式如【圖10】。圖102 RTKPPP定位算法2.1 單點(diǎn)定位（pntpos ）1:satposs 2: estpos 3: estvel1. 計(jì)算計(jì)算衛(wèi)星位置、速度和時(shí)鐘（satposs）a）通過廣播星歷計(jì)算衛(wèi)星鐘差（ephclk衛(wèi)星編號(hào)到衛(wèi)星系統(tǒng)的轉(zhuǎn)換

5、（satsys）根據(jù)衛(wèi)星的編號(hào)，獲取到對(duì)應(yīng)的衛(wèi)星導(dǎo)航系統(tǒng)。選擇星歷（seleph）1、傳入信號(hào)傳輸時(shí)間，衛(wèi)星編號(hào)，導(dǎo)航數(shù)據(jù)等參數(shù)。2、遍歷導(dǎo)航數(shù)據(jù)，遍歷導(dǎo)航數(shù)據(jù)里面的星歷數(shù)據(jù)，判斷星歷數(shù)據(jù)的衛(wèi)星編號(hào)是否和傳入的衛(wèi)星編號(hào)相等。3、 如果星歷數(shù)據(jù)的衛(wèi)星編號(hào)和傳入的衛(wèi)星編號(hào)相等，就計(jì)算星歷參考時(shí)間（toe） 和信號(hào)傳輸時(shí)間的時(shí)間差。如果不相等，繼續(xù)處理下一條星歷數(shù)據(jù)。4、判斷計(jì)算出來的時(shí)間差，如果時(shí)間差大于了允許的最大時(shí)間差，繼續(xù)查找下一個(gè)星歷數(shù)據(jù)。否則，判斷時(shí)間差最小的星歷數(shù)據(jù)，記錄星歷數(shù)據(jù)的位置。5、返回之前記錄出來的星歷數(shù)據(jù)。使用廣播星歷計(jì)算衛(wèi)星時(shí)鐘偏差（eph2clk）1、傳入信號(hào)發(fā)射時(shí)刻的

6、時(shí)間和星歷數(shù)據(jù)。2、 計(jì)算信號(hào)發(fā)射時(shí)刻的時(shí)間和本時(shí)段鐘差參數(shù)參考時(shí)間（星歷參數(shù)toc）的時(shí)間 差。3、 通過下式計(jì)算鐘差，這里還沒有處理相對(duì)論校正項(xiàng)和tgd ：（代碼中有個(gè)迭 代過程，資料上沒看到寫 ）衛(wèi)星鐘差計(jì)算出來之后，信號(hào)發(fā)射時(shí)刻的時(shí)間還要加上這個(gè)鐘差。b）計(jì)算衛(wèi)星在信號(hào)發(fā)射時(shí)刻的位置、速度和時(shí)鐘（satpos）根據(jù)星歷表選項(xiàng)來選擇不同的處理，如下：廣播星歷（EPHOPT_BRDC : ephpos （廣播星歷到衛(wèi)星位置和鐘差）1、 根據(jù)公式計(jì)算出tk ；2、 根據(jù)使用的衛(wèi)星系統(tǒng)，選擇使用的地球引力常數(shù)（mu）和地球的角速度（omge）3、根據(jù)公式J?計(jì)算出平近點(diǎn)角M。Ej -esinE

7、j - A/I -£?co$£4、求解開普勒方程止丄皿山，按照以下公式迭代求解。E = liin EjF5、根據(jù)以下公式計(jì)算出u（改正后的緯度幅角）,r（改正后的徑向）,i（改正后的軌道傾角）的值。6、根據(jù)以下公式計(jì)算衛(wèi)星在軌道平面內(nèi)的坐標(biāo)。7、根據(jù)不同的衛(wèi)星系統(tǒng)，做不同的計(jì)算。GPS計(jì)算方式:北斗計(jì)算方式:00叫sin0"Rx （應(yīng)）=0cos Bcos 30其中一晶&cm/< o018、按照公式計(jì)算出時(shí)間tc。9、按照以下公式計(jì)算出鐘差和鐘漂。精密星歷（EPHOPT_PRE）: peph2pos廣播 +SBAS （ EPHOPT_SBA）： sa

8、tpos_sbas廣播 +SSR_AP（ EPHOPT_SSRAPC: satpos_ssr廣播 +SSR_COM（ EPHOPT_SSRCO）satpos_ssrQZSS LE星歷（EPHOPT_LEX： lexeph2pos2. 使用偽距估算接收機(jī)的位置，返回估算狀態(tài)結(jié)果（estpos）a）偽距殘差（rescode）把ecef坐標(biāo)系轉(zhuǎn)換成大地坐標(biāo)系（ecef2pos）1、按照以下公式做轉(zhuǎn)換，暫時(shí)還沒看懂。計(jì)算幾何距離和接收機(jī)到衛(wèi)星的單位矢量（geodist）1、 用衛(wèi)星的坐標(biāo)向量做歐幾里德范數(shù)，返回值和地球長(zhǎng)半軸（WGS84）比較。小于地球長(zhǎng)半軸（WGS84），返回-1 ；2、計(jì)算衛(wèi)星坐標(biāo)

9、和接收機(jī)坐標(biāo)的差值向量。3、用差值向量做歐幾里德范數(shù)，再用上一步計(jì)算出來的差值向量和計(jì)算結(jié)果做除法，得到視線向量。按照如下公式:4、使用以下公式計(jì)算幾何距離。計(jì)算衛(wèi)星方位角/仰角（satazel）1、把接收機(jī)ecef坐標(biāo)轉(zhuǎn)換到大地坐標(biāo)；2、 判斷高度是否大于地球半長(zhǎng)軸（WGS84）的負(fù)數(shù)值；3、 如果高度小于等于地球半長(zhǎng)軸（WGS84）的負(fù)數(shù)值，方位角為0,仰角為PI/2;4、如果高度大于地球半長(zhǎng)軸（ WGS84 ）的負(fù)數(shù)值，把ECEF向量轉(zhuǎn)換到局部坐 標(biāo)。然后對(duì)轉(zhuǎn)換出來的坐標(biāo)做內(nèi)積。5、使用如下公式計(jì)算衛(wèi)星方位角和仰角。偽距使用編碼殘差改正（prange）暫時(shí)沒找到對(duì)應(yīng)的文檔對(duì)應(yīng)。電離層改正

10、（ionocorr）通過廣播電離層模型（klobuchar模型）計(jì)算出電離層延遲（ion model）1、校驗(yàn)傳入的電離層模型參數(shù)，校驗(yàn)失敗，使用默認(rèn)的電離層模型參數(shù)；2、 使用以下公式計(jì)算出地球?yàn)橹行牡慕嵌龋ò雸A）；3、 使用以下公式計(jì)算子的電離層的緯度/經(jīng)度（半圓）；4、使用以下公式計(jì)算地磁緯度。5、計(jì)算本地時(shí)間，返回值按這個(gè)公式【tt-=floor（tt/86400.0）*86400.0;】處理，保證tt的范圍（0<=tt<86400 ）。6、計(jì)算傾斜因子。7、電離層延遲計(jì)算，公式如下。對(duì)流層改正（tropcorr）通過標(biāo)準(zhǔn)大氣壓和 saastamoinen模型計(jì)算對(duì)流層延遲

11、（tropmodel ）1、使用以下公式計(jì)算總氣壓。2、使用以下公式計(jì)算絕對(duì)溫度。3、 使用以下公式計(jì)算水蒸汽的分壓。代碼中沒有使用。4、使用以下公式計(jì)算【Saastamoi ner模型】。偽距殘差殘差值=偽距-（物理距離+dtr-光速*時(shí)鐘偏差+電離層誤差+對(duì)流層誤差）時(shí)鐘系統(tǒng)和接收器的偏置補(bǔ)償偽距測(cè)量誤差方差（varerr）b) 方差權(quán)重值 (weight by varian ce)c) 最小二乘估計(jì)(Isq)最小二乘估計(jì)通過求解正規(guī)方程(X = (A * A ) A - 1* A * Y )1、 計(jì)算矩陣A*Y的結(jié)果，保存到矩陣Ay ;2、 計(jì)算矩陣A*A '勺結(jié)果，保存到矩陣Q

12、 ；3、求的矩陣Q的逆矩陣,結(jié)果保存到矩陣Q;4、最后 x=Q*Ayd) 歐幾里德范數(shù)(norm )1、 通過最小二乘法估算出來的參數(shù)dx【長(zhǎng)度為4】，分別加到位置向量上 x【長(zhǎng)度為 4】。2、 對(duì)這個(gè)速度向量做歐幾里德范數(shù)，返回值同1E-4比較，小于這個(gè)值時(shí)，就得到估 算出的接收機(jī)的位置為向量 x的值。e) 驗(yàn)證求解(valsol)1、 對(duì)偽距殘差值做內(nèi)積，然后同卡方分布的自由度分布值(alpha=O.OO1)做比較，大 于卡方分布的自由度分布值的數(shù)據(jù)，無效。2、 計(jì)算dops;暫時(shí)還沒沒找到資料。3、 用計(jì)算出來的dops和配置的最大dops閾值比較，大于配置的最大dops閾值的數(shù)據(jù) 無效

13、。3. 使用多普勒估算接收機(jī)速度(estvel)a) 多普勒殘差(resdop)把ecef坐標(biāo)系轉(zhuǎn)換成大地坐標(biāo)系(ecef2pos)把xyz坐標(biāo)轉(zhuǎn)換成 enu坐標(biāo)(xyz2enu)計(jì)算ecef中的瞄準(zhǔn)線向量計(jì)算相對(duì)于接收機(jī)在 ECEF中的衛(wèi)星速度多普勒殘差計(jì)算1、速率公式：b) 最小二乘估計(jì) (lsq)最小二乘估計(jì)通過求解正規(guī)方程( X = (A * A') a - 1* A * Y )1、計(jì)算矩陣A*Y的結(jié)果，保存到矩陣Ay;2、計(jì)算矩陣A*A'的結(jié)果，保存到矩陣 Q;3、求的矩陣Q的逆矩陣，結(jié)果保存到矩陣 Q;4、最后 x=Q*Ayc）歐幾里德范數(shù)（norm ）公式：倒：=

14、荷+吒3、 通過最小二乘法估算出來的參數(shù)dx【長(zhǎng)度為4】，分別加到速度向量上 x【長(zhǎng)度為 4】。4、 對(duì)這個(gè)速度向量做歐幾里德范數(shù)，返回值同1E-6比較，小于這個(gè)值時(shí)，就得到估 算出的接收機(jī)速度為向量 x的值。2.2 精確定位 （pppos）1:udstate_ppp 2: satposs 3: testeclipse 4: res_ppp 5: res_ppp 6: filter 7: res_ppp1. 暫時(shí)更新狀態(tài)（udstate_ppp）a）位置更新b）時(shí)鐘更新c）對(duì)流層參數(shù)更新d）相位偏差更新1、通過LLI檢測(cè)周跳；2、通過Geometry-Free相位跳變檢測(cè)周跳（如果雙頻測(cè)量值可

15、用）；2. 計(jì)算衛(wèi)星的位置和速度和時(shí)鐘（satposs）c）通過廣播星歷計(jì)算衛(wèi)星鐘差（ephclk）衛(wèi)星編號(hào)到衛(wèi)星系統(tǒng)的轉(zhuǎn)換（satsys）根據(jù)衛(wèi)星的編號(hào)，獲取到對(duì)應(yīng)的衛(wèi)星導(dǎo)航系統(tǒng)。選擇星歷（seleph）1、傳入信號(hào)傳輸時(shí)間，衛(wèi)星編號(hào)，導(dǎo)航數(shù)據(jù)等參數(shù)。2、遍歷導(dǎo)航數(shù)據(jù)，遍歷導(dǎo)航數(shù)據(jù)里面的星歷數(shù)據(jù)，判斷星歷數(shù)據(jù)的衛(wèi)星編號(hào)是 否和傳入的衛(wèi)星編號(hào)相等。3、 如果星歷數(shù)據(jù)的衛(wèi)星編號(hào)和傳入的衛(wèi)星編號(hào)相等，就計(jì)算星歷參考時(shí)間（toe） 和信號(hào)傳輸時(shí)間的時(shí)間差。如果不相等，繼續(xù)處理下一條星歷數(shù)據(jù)。4、判斷計(jì)算出來的時(shí)間差，如果時(shí)間差大于了允許的最大時(shí)間差，繼續(xù)查找下 一個(gè)星歷數(shù)據(jù)。否則，判斷時(shí)間差最小的星

16、歷數(shù)據(jù)，記錄星歷數(shù)據(jù)的位置。5、返回之前記錄出來的星歷數(shù)據(jù)。使用廣播星歷計(jì)算衛(wèi)星時(shí)鐘偏差（eph2clk）1、傳入信號(hào)傳輸時(shí)間和星歷數(shù)據(jù)。2、傳入信號(hào)發(fā)射時(shí)刻的時(shí)間和星歷數(shù)據(jù)。3、 計(jì)算信號(hào)發(fā)射時(shí)刻的時(shí)間和本時(shí)段鐘差參數(shù)參考時(shí)間（星歷參數(shù)toe）的時(shí)間 差。4、 通過下式計(jì)算鐘差，這里還沒有處理相對(duì)論校正項(xiàng)和tgd :（代碼中有個(gè)迭 代過程，資料上沒看到寫 ）d）計(jì)算衛(wèi)星位置、速度和時(shí)鐘（satpos）根據(jù)星歷表選項(xiàng)來選擇不同的處理，如下：廣播星歷（EPHOPT_BRDC : ephpos （廣播星歷到衛(wèi)星位置和鐘差）1、根據(jù)公式.計(jì)算出tk ；2、根據(jù)使用的衛(wèi)星系統(tǒng)，選擇使用的地球引力常數(shù)（

17、mu）和地球的角速度（omge）計(jì)算出平近點(diǎn)角M。4、求解開普勒方程 蟲二 八"，按照以下公式迭代求解。Ej-ME = liin Ej5、根據(jù)以下公式計(jì)算出u（改正后的緯度幅角）,r（改正后的徑向）,i（改正后的軌道傾角）的值。6、根據(jù)以下公式計(jì)算衛(wèi)星在軌道平面內(nèi)的坐標(biāo)。7、根據(jù)不同的衛(wèi)星系統(tǒng)，做不同的計(jì)算。GPS計(jì)算方式：100 "cos比但）=0-siiii?emF 00singcm日< o0 1北斗計(jì)算方式：其中f = T 一 t8、 按照公式計(jì)算出時(shí)間tc。9、按照以下公式計(jì)算出鐘差和鐘漂。精密星歷（EPHOPT_PRE）: peph2pos 廣播 +SBAS

18、 （ EPHOPT_SBAE satpos_sbas廣播 +SSR_AP（ EPHOPT_SSRAPC: satpos_ssr廣播 +SSR_COM （EPHOPT_SSRCO） satpos_ssrQZSS LE星歷（EPHOPT_LEX： Iexeph2pos3. 排除對(duì)被遮蔽衛(wèi)星的測(cè)量（testeclipse）a）獲得太陽(yáng)和月亮在 ecef中的位置（sunmoonpos）b）歸一化三維矢量（normv3）c）歐幾里德范數(shù)的向量（norm）d）計(jì)算太陽(yáng)-地球-衛(wèi)星角（dot）4. 相位和編碼殘差（res_ppp）a）地球潮汐改正（tidedisp）b）計(jì)算幾何距離（geodist）1、 用

19、衛(wèi)星的坐標(biāo)向量做歐幾里德范數(shù)，返回值和地球長(zhǎng)半軸（WGS84）比較。小于地球長(zhǎng)半軸（WGS84），返回-1 ；2、計(jì)算衛(wèi)星坐標(biāo)和接收機(jī)坐標(biāo)的差值向量。3、用差值向量做歐幾里德范數(shù)，再用上一步計(jì)算出來的差值向量和計(jì)算結(jié)果做除法，得到視線向量。4、使用以下公式計(jì)算幾何距離。備注：這個(gè)公式 是地球自轉(zhuǎn)改正公式。地球自轉(zhuǎn)改正產(chǎn)生地球自轉(zhuǎn)改正的機(jī)制在于地固系隨地球的自轉(zhuǎn)而旋轉(zhuǎn)，地固系是非慣性系統(tǒng)，用地固系中的坐標(biāo)計(jì)算衛(wèi)星到接收機(jī)幾何距離時(shí)，由于衛(wèi)星位置和接收機(jī)位置是兩個(gè)不同時(shí)刻的位置矢量，而且這兩個(gè)時(shí)刻的地固系相對(duì)于慣性系是變化的，因此要考慮地球自轉(zhuǎn)引起的距離變化。其改正公式為：其中,w為地球自轉(zhuǎn)角速度

20、；C為光速；X si和X ri分別代表衛(wèi)星位置矢量和測(cè)站位置矢量的分量，i= 1,2對(duì)應(yīng)x , y分量。地球自轉(zhuǎn)對(duì)緯度影響甚小 ，對(duì)經(jīng)度影響最大，其次是 高度。同時(shí)地球的自轉(zhuǎn)影響還跟測(cè)站的經(jīng)緯度以及測(cè)站與衛(wèi)星的幾何關(guān)系有關(guān)，對(duì)兩極的測(cè)站，影響為零；對(duì)赤道上的測(cè)站影響最大；當(dāng)衛(wèi)星在測(cè)站子午面內(nèi)影響為零 ；衛(wèi)星在測(cè)站 東方時(shí)，影響為負(fù)，衛(wèi)星在測(cè)站西方時(shí)，影響為正。c）衛(wèi)星方位角/仰角計(jì)算（satazel）1、把接收機(jī)ecef坐標(biāo)轉(zhuǎn)換到大地坐標(biāo)；2、 判斷高度是否大于地球半長(zhǎng)軸（WGS84）的負(fù)數(shù)值；3、 如果高度小于等于地球半長(zhǎng)軸（WGS84）的負(fù)數(shù)值，方位角為0,仰角為PI/2;4、如果高度大于

21、地球半長(zhǎng)軸（ WGS84）的負(fù)數(shù)值，把ECEF向量轉(zhuǎn)換到局部坐標(biāo)。然后對(duì)轉(zhuǎn)換出來的坐標(biāo)做內(nèi)積。5、使用如下公式計(jì)算衛(wèi)星方位角和仰角。d) 對(duì)流層延遲修正：通過標(biāo)準(zhǔn)大氣壓和 saastamoinen模型計(jì)算對(duì)流層延遲(tropmodel)1、使用以下公式計(jì)算總氣壓。2、使用以下公式計(jì)算絕對(duì)溫度。3、 使用以下公式計(jì)算水蒸汽的分壓。代碼中沒有使用。4、使用以下公式計(jì)算【Saastamoi ner模型】。e) 衛(wèi)星天線模型(satantpcv)f) 接收機(jī)天線模型，通過天線相位中心參數(shù)計(jì)算天線偏移(antmodel)g) 相位纏繞校正(windupcorr)h) 電離層和天線相位校正測(cè)量(corrmeas)i) 計(jì)算衛(wèi)星時(shí)鐘和對(duì)流層延遲5. 卡爾曼濾波(filter)卡爾曼濾波按照以下的公式更新狀態(tài)：K = P * H * (H' * P * H + R)A-1;xp = x + K * v ;Pp = (I -K