本博客是转载学习之用，感谢：http://t.csdnimg.cn/rC8iC
pntpos流程图
/* single-point positioning ----------------------------------------------------
* compute receiver position, velocity, clock bias by single-point positioning
* with pseudorange and doppler observables
* args : obsd_t *obs I observation data
* int n I number of observation data
* nav_t *nav I navigation data
* prcopt_t *opt I processing options
* sol_t *sol IO solution
* double *azel IO azimuth/elevation angle (rad) (NULL: no output)
* ssat_t *ssat IO satellite status (NULL: no output)
* char *msg O error message for error exit
* return : status(1:ok,0:error)
* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
* receiver bias are negligible (only involving glonass-gps time offset
* and receiver bias)
*-----------------------------------------------------------------------------*/
extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg)pntpos函数利用多普勒频移测量和伪距进行单点定位，给出接收机位置，速度和钟差
主程序
if (n<=0) {strcpy(msg,"no observation data"); return 0;}//判断卫星个数是否大于零 if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
#if 0
opt_.sateph =EPHOPT_BRDC;//可自己进行调试
#endif
opt_.ionoopt=IONOOPT_BRDC;
opt_.tropopt=TROPOPT_SAAS;
}处理选项opt中的模式不是单点模式时，电离层校正采用broadcast model，对流层校正采用SSaastamoinen model
satposs函数
/* satellite positions and clocks ----------------------------------------------
* compute satellite positions, velocities and clocks
* args : gtime_t teph I time to select ephemeris (gpst)
* obsd_t *obs I observation data
* int n I number of observation data
* nav_t *nav I navigation data
* int ephopt I ephemeris option (EPHOPT_???)
* double *rs O satellite positions and velocities (ecef)
* double *dts O satellite clocks
* double *var O sat position and clock error variances (m^2)
* int *svh O sat health flag (-1:correction not available)
* return : none
* notes : rs [(0:2)+i*6]= obs[i] sat position {x,y,z} (m)
* rs [(3:5)+i*6]= obs[i] sat velocity {vx,vy,vz} (m/s)
* dts[(0:1)+i*2]= obs[i] sat clock {bias,drift} (s|s/s)
* var[i] = obs[i] sat position and clock error variance (m^2)
* svh[i] = obs[i] sat health flag
* if no navigation data, set 0 to rs[], dts[], var[] and svh[]
* satellite position and clock are values at signal transmission time
* satellite position is referenced to antenna phase center
* satellite clock does not include code bias correction (tgd or bgd)
* any pseudorange and broadcast ephemeris are always needed to get
* signal transmission time
*-----------------------------------------------------------------------------*/
extern void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
int ephopt, double *rs, double *dts, double *var, int *svh)satposs函数位于ephemeris.c，按所观测到的卫星顺序计算每颗卫星的位置和速度（rs）、钟差和频漂（dts）
for (i=0;i<n&&i<MAXOBS;i++) {
//按观测数据的顺序，将当前观测卫星的rs、dts、var和svh数组的元素设置为零
for (j=0;j<6;j++) rs [j+i*6]=0.0;
for (j=0;j<2;j++) dts[j+i*2]=0.0;
var[i]=0.0; svh[i]=0;
/* 通过判断某一频率下信号的伪距是否为 0，来得到此时所用的频率个数。
注意，频率个数不能大于 NFREQ（默认为 3）*/
for (j=0,pr=0.0;j<NFREQ;j++) if ((pr=obs[i].P[j])!=0.0) break;
if (j>=NFREQ) {
trace(2,"no pseudorange %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat);
continue;//处理下一个观测数据
}
/* 用数据接收时间减去伪距信号传播时间，得到卫星信号的发射时间*/
time[i]=timeadd(obs[i].time,-pr/CLIGHT);
/* 调用 ephclk函数，由广播星历计算出当前观测卫星的钟差。
注意，此时的钟差是没有考虑相对论效应和TGD的*/
if (!ephclk(time[i],teph,obs[i].sat,nav,&dt)) {
trace(2,"no broadcast clock %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
continue;
}
/*用得到的信号发射时间减去卫星钟差，得到 GPS时间下的卫星信号发射时间 */
time[i]=timeadd(time[i],-dt);
/* 调用satpos函数，计算信号发射时刻卫星的 P(ecef,m)、V(ecef,m/s)、C((s|s/s))。注意，这里计算出的钟差是考虑了相对论效应，没考虑TGD */
if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i,
svh+i)) {
trace(2,"no ephemeris %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
continue;
}
/* 如果上一步计算出的钟偏为0，就再次调用ephclk函数，将其计算出的卫星钟偏作为最终的结果 */
if (dts[i*2]==0.0) {
if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue;
dts[1+i*2]=0.0;
*var=SQR(STD_BRDCCLK);
}
}注：TGD（time group delay）：卫星内部信号从产生到发射的时延之差 ，采用双频接收机时不必要采用。
clock {bias,drift} (s|s/s) 频漂drift单位？？？
ephclk 函数利用广播星历计算卫星钟差
static int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *dts)
{
eph_t *eph;
geph_t *geph;
seph_t *seph;
int sys;
trace(4,"ephclk : time=%s sat=%2d\n",time_str(time,3),sat);
sys=satsys(sat,NULL);//satsys函数根据卫星编号确定该卫星所属的导航系统和该卫星的PRN编号
if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
if (!(eph=seleph(teph,sat,-1,nav))) return 0;
//调用seleph函数来选择toe值与星历选择时间标准teph最近的那个星历
*dts=eph2clk(time,eph);//eph2clk函数通过广播星历和信号发射时间计算卫星钟差
}
else if (sys==SYS_GLO) {
if (!(geph=selgeph(teph,sat,-1,nav))) return 0;
*dts=geph2clk(time,geph);
}
else if (sys==SYS_SBS) {
if (!(seph=selseph(teph,sat,nav))) return 0;
*dts=seph2clk(time,seph);
}
else return 0;
return 1;
}satpos函数计算卫星位置
/* satellite position and clock ------------------------------------------------
* compute satellite position, velocity and clock
* args : gtime_t time I time (gpst)
* gtime_t teph I time to select ephemeris (gpst)
* int sat I satellite number
* nav_t *nav I navigation data
* int ephopt I ephemeris option (EPHOPT_???)
* double *rs O sat position and velocity (ecef)
* {x,y,z,vx,vy,vz} (m|m/s)
* double *dts O sat clock {bias,drift} (s|s/s)
* double *var O sat position and clock error variance (m^2)
* int *svh O sat health flag (-1:correction not available)
* return : status (1:ok,0:error)
* notes : satellite position is referenced to antenna phase center
* satellite clock does not include code bias correction (tgd or bgd)
*-----------------------------------------------------------------------------*/
extern int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
const nav_t *nav, double *rs, double *dts, double *var,
int *svh)
satpos函数判断星历类型，调用不同的函数计算观测瞬间卫星的平近点角、偏近点角、升交距角 u、卫星矢径 r 、卫星轨道倾角 i、卫星在轨道面坐标系中的位置、卫星在ECEF坐标系中的位置
调用estpos函数计算接收机位置
/* estimate receiver position with pseudorange */
stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);通过伪距观测值实现绝对定位，计算出接收机的位置和钟差，顺带返回实现定位后每颗卫星的{方位角、仰角}、定位时有效性、定位后伪距残差
for (i=0;i<3;i++) x[i]=sol->rr[i];//将sol->rr的前3项赋值给x数组第一次定位时，输入的sol为空，x初值为零；若之前有过定位，则可以将上一历元的定位值作为该历元定位的初始值
进行迭代定位
调用rescode函数
计算在当前接收机位置和钟差值的情况下，定位方程的右端部分 v(nv\*1)、几何矩阵 H(NX*nv)、此时所得的伪距残余的方差 var、所有观测卫星的 azel{方位角、仰角}、定位时有效性 vsat、定位后伪距残差 resp、参与定位的卫星个数 ns和方程个数 nv
static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, const prcopt_t *opt,
double *v, double *H, double *var, double *azel, int *vsat,
double *resp, int *ns)rescode函数完成了解算模型的线性化，以及伪距补偿，加权参数估计等工作
调用ecef2pos函数
ecef2pos函数将将 ECEF坐标系(X、Y、Z)转换成大地坐标系{lat,lon,h}
/* transform ecef to geodetic postion ------------------------------------------
* transform ecef position to geodetic position
* args : double *r I ecef position {x,y,z} (m)
* double *pos O geodetic position {lat,lon,h} (rad,m)
* return : none
* notes : WGS84, ellipsoidal height
*-----------------------------------------------------------------------------*/
extern void ecef2pos(const double *r, double *pos)调用satsys函数
根据卫星编号确定该卫星所属导航系统和该卫星在该系统中的PRN编号
/* satellite number to satellite system ----------------------------------------
* convert satellite number to satellite system
* args : int sat I satellite number (1-MAXSAT)
* int *prn IO satellite prn/slot number (NULL: no output)
* return : satellite system (SYS_GPS,SYS_GLO,...)
*-----------------------------------------------------------------------------*/
extern int satsys(int sat, int *prn)处理过程
- 为处理意外情况（卫星号不在
1-MAXSAT之内），先令卫星系统为SYS_NONE。- 按照 TRKLIB中定义相应宏的顺序来判断是否是 GPS、GLO、GAL系统等，判断标准是将卫星号减去前面的导航系统所拥有的卫星个数，来判断剩余卫星个数是否小于等于本系统的卫星个数。
- 确定所属的系统后，通过加上最小卫星编号的
PRN再减去 1，得到该卫星在该系统中的PRN编号。
调用geodist函数
geodist计算卫星与当前接收机位置之间的几何距离和接收机到卫星方向的单位向量
/* 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)处理过程
- 检查卫星到 WGS84坐标系原点的距离是否大于基准椭球体的长半径。
ps-pr，计算由接收机指向卫星方向的观测矢量，之后在计算出相应的单位矢量。- 考虑到地球自转，即信号发射时刻卫星的位置与信号接收时刻卫星的位置在 WGS84坐标系中并不一致，进行关于
Sagnac效应的校正。
调用satazel函数
satazel函数计算在接收机位置处的站心坐标系中卫星的方位角和仰角
/* 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)- 调用 ecef2enu函数，将接收机指向卫星方向的观测矢量转换到以接收机为原点的站心坐标系中。
- 调用
atan2函数计算出方位角，然后再算出仰角。
调用prange函数
修正伪距值
/* psendorange with code bias correction --------------------
函数参数，6个
obsd_t *obs I observation data
nav_t *nav I navigation data
double *azel I 对于当前定位值，每一颗观测卫星的 {方位角、高度角}
int iter I 迭代次数
prcopt_t *opt I processing options
double *var O 伪距测量的码偏移误差
返回类型：
double O 最终能参与定位解算的伪距值
------------------------------------------------------------*/
static double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
int iter, const prcopt_t *opt, double *var)处理过程
- 首先调用 satsys确定该卫星属于 RTKLIB设计时给定的几个导航系统之中。
- 如果
NFREQ>=3且该卫星属于GAL/SBS系统，则j=2。而如果出现NFREQ<2||lam[i]==0.0||lam[j]==0.0中的其中一个，直接返回 0.- 当
iter>0时，调用 testsnr函数，测试第i、j(IFLC)个频率信号的载噪比是否符合要求。- 计算出 值(f1^2/f2^2，见ICD-GPS-200C P90)，从
obs和nav数据中读出测量伪距值和码偏移值。- 从数据中读出的P1_P2==0（无DCB），则使用 TGD代替，TGD值由 gettgd函数计算得到。
- 如果
ionoopt==IONOOPT_IFLC，根据obs->code的值来决定是否对 P1、P2进行修正，之后再组合出 IFLC时的伪距值(ICD-GPS-200C P91)。否则，则是针对单频接收即进行的数据处理。先对 P1进行修正，然后再计算出伪距值（PC）- 如果
sateph==EPHOPT_SBAS，则还要对 PC进行处理。之后给该函数计算出的伪距值的方差赋值。
if (opt->ionoopt==IONOOPT_IFLC) { /* dual-frequency */
if (P1==0.0||P2==0.0) return 0.0;
if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
if (obs->code[j]==CODE_L2C) P2+=P2_C2; /* C2->P2 */
/* iono-free combination 消电离层组合*/
PC=(gamma*P1-P2)/(gamma-1.0);
}消电离层组合
调用satexclude函数
检测某颗卫星在定位时是否需要将其排除，排除标准为该卫星是否处理选项预先规定的导航系统或排除标志
/* test excluded satellite -----------------------------------------------------
* test excluded satellite
* args : int sat I satellite number
* int svh I sv health flag
* prcopt_t *opt I processing options (NULL: not used)
* return : status (1:excluded,0:not excluded)
*-----------------------------------------------------------------------------*/
extern int satexclude(int sat, int svh, const prcopt_t *opt)处理过程
注意事项
- 3中在比较该卫星与预先规定的导航系统是否一致时，使用了
sys&opt->navsys来进行比较。这样做的好处是当opt->navsys=sys或opt->navsys=SYS_ALL时，结果都会为真。之所以会这样，是因为在rtklib.h文件中定义这些导航系统变量的时候，所赋的值在二进制形式下都是只有一位为1的数。
调用ionocorr函数
计算给定电离层选项时的电离层延时(m)
/* ionospheric correction ------------------------------------------------------
* compute ionospheric correction
* args : gtime_t time I time
* nav_t *nav I navigation data
* int sat I satellite number
* double *pos I receiver position {lat,lon,h} (rad|m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* int ionoopt I ionospheric correction option (IONOOPT_???)
* double *ion O ionospheric delay (L1) (m)
* double *var O ionospheric delay (L1) variance (m^2)
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
const double *azel, int ionoopt, double *ion, double *var)-
处理过程：
-
注意事项：
调用tropcorr函数
计算对流层延时(m)及其方差
/* tropospheric correction -----------------------------------------------------
* compute tropospheric correction
* args : gtime_t time I time
* nav_t *nav I navigation data
* double *pos I receiver position {lat,lon,h} (rad|m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* int tropopt I tropospheric correction option (TROPOPT_???)
* double *trp O tropospheric delay (m)
* double *var O tropospheric delay variance (m^2)
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int tropcorr(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, int tropopt, double *trp, double *var)-
处理过程：
- 当
tropopt==TROPOPT_SAAS或一些其它情况时，调用 tropmodel函数，计算Saastamoinen模型下的对流层延时。
- 当
-
注意事项：
- 对流层延时与信号频率无关，所以这里计算得到的值并不是只针对于
L1信号！
- 对流层延时与信号频率无关，所以这里计算得到的值并不是只针对于
计算伪距残差
/* pseudorange residual */
v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);其中
P为prange函数计算出的伪距值
r为geodist计算出的星地几何距离
dts[i*2]为卫星钟差
dion和dtrp分别为电离层延时误差和对流层延时误差
进行几何矩阵设计
/* design matrix */
for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0);
/* time system and receiver bias offset correction */
if (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;}
else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;}
else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}
else mask[0]=1;
vsat[i]=1; resp[i]=v[nv]; (*ns)++;前 3行为 geodist函数计算得到的视线单位向量的反向，第 4行为 1，其它行为 0。
将参与定位的卫星的定位有效性标志设为 1，给当前卫星的伪距残余赋值，参与定位的卫星个数 ns加 1
调用varerr函数
调用 varerr函数计算导航系统误差（可能会包括 IFLC选项时的电离层延时），然后累加计算用户测距误差(URE)： ①卫星星历和钟差的误差 ②大气延时误差 ③伪距测量的码偏移误差 ④导航系统的误差
为了防止H亏秩，人为的添加了几组观测方程
/* error variance */
//计算导航系统误差
var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
}
/* constraint to avoid rank-deficient */
for (i=0;i<4;i++) {
if (mask[i]) continue;
v[nv]=0.0;
for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;
var[nv++]=0.01;
}
return nv;确定权重
以伪距残余的标准差的倒数作为权重
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}//确定方程组个数>未知数个数
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}//以伪距残余的标准差的倒数作为权重，H和v分别左乘权重对角阵，得到加权后的H和v- 确定方程组中方程的个数要大于未知数的个数。
- 以伪距残余的标准差的倒数作为权重，对 H和 v分别左乘权重对角阵，得到加权之后的 H和 v。
调用lsq函数
由lsq函数得到x的修改量和定位误差协方差矩阵中的权系数阵，并将x修改量加入当前的x值中，更新x值
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++) x[j]+=dx[j];//更新x值/* least square estimation -----------------------------------------------------
* least square estimation by solving normal equation (x=(A*A')^-1*A*y)
* args : double *A I transpose of (weighted) design matrix (n x m)
* double *y I (weighted) measurements (m x 1)
* int n,m I number of parameters and measurements (n<=m)
* double *x O estmated parameters (n x 1)
* double *Q O esimated parameters covariance matrix (n x n)
* return : status (0:ok,0>:error)
* notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2))
* matirix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
extern int lsq(const double *A, const double *y, int n, int m, double *x,
double *Q)
{
double *Ay;
int info;
if (m<n) return -1;
Ay=mat(n,1);
matmul("NN",n,1,m,1.0,A,y,0.0,Ay); /* Ay=A*y */
matmul("NT",n,n,m,1.0,A,A,0.0,Q); /* Q=A*A' */
if (!(info=matinv(Q,n))) matmul("NN",n,1,n,1.0,Q,Ay,0.0,x); /* x=Q^-1*Ay */
free(Ay);
return info;
} lsq函数的本质为求出了间接平差中的参数改正数和
,返回的info即
注：lsq函数中不需要观测值权阵，因为权阵已在上一步中分配到伪距残余v和几何矩阵H中
迭代判断
if (norm(dx,NX)<1E-4) {
/* 对sol参数进行赋值，代码略 */
/* validate solution */
//调用valsol函数确认当前解是否满足要求（伪距残余小于某个值、GDOP小于某个阈值）
if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
}
free(v); free(H); free(var);
return stat;
}若求得的x的修改量小于截断因子(目前是1e-4)，则将 上一步更新的x值作为最终的定位结果，对 sol的相应参数赋值，之后再调用 valsol函数确认当前解是否符合要求（伪距残差小于某个值和 GDOP小于某个门限值）。否则，进行下一次循环
注：
1、伪距残差检验使用的是卡方检验，这个检验对低精度板卡来说有点严格，若解算对象为低精度板卡，这个检测应该禁掉，否则很多结算结果是无法通过的
2、GDOP值仅与卫星的几何分布有关，azel存储卫星的方位角和仰角信息
如果超过了规定的循环次数MAXITR=10，则输出发散信息后，返回 0
调用raim_fde函数
RAIM即Receiver Autonomous Integrity Monitoring，FDE即Fault Detection Exclusion,是接收机自主完好性监测和故障检测与排除
/* raim fde */
if (!stat&&n>=6&&opt->posopt[4]) {
stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);raim_fde函数对计算得到的定位结果进行接收机自主正直性检测（RAIM），通过再次使用 vsat数组，这里只会在对定位结果有贡献的卫星数据进行检测
raim_fde返回stat，为0时说明没有找到更好的解，为1时说明在弃用卫星时有更好的解出现
/* raim fde (failure detection and exclution) ------------------------------
函数参数，13个：
obsd_t *obs I observation data
int n I number of observation data
double *rs I satellite positions and velocities，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double *dts I satellite clocks，长度为2*n， {bias,drift} (s|s/s)
double *vare I sat position and clock error variances (m^2)
int *svh I sat health flag (-1:correction not available)
nav_t *nav I navigation data
prcopt_t *opt I processing options
sol_t *sol IO solution
double *azel IO azimuth/elevation angle (rad)
int *vsat IO 表征卫星在定位时是否有效
double *resp IO 定位后伪距残差 (P-(r+c*dtr-c*dts+I+T))
char *msg O error message for error exit
返回类型:
int O (1:ok,0:error)
--------------------------------------------------------------------------*/
static int raim_fde(const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
double *azel, int *vsat, double *resp, char *msg)处理过程：
- 观测卫星数目的循环，每次舍弃一颗卫星，计算使用余下卫星进行定位的定位值。
- 舍弃一颗卫星后，将剩下卫星的数据复制到一起，调用 estpos函数，计算使用余下卫星进行定位的定位值。
- 累加使用当前卫星实现定位后的伪距残差平方和与可用卫星数目，如果
nvsat<5，则说明当前卫星数目过少，无法进行RAIM_FDE操作。- 计算伪距残差平方和的标准平均值，如果小于
rms，则说明当前定位结果更合理，将stat置为 1，重新更新sol、azel、vsat(当前被舍弃的卫星，此值置为0)、resp等值，并将当前的rms_e更新到 `rms'中。- 继续弃用下一颗卫星，重复 2-4操作。总而言之，将同样是弃用一颗卫星条件下，伪距残差标准平均值最小的组合所得的结果作为最终的结果输出。
- 如果 stat不为 0，则说明在弃用卫星的前提下有更好的解出现，输出信息，指出弃用了哪颗卫星。
- 使用
free函数回收相应内存。
注意事项：
- 使用了
mat和malloc函数后要使用free函数来回收内存。- 源码中有很多关于 i、j、k的循环。其中，i表示最外面的大循环，每次将将第 i颗卫星舍弃不用，这是通过
if (j==i) continue实现的；j表示剩余使用的卫星的循环，每次进行相应数据的赋值；k表示参与定位的卫星的循环，与 j一起使用。- 关于raim_fde，至少需要6颗卫星。如果有5颗卫星，那么在5颗卫星的情况下，进行raim检测的时候，去掉1颗星，剩下4颗。4个观测量对应解4个参数，残差为0，RMS为0，无法筛选出更好的定位解。
调用estvel函数
/* estimate receiver velocity with doppler */
if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);出现依靠多普勒频移测量值计算接收机的速度。在弃用卫星时有更好的解时，调用estvel函数计算接收机速度。
注：多普勒频移观测值即结构体obsd_t中的D
通过if（stat）条件实现只使用通过了上一步 RAIM_FDE操作后得到的最优解的卫星数据，所以对于计算出的速度就没有再次进行 RAIM_FDE检验
/* estimate receiver velocity ------------------------------------------------
函数参数，9个：
obsd_t *obs I observation data
int n I number of observation data
double *rs I satellite positions and velocities，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double *dts I satellite clocks，长度为2*n， {bias,drift} (s|s/s)
nav_t *nav I navigation data
prcopt_t *opt I processing options
sol_t *sol IO solution
double *azel IO azimuth/elevation angle (rad)
int *vsat IO 表征卫星在定位时是否有效
返回类型:
int O (1:ok,0:error)
----------------------------------------------------------------------------*/
static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
const double *azel, const int *vsat)处理过程：
解算原理
调用resdop函数
调用resdop函数，计算定速方程组左边的几何矩阵和右端的速度残余，返回定速时所使用的卫星数目
/* doppler residuals ---------------------------------------------------------
函数参数，11个：
obsd_t *obs I observation data
int n I number of observation data
double *rs I satellite positions and velocities，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double *dts I satellite clocks，长度为2*n， {bias,drift} (s|s/s)
nav_t *nav I navigation data
double *rr I receiver positions and velocities，长度为6，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double *x I 本次迭代开始之前的定速值
double *azel I azimuth/elevation angle (rad)
int *vsat I 表征卫星在定速时是否有效
double *v O 定速方程的右端部分，速度残余
double *H O 定速方程中的几何矩阵
返回类型:
int O 定速时所使用的卫星数目
----------------------------------------------------------------------------*/
static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const double *rr, const double *x,
const double *azel, const int *vsat, double *v, double *H)-
处理过程：
- 调用 ecef2pos函数，将接收机位置由 ECEF转换为大地坐标系。
- 调用 xyz2enu函数，计算此时的坐标转换矩阵。
- 满足下列条件
obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0之一，则当前卫星在定速时不可用，直接进行下一次循环。 - 计算当前接收机位置下 ENU中的视向量，然后转换得到 ECEF中视向量的值。
- 计算 ECEF中卫星相对于接收机的速度，然后再计算出考虑了地球自转的用户和卫星之间的几何距离变化率，校正公式见
RTKLIB manual P159 (E.6.29)。 - 根据公式 计算出方程右端项的多普勒残余，然后再构建左端项的几何矩阵。最后再将观测方程数增 1.
给ssat_t结构体赋值
将 ssat_t结构体数组的 vs(定位时有效性)、azel（方位角、仰角）、resp(伪距残余)、resc(载波相位残余)和 snr(信号强度)都置为 0，然后再将实现定位后的 azel、snr赋予 ssat_t结构体数组，而 vs、resp则只赋值给那些对定位有贡献的卫星，没有参与定位的卫星，这两个属性值为 0
if (azel) {
for (i=0;i<n*2;i++) azel[i]=azel_[i];
}
if (ssat) {
for (i=0;i<MAXSAT;i++) {
ssat[i].vs=0;
ssat[i].azel[0]=ssat[i].azel[1]=0.0;
ssat[i].resp[0]=ssat[i].resc[0]=0.0;
ssat[i].snr[0]=0;
}
for (i=0;i<n;i++) {
ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
if (!vsat[i]) continue;
ssat[obs[i].sat-1].vs=1;
ssat[obs[i].sat-1].resp[0]=resp[i];
}
}
free(rs); free(dts); free(var); free(azel_); free(resp);
return stat;总结
- pntpos函数返回stat，为0时说明没有找到更好的解，为1时说明在弃用某颗卫星时有更好的解出现
- pntpos函数只给出了接收机的钟差，而没有计算接收机的频漂，原因在于 estvel函数中虽然计算得到了接收机频漂，但并没有将其输出到
sol_t:dtr中- C语言中用
malloc申请的内存需要自己调用free来予以回收，源码中的mat、imat、zeros等函数都只是申请了内存，并没有进行内存的回收，在使用这些函数时，用户必须自己调用free来回收内存！源码中将使用这些函数的代码放置在同一行，在调用函数结尾处也统一进行内存回收，位置较为明显，不致于轻易忘记
1581
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
