rtklib源碼 rtk差分解算，rtkpos和replos函數流程梳理
- rtkpos函數梳理
- 總體流程
- replos函數梳理
- replos總體流程
- 1. 通過satposs函數計算衛星的位置、速度等參數
- 2. 通過zdres函數計算基站僞距和載波相位的非差分殘差
- 2.1 初始化殘差數組 y
- 2.2 地球潮汐校正
- 2.3 基站接收機的星地距離修正（每顆衛星都修正）
- 2.3.1 計算接收機到衛星的幾何距離 geodist
- 2.3.2 計算每顆衛星的方位角和仰角 satazel
- 2.3.3 剔除衛星 satexclude
- 2.3.4 使用鐘差補償到星地幾何距離
- 2.3.5 估算對流層延遲并補償到星地距離 tropmodel、tropmapf
- 2.3.6 接收機天線相位中心校正
- 2.4 計算僞距和載波相位殘差 zdres_sat
- 3. 選擇移動站和基站之間所有的公共衛星 selsat
- 4. 目前狀态更新 udstate
- 4.1 得到相鄰曆元時間差
- 4.2 rover位置、速度、加速度及其方差更新 udpos
rtkpos函數梳理
總體流程
- 分别計算移動站和基站的觀測值數量 nu和nr
- 通過單點定位函數得到移動站的大緻位置和速度
- 判斷定位模式，根據不同的定位模式進行解算結果輸出，如果是rtk則執行replos函數進行差分解算
replos函數梳理
replos總體流程
- 計算衛星的位置、速度、鐘差、鐘漂等參數
1. 通過satposs函數計算衛星的位置、速度等參數
在函數的最開始可以看到，這裡計算的衛星資料是所有移動站和基準站的觀測到的衛星星曆得到的。（此處的n是n=nu+nr，在replos函數頭部處定義時已經相加了 ）
for (i=0;i<n&&i<2*MAXOBS;i++) { 2. 通過zdres函數計算基站僞距和載波相位的非差分殘差
if (!zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,svh+nu,nav,rtk->rb,opt,1,
y+nu*nf*2,e+nu*3,azel+nu*2)) {
errmsg(rtk,"initial base station position error\n");
free(rs); free(dts); free(var); free(y); free(e); free(azel);
return 0;
} 傳入的obs是obs+nu而不是obs是因為，obs是由移動站和基站的觀測值共同組成，基站部分的在後面，是以加一個nu作為偏移，nu：移動站觀測的衛星數 nr：參考站觀測的衛星數
輸入的rtk->rb是基站的位置資訊，這個在前面已經進行了定位了，或者配置中已經給出了基站位置
2.1 初始化殘差數組 y
for (i=0;i<n*nf*2;i++) y[i]=0.0; //nf=2，載波數量 2.2 地球潮汐校正
if (opt->tidecorr) {
tidedisp(gpst2utc(obs[0].time),rr_,opt->tidecorr,&nav->erp,
opt->odisp[base],disp);
for (i=0;i<3;i++) rr_[i]+=disp[i];
} （在配置中沒有用到這個改正）
2.3 基站接收機的星地距離修正（每顆衛星都修正）
//huhaoming：将 ecef 轉換為大地坐标 pos是轉化後的坐标
ecef2pos(rr_,pos);
for (i=0;i<n;i++) {
/* compute geometric-range and azimuth/elevation angle 計算幾何範圍和方位角/仰角 */
if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue;
if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue;
/* excluded satellite? 剔除衛星*/
if (satexclude(obs[i].sat,svh[i],opt)) continue;
/* satellite clock-bias 使用鐘差補償到星地幾何距離 */
r+=-CLIGHT*dts[i*2];
/* troposphere delay model (hydrostatic)*/
zhd=tropmodel(obs[0].time,pos,zazel,0.0);
r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd;
/* receiver antenna phase center correction 接收機天線相位中心校正*/
antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1], dant);
/* undifferenced phase/code residual for satellite */
zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2);
} 2.3.1 計算接收機到衛星的幾何距離 geodist
//huhaoming：求接收機到衛星的幾何距離，存在變量r中
if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue;```
```c
/* geometric distance ----------------------------------------------------------
* compute geometric distance and receiver-to-satellite unit vector
* args : double *rs I satellilte position (ecef at transmission) (m)
* double *rr I receiver position (ecef at reception) (m)
* double *e O line-of-sight vector (ecef)
* return : geometric distance (m) (0>:error/no satellite position)
* notes : distance includes sagnac effect correction
*-----------------------------------------------------------------------------*/
extern double geodist(const double *rs, const double *rr, double *e)
{
double r;
int i;
if (norm(rs,3)<RE_WGS84) return -1.0;
for (i=0;i<3;i++) e[i]=rs[i]-rr[i];
r=norm(e,3);
for (i=0;i<3;i++) e[i]/=r;
//huhaoming：OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT：地球自轉改正
return r+OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;
} - rs：衛星位置
- rr：接收機位置
- e：接收機到衛星的機關向量，方向是由接收機指向衛星
計算的時候通過兩個三維向量相減，最後再求範數即可得到星地距離
最後傳回值是地球自轉改正後的星地距離
2.3.2 計算每顆衛星的方位角和仰角 satazel
//如果大于最小值，則在後面用衛星鐘差和對流層延時來修正星地距離
if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue;``` /* satellite azimuth/elevation angle -------------------------------------------
* compute satellite azimuth/elevation angle
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
* double *e I receiver-to-satellilte unit vevtor (ecef)
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
* (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)
* return : elevation angle (rad)
*-----------------------------------------------------------------------------*/
extern double satazel(const double *pos, const double *e, double *azel)
{
double az=0.0,el=PI/2.0,enu[3];
if (pos[2]>-RE_WGS84) {
ecef2enu(pos,e,enu);
az=dot(enu,enu,2)<1E-12?0.0:atan2(enu[0],enu[1]);
if (az<0.0) az+=2*PI;
el=asin(enu[2]);
}
if (azel) {azel[0]=az; azel[1]=el;}
return el;
} 先判斷基站位置在經緯高坐标系中的高度是否大于-RE_WGS84（基準橢球面的長半徑），隻有滿足才可以進行方位角和高度角的計算。
傳入的位置資訊 pos 是大地坐标系下的坐标，需要将基站位置坐标由經緯高轉換為東北天坐标系中。
ecef2enu(pos,e,enu); 然後在enu坐标系下用課本的公式來計算即可
=dot(enu,enu,2)<1E-12?0.0:atan2(enu[0],enu[1]);
if (az<0.0) az+=2*PI;
el=asin(enu[2]); 2.3.3 剔除衛星 satexclude
排除衛星的主要是根據prcopt_t結構體中的exsats進行判斷是否有要排除或者保留的衛星資料
2.3.4 使用鐘差補償到星地幾何距離
r+=-CLIGHT*dts[i*2]; 2.3.5 估算對流層延遲并補償到星地距離 tropmodel、tropmapf
//huhaoming：用saastamoinen經驗模型，通過測站緯度、高程、氣溫、氣壓和水汽壓等資訊計算對流層延遲
zhd=tropmodel(obs[0].time,pos,zazel,0.0);
/*huhaoming：計算對流層延遲投影函數（即天頂方向到接收機相對衛星觀測方向上的對流層延遲投影系數）
* 這個函數中有兩種投影函數的計算方法，分别是GMF和NMF，對應的兩篇參考論文為“Global mapping functions for the atmosphere delay at radio wavelengths”和“Global Mapping Function(GMF): A new empirical mapping function base on numerical weather model data”
*預設使用的是NMF方法，也可以通過定義IERS_MODEL宏來使用GMF方法。
*這個函數調用完成後，會将傳回的幹投影函數和tropmodel中計算得到的天頂方向對流層幹延遲相乘，進而得到接收機相對衛星觀測方向上的對流層延遲。
*幹投影函數是通過傳回值獲得的，而濕投影是通過輸入/輸出參數mapfw獲得的。在zdres函數中，本函數輸入的mapfw=NULL，即不輸出濕投影。
*濕分量會在之後的ddres（計算雙差殘差的函數）函數中進行扣除。
*/
r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd; 2.3.6 接收機天線相位中心校正
antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1], dant); 生成一組接收機相對于衛星觀測方向的機關矢量e，e[0]，e[1]，e[2]分别為東、北、天三個方向上的分量；
頻段不同，天線的相位中心偏移(PCO)不同。先計算出每個頻段天線在東、北、天三個方向總的偏移，即相位中心偏移pcv-off[i][j]與del[j]之和
計算2中的相位中心偏移在觀測機關矢量e上的投影dot(off,e,3)，由于e是機關矢量，是以和它求内積實際上就在該方向上的投影；
計算天線相位中心變化量（PCV）：不同的高度角，相位中心變化不同，是以根據高度角對pcv->var[i]進行插值計算。
3、4兩部分即為總的天線偏移：dant[i]=-dot(off,e,3)+(opt?interpvar(90.0-azel[1]*R2D,pcv->var[i]):0.0);
2.4 計算僞距和載波相位殘差 zdres_sat
zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2); /* undifferenced phase/code residual for satellite ---------------------------*/
static void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav,
const double *azel, const double *dant,
const prcopt_t *opt, double *y)
{
const double *lam=nav->lam[obs->sat-1];
double f1,f2,C1,C2,dant_if;
int i,nf=NF(opt);
if (opt->ionoopt==IONOOPT_IFLC) { /* iono-free linear combination */
if (lam[0]==0.0||lam[1]==0.0) return;
if (testsnr(base,0,azel[1],obs->SNR[0]*0.25,&opt->snrmask)||
testsnr(base,1,azel[1],obs->SNR[1]*0.25,&opt->snrmask)) return;
f1=CLIGHT/lam[0];
f2=CLIGHT/lam[1];
C1= SQR(f1)/(SQR(f1)-SQR(f2));
C2=-SQR(f2)/(SQR(f1)-SQR(f2));
dant_if=C1*dant[0]+C2*dant[1];
if (obs->L[0]!=0.0&&obs->L[1]!=0.0) {
y[0]=C1*obs->L[0]*lam[0]+C2*obs->L[1]*lam[1]-r-dant_if;
}
if (obs->P[0]!=0.0&&obs->P[1]!=0.0) {
y[1]=C1*obs->P[0]+C2*obs->P[1]-r-dant_if;
}
}
else {
for (i=0;i<nf;i++) {
if (lam[i]==0.0) continue;
/* check snr mask */
/*
函數testsnr()是用來排除接收信号強度小于規定強度的資料，這個規定信号強度（最小信号強度）
是根據衛星的仰角來得到的（或者說與衛星的仰角有關）。
*/
if (testsnr(base,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)) {
continue;
}
/* residuals = observable - pseudorange 殘差 = 觀測值 - 僞距 */
//huhaoming： 計算每個頻段下的載波相位、僞距殘差：殘差 = 觀測量 - 衛地距 - 天線偏移
//dant:天線相位偏差
//y[0]到y[nf - 1]儲存載波相位殘差，y[nf]到y[2nf - 1]儲存僞距殘差
if (obs->L[i]!=0.0) y[i ]=obs->L[i]*lam[i]-r-dant[i];//載波相位
if (obs->P[i]!=0.0) y[i+nf]=obs->P[i] -r-dant[i];//僞距
}
}
} 處理過程
如果在配置中選擇了無電離層線性組合（IFLC：Ionospheric free linear combination）：
a). 調用testsnr函數檢查L1，L2觀測量的載噪比是否大于配置中SNR Mask的最低要求（SNR Mask設定見RTKlib mannual 3.5章）；
b). 計算無電離層線性組合系數C1、C2, 并采用該系數計算無電離層組合的天線偏移量；
c). 計算無電離層線性組合載波相位和僞距殘差：殘差 = IFLC觀測量 - 衛地距 - 天線偏移量。
如果不是無電離層組合:
a). 調用testsnr，檢查每個頻段的載噪比是否大于配置中SNR Mask的要求；
b). 計算每個頻段下的載波相位、僞距殘差：殘差 = 觀測量 - 衛地距 - 天線偏移量。
3. 選擇移動站和基站之間所有的公共衛星 selsat
/* select common satellites between rover and reference station 選擇流動站和參考站之間的公共衛星 --------------*/
static int selsat(const obsd_t *obs, double *azel, int nu, int nr,
const prcopt_t *opt, int *sat, int *iu, int *ir)
{
int i,j,k=0;
trace(3,"selsat : nu=%d nr=%d\n",nu,nr);
for (i=0,j=nu;i<nu&&j<nu+nr;i++,j++) {
if (obs[i].sat<obs[j].sat) j--;
else if (obs[i].sat>obs[j].sat) i--;
else if (azel[1+j*2]>=opt->elmin) { /* elevation at base station */
sat[k]=obs[i].sat; iu[k]=i; ir[k++]=j;
trace(3,"(%2d) sat=%3d iu=%2d ir=%2d\n",k-1,obs[i].sat,i,j);
}
}
return k;
} 查找算法：
基站和移動站的觀測資料都在obs中，并且obs的衛星号都是經過排序的，是以星号大小是按照從小打到的順序排列。如果移動站所觀察的資料衛星号小于基站所觀察的衛星号，則基站觀察資料不變，依次搜尋移動站的觀察資料，直到不再小于，此時有兩種情況，一種是等于，一種是大于。
等于時：将目前為星号存入sat[0,1….]中，将這顆衛星所對應的移動站觀測資料号i,
基站觀測資料j,分别存入iu[0,1….],ir[0,1…].
大于時：說明移動站沒有觀測的衛星與目前基站的這顆衛星相同。
如果移動站所觀察的資料衛星号大于基站所觀察的衛星号，算法思路與上面的一緻。
4. 目前狀态更新 udstate
主要更新：
- udpos ：kalman中狀态更新方程、狀态協方差方程更新，包括接收機位置、速度、加速度；
- udion ： 電離層狀态、協方差更新；
- udtrop ： 對流層狀态、協方差更新；
- udrcvbias：接收器的時間更新
-
udbias ：更新單差模糊度狀态、協方差；
最後存放到了rtk參數中
4.1 得到相鄰曆元時間差
double tt=rtk->tt,bl,dr[3]; 在前面的時候rtk->tt已經計算過了 rtk->tt=timediff(rtk->sol.time,time); time是前一曆元時間，rtk->sol.time是目前曆元時間
4.2 rover位置、速度、加速度及其方差更新 udpos
udpos(rtk,tt); ：更新位置、速度、加速度，得到rtk->x，也就是浮點解 float狀态
- 如果定位模式是PMODE_FIXED，則将配置中的移動站坐标rtk->opt.ru[]指派給rtk->x，由此得到狀态的協方差矩陣
-
初始化第一個曆元的位置
如果檢測到狀态矩陣位置為0，則說明是目前的曆元是首曆元，我們将狀态的位置部分初始化為單點定位得到的大緻移動站位置，狀态矩陣對應的協方差矩陣為：
- 如果是靜态模式 static mode 則直接傳回，結束udpos
-
如果是 Kinmatic模式（無動力模型）(本次分析屬于這種模式)
目前的狀态矩陣為目前的狀态矩陣為：将狀态的位置部分初始化為單點定位得到的大緻移動站位置。協方差矩陣：
