1 单点定位整体框架、输入输出、配置、卫星位置计算
1.1 rtkpos( )函数
功能：处理单历元的所有卫星观测值数据（流动站+基准站）
参数：
[1] rtk_t *rtk
[2] const obsd_t *obs 每个历元所有的观测值
[3] int n 卫星数量。也是结构体数组obs下标大小
[4] const nav_t *nav 星历
首先备份配置参数，因为后面使用时会改。之后统计基准站和流动站个数，记录上一历元的时间，进行流动站单点定位pntpos()，然后计算两历元的时差。最后选择不同的定位模式进行解算、输出
extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
prcopt_t *opt=&rtk->opt; //先复制一份配置，因为后面要改
sol_t solb={{0}};
gtime_t time;
int i,nu,nr;
char msg[128]="";
trace(3,"rtkpos : time=%s n=%d\n",time_str(obs[0].time,3),n);
trace(4,"obs=\n"); traceobs(4,obs,n);
/*trace(5,"nav=\n"); tracenav(5,nav);*/
/* set base staion position */
if (opt->refpos<=3&&opt->mode!=PMODE_SINGLE&&opt->mode!=PMODE_MOVEB) {
for (i=0;i<6;i++) rtk->rb[i]=i<3?opt->rb[i]:0.0;
}
/* count rover/base station observations */
for (nu=0;nu <n&&obs[nu ].rcv==1;nu++) ; //流动站个数统计
for (nr=0;nu+nr<n&&obs[nu+nr].rcv==2;nr++) ; //基准站个数统计
time=rtk->sol.time; /* previous epoch */ //上一历元的时间，时间的获取是在ontpos()这个函数里
/* rover position by single point positioning 流动站单点定位 */
if (!pntpos(obs,nu,nav,&rtk->opt,&rtk->sol,NULL,rtk->ssat,msg)) {
errmsg(rtk,"point pos error (%s)\n",msg);
if (!rtk->opt.dynamics) {
outsolstat(rtk);
return 0;
}
}
//得出两个历元的时间差
//rtk->tt，time difference between current and previous
if (time.time!=0) rtk->tt=timediff(rtk->sol.time,time);
//下面是选择定位的模式
/* single point positioning 单点定位*/ //如果是单点定位，那么我们之前就解算过了，直接输出即可
if (opt->mode==PMODE_SINGLE) {
outsolstat(rtk);
return 1;
}
/* precise point positioning 精密单点定位*/
if (opt->mode>=PMODE_PPP_KINEMA) {
pppos(rtk,obs,nu,nav);
pppoutsolstat(rtk,statlevel,fp_stat);
return 1;
}
/* check number of data of base station and age of differential */
if (nr==0) {
errmsg(rtk,"no base station observation data for rtk\n");
outsolstat(rtk);
return 1;
}
if (opt->mode==PMODE_MOVEB) { /* moving baseline 基线 */
/* estimate position/velocity of base station */
if (!pntpos(obs+nu,nr,nav,&rtk->opt,&solb,NULL,NULL,msg)) {
errmsg(rtk,"base station position error (%s)\n",msg);
return 0;
}
rtk->sol.age=(float)timediff(rtk->sol.time,solb.time);
if (fabs(rtk->sol.age)>TTOL_MOVEB) {
errmsg(rtk,"time sync error for moving-base (age=%.1f)\n",rtk->sol.age);
return 0;
}
for (i=0;i<6;i++) rtk->rb[i]=solb.rr[i];
/* time-synchronized position of base station */
for (i=0;i<3;i++) rtk->rb[i]+=rtk->rb[i+3]*rtk->sol.age;
}
else {
rtk->sol.age=(float)timediff(obs[0].time,obs[nu].time);
if (fabs(rtk->sol.age)>opt->maxtdiff) {
errmsg(rtk,"age of differential error (age=%.1f)\n",rtk->sol.age);
outsolstat(rtk);
return 1;
}
}
/* relative potitioning */
relpos(rtk,obs,nu,nr,nav);
outsolstat(rtk);
return 1;
}
1.2 pntpos( )函数
解算接收机(流动站接收机)位置、速度、钟差
* 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
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)
{
prcopt_t opt_=*opt;
double *rs,*dts,*var,*azel_,*resp; //定义的指针，为了后面申请内存使用，二维数组。
int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
//先设置没有输出
sol->stat=SOLQ_NONE;
//判断有没有卫星，没有直接返回
if (n<=0) {strcpy(msg,"no observation data"); return 0;}
trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
//获取该历元时间。因为都是这个历元的数据，下标无所谓
sol->time=obs[0].time; msg[0]='\0';
//申请内存，数组
//mat() malloc一段内存。每个数组含义，后面有解释
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
//如果不是单点定位，启用电离层模型纠正
if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
#if 0
opt_.sateph =EPHOPT_BRDC;
#endif
opt_.ionoopt=IONOOPT_BRDC;
opt_.tropopt=TROPOPT_SAAS;
}
/* satellite positons, velocities and clocks 计算卫星位置、速度和时间*/
satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
/* estimate receiver position with pseudorange 评价精度*/
stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
/* 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);
}
/* estimate receiver velocity with doppler */
if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
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;
}1.3 mat( )函数
malloc( )申请内存。在pntpos()函数中，申请的是m*n大小的空间，二维数组。但我们需要清除，这其实是内存的一段连续空间
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
n是卫星数量。推测rs是每颗卫星包含的6种信息，dts是卫星包含的2种信息.....
rs：x、y、z、vx、vy、vz satellite positions and velocities
dts：钟、钟piao satellite clocks
var：方差，精度。 sat position and clock error variances (m^2)
azel_：高度角/方位角
resp：先验残差，具体可见estpos()函数详解，后文会说明
extern double *mat(int n, int m)
{
double *p;
if (n<=0||m<=0) return NULL;
if (!(p=(double *)malloc(sizeof(double)*n*m))) {
fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m);
}
return p;
}1.4 satposs()函数
计算的是卫星信号对应的卫星发射时刻的卫星位置、速度，所以我们得先计算出卫星发射时刻是多少。因为传时间参数--teph是接收机接收时刻，我们需要使用伪距去推算卫星发射时刻。
- 卫星发射信号时刻，理论计算
[1] 卫星钟差假设没有，只考虑接收机钟差。但是(接收机钟差、真实卫地距、传播时间)我们不知道，需要把这个未知量代换
[2] 伪距观测方程
我们不考虑电离层、对流层、噪声影响。
[3] 推导，由伪距观测方方程。卫星钟差可以通过导航电文获得，伪距也有
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)
{
gtime_t time[MAXOBS]={{0}};
double dt,pr;
int i,j;
//teph是时间。每包数据都有个时间，这个时间是接收机捕获观测值时，接收机的钟面时刻
trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt);
for (i=0;i<n&&i<MAXOBS;i++) {
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;
/* search any psuedorange 伪距 */
//选择任意伪距值即可
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;
}
//计算卫星发射时刻
/* transmission time by satellite clock */
time[i]=timeadd(obs[i].time,-pr/CLIGHT); //pr是伪距
/* satellite clock bias by broadcast ephemeris 通过广播星历获取卫星钟钟差*/
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;
}
time[i]=timeadd(time[i],-dt); //至此，我们已经得出卫星发射时刻
/* satellite position and clock at transmission time 卫星信号发射时刻，卫星位置计算 */
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;
}
/* if no precise clock available, use broadcast clock instead */
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);
}
}
for (i=0;i<n&&i<MAXOBS;i++) {
trace(4,"%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
time_str(time[i],6),obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],
dts[i*2]*1E9,var[i],svh[i]);
}
}1.5 satpos()
卫星位置和钟解算。我们只针对广播星历进行介绍，ephpos()
1.6 ephpos()
两次调用eph2pos()，计算卫星位置。第一次调用是卫星信号发射时刻的卫星位置，第二次调用是间隔tt事件后的位置。位移差，除以tt就得出速度
eph2pos()是一个接口，broadcast ephemeris to satellite position and clock bias。由广播星历得出卫星位置和钟差
static int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int iode, double *rs, double *dts, double *var, int *svh)
{
eph_t *eph;
geph_t *geph;
seph_t *seph;
double rst[3],dtst[1],tt=1E-3;
int i,sys;
trace(4,"ephpos : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode);
sys=satsys(sat,NULL);
*svh=-1;
if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
if (!(eph=seleph(teph,sat,iode,nav))) return 0;
eph2pos(time,eph,rs,dts,var);//这个就算出卫星位置了
time=timeadd(time,tt);
eph2pos(time,eph,rst,dtst,var);//调用两次，求解tt时间间隔的位置。之后rst-rs得出位移变化，再除以tt求出速度。后面这个是1ms后的位置
*svh=eph->svh;
}
else if (sys==SYS_GLO) {
if (!(geph=selgeph(teph,sat,iode,nav))) return 0;
geph2pos(time,geph,rs,dts,var);
time=timeadd(time,tt);
geph2pos(time,geph,rst,dtst,var);
*svh=geph->svh;
}
else if (sys==SYS_SBS) {
if (!(seph=selseph(teph,sat,nav))) return 0;
seph2pos(time,seph,rs,dts,var);
time=timeadd(time,tt);
seph2pos(time,seph,rst,dtst,var);
*svh=seph->svh;
}
else return 0;
/* satellite velocity and clock drift by differential approx */
for (i=0;i<3;i++) rs[i+3]=(rst[i]-rs[i])/tt; //速度计算。i+3，对应着[x,y,z,vx,vy,vz] 从vx开始
dts[1]=(dtst[0]-dts[0])/tt; //钟piao计算
return 1;
}2 估计接收机位置
pntpos()函数下，解算出卫星位置速度，就解算接收机位置。先验残差推导，相关理论知识见下
2.1 流程图
2.2 estpos()函数
需要注意：rtklib中的矩阵是按列存的，和我们日常所使用的呈转置关系
/* 没有const，一般是输出
obs 观测值
n 卫星数量
rs 卫星位置和速度(x/y/z/vx/vy/vz)
dts 钟和钟piao!!
vare 卫星位置和钟的方差
svh 卫星健康
nav 星历
azel 高度角/方位角
vast 卫星是否可用
resp 卫星先验残差
msg 错误信息(Debug使用）
static int estpos(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)
{
//NX 4+3,4是(xyz钟），3是其他系统？？
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
trace(3,"estpos : n=%d\n",n);
//v 先验残差 H 设计矩阵 var 方差
v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
//数组x就是接收机概略位置，赋予rr前三个元素(xyz)(0,0,0)
for (i=0;i<3;i++) x[i]=sol->rr[i];
//迭代，使得概略位置逐渐精确
//MAXITR最大迭代次数
for (i=0;i<MAXITR;i++) {
/* pseudorange residuals 伪距残差*/
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
//观测值(卫星数/方程数)少于未知数(xyz+系统钟)，就无解直接退出
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;
}
/* 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]; //更新位置
//收敛判断，当dx[]变化少于一定值
if (norm(dx,NX)<1E-4) {
sol->type=0;
sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */
sol->qr[4]=(float)Q[2+NX]; /* cov yz */
sol->qr[5]=(float)Q[2]; /* cov zx */
sol->ns=(unsigned char)ns;//卫星数量
sol->age=sol->ratio=0.0;
/* validate solution */
//判断定位结果是否正确
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;
}
}
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(H); free(var);
return 0;
}2.3 rescode( )
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)
{
double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1;
int i,j,nv=0,sys,mask[4]={0};
trace(3,"resprng : n=%d\n",n);
//rr[]局部变量--接收机概略位置，赋予给rr[]
//最开始概略位置rr[]={0,0,0} 钟也是0
for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3];
//xyz-->BLH 坐标转换
ecef2pos(rr,pos); //这里并不是改正rr[]的值，而是产生的一个pos[]这里面是BLH,rr[]是xyz,都表示同一个位置
//遍历所有卫星
for (i=*ns=0;i<n&&i<MAXOBS;i++) {
vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0; //azel有高度角和方位角，所有*2
if (!(sys=satsys(obs[i].sat,NULL))) continue; //判断是那个导航系统GPS/GLOS/BDL
/* reject duplicated observation data 去掉重复观测数据*/
//依据的是sat是否相同，sat是卫星编号
if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) {
trace(2,"duplicated observation data %s sat=%2d\n",
time_str(obs[i].time,3),obs[i].sat);
i++;
continue;
}
/* geometric distance/azimuth/elevation angle
geodist()卫星与概率位置距离r计算+方向余弦e计算
satazel()高度角/方位角计算
*/
if ((r=geodist(rs+i*6,rr,e))<=0.0||
satazel(pos,e,azel+i*2)<opt->elmin) continue;
/* psudorange with code bias correction */
//码偏差修正，P是经过修正的伪距
if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue;
/* excluded satellite? */
if (satexclude(obs[i].sat,svh[i],opt)) continue;
/* 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]);
}
/* tropospheric corrections 对流层修正*/
if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {
continue;
}
/* pseudorange residual */
//dtr是接收机钟的概略。不懂！！为什么减去dtr
v[nv]=P-(r+dtr-CLIGHT*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)++; //resp收的是先验残差
/* error variance */
//方差 精度，30分钟！！
//varerr()观测值误差 vare卫星位置误差和钟差 vmeans 码偏差修正方差 vion 电离层 vtrp对流层
//var[]伪距方差，都是方差加和
var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
trace(4,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n",obs[i].sat,
azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1]));
}
/* 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;
}设计矩阵构建
每行前三列都是dx,dy,dz的系数，第四列系数是1.
后面几列是其他系统的相对于GPS钟的钟差
mask[ ]标记是否有该卫星系统的值
添加约数
循环四次（对应四个导航系统）GPS/GLA/BDS/GLO。根据mask[ ]标记，在没有值的那一列，填个1
2.4 geodist( )函数
* 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)e是单位向量，表示卫星与概率位置的方向
地球自转修正借鉴如下公式
{
double r;
int i;
//norm()距离计算，卫星距地心距离，判断是否小于地球半径。小于地球半径，说明就不是卫星
if (norm(rs,3)<RE_WGS84) return -1.0;
for (i=0;i<3;i++) e[i]=rs[i]-rr[i]; //Xs-Xo
r=norm(e,3);//卫星与概略位置距离
for (i=0;i<3;i++) e[i]/=r;//方向余弦计算
return r+OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;//后面是地球自转改正，这里只是把卫地距换成了卫星与概略位置的距离
}rs[0]是卫星x
rr[1]是概略位置y，依次类推
2.5 satazel( ) 高度角方位角
* args : double *pos I geodetic position {lat,lon,h} (rad,m) 卫星位置的BLH
* double *e I receiver-to-satellilte unit vevtor (ecef)
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
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);//xyz-->enu，产生了enu[],pos[]值并没有变
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;
}2.6 ionocorr( )电离层修正
根据配置，选择相应的模型进行修正
* 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) 电离层方差
extern int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
const double *azel, int ionoopt, double *ion, double *var)
{
trace(4,"ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n",
time_str(time,3),ionoopt,sat,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,
azel[1]*R2D);
/* broadcast model */
if (ionoopt==IONOOPT_BRDC) {
*ion=ionmodel(time,nav->ion_gps,pos,azel);
*var=SQR(*ion*ERR_BRDCI);
return 1;
}
/* sbas ionosphere model */
if (ionoopt==IONOOPT_SBAS) {
return sbsioncorr(time,nav,pos,azel,ion,var);
}
/* ionex tec model */
if (ionoopt==IONOOPT_TEC) {
return iontec(time,nav,pos,azel,1,ion,var);
}
/* qzss broadcast model */
if (ionoopt==IONOOPT_QZS&&norm(nav->ion_qzs,8)>0.0) {
*ion=ionmodel(time,nav->ion_qzs,pos,azel);
*var=SQR(*ion*ERR_BRDCI);
return 1;
}
/* lex ionosphere model */
if (ionoopt==IONOOPT_LEX) {
return lexioncorr(time,nav,pos,azel,ion,var);
}
*ion=0.0;
*var=ionoopt==IONOOPT_OFF?SQR(ERR_ION):0.0;
return 1;
}2.7 tropcorr( ) 对流层
同上
2.8 prange( )伪距码偏差修正
略，还不太懂
2.9 varerr( ) 方差
观测值误差
varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el)); //观测值误差
针对这个，我们通常采用如下定义观测值误差
static double varerr(const prcopt_t *opt, double el, int sys)
{
double fact,varr;
fact=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS); //不同系统，权重是不一样的
varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el)); //观测值误差
if (opt->ionoopt==IONOOPT_IFLC) varr*=SQR(3.0); /* iono-free */
return SQR(fact)*varr;
}2.10 lsq( )最小二乘法
solving normal equation (x=(A*A')^-1*A*y) 这里的计算公式不同，是因为rtklib的矩阵按列存储，所以和计算公式相反
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;
}2.11 raim_fde( )接收机自主性检验
遍历所有卫星，一个一个卫星挨个删除，再执行单点定位，后验残差最小的作为定位结果。
这个方法可以除去粗差较大卫星的影响。缺点是只能检测单个不良卫星
2.12 valsol( )检验定位结果准确
static int valsol(const double *azel, const int *vsat, int n,
const prcopt_t *opt, const double *v, int nv, int nx,
char *msg)
{
double azels[MAXOBS*2],dop[4],vv;
int i,ns;
trace(3,"valsol : n=%d nv=%d\n",n,nv);
/* chi-square validation of residuals 卡方检验 不懂！不懂！*/
vv=dot(v,v,nv);
if (nv>nx&&vv>chisqr[nv-nx-1]) {
sprintf(msg,"chi-square error nv=%d vv=%.1f cs=%.1f",nv,vv,chisqr[nv-nx-1]);
return 0;
}
/* large gdop check */
for (i=ns=0;i<n;i++) {
if (!vsat[i]) continue;
azels[ ns*2]=azel[ i*2];
azels[1+ns*2]=azel[1+i*2];
ns++;
}
//传进去高度角和方位角是为了重新计算设计矩阵
dops(ns,azels,opt->elmin,dop);
//判断精度，dop[]是精度矩阵
if (dop[0]<=0.0||dop[0]>opt->maxgdop) {
sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]);
return 0;
}
return 1;
}3 估计接收机速度
3.1 estvel( )
思路和单点定位差不多，构建设计矩阵、残差阵。使用最小二乘逼近速度
/* estimate receiver velocity ------------------------------------------------*/
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)
{
double x[4]={0},dx[4],Q[16],*v,*H;
int i,j,nv;
trace(3,"estvel : n=%d\n",n);
v=mat(n,1); H=mat(4,n);
for (i=0;i<MAXITR;i++) {
/* doppler residuals */
//re--＞卫星速度 x--＞接收机速度，从0开始，和单点定位一样，逐渐逼近
if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,x,azel,vsat,v,H))<4) {
break;
}
/* least square estimation 最小二乘，参数估计速度变化 */
if (lsq(H,v,4,nv,dx,Q)) break;
for (j=0;j<4;j++) x[j]+=dx[j];
//达到一定精度时，填充到sol结构体中。
if (norm(dx,4)<1E-6) {
for (i=0;i<3;i++) sol->rr[i+3]=x[i];
//其实后面还有，就是xx,xy,yy等协方差
break;
}
}
free(v); free(H);
}3.2 resdop( )
由高度角方位角计算设计矩阵--方向余弦
/* doppler residuals ---------------------------------------------------------*/
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)
{
double lam,rate,pos[3],E[9],a[3],e[3],vs[3],cosel;
int i,j,nv=0;
trace(3,"resdop : n=%d\n",n);
ecef2pos(rr,pos); xyz2enu(pos,E);
for (i=0;i<n&&i<MAXOBS;i++) {
lam=nav->lam[obs[i].sat-1][0];
if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) {
continue;
}
/* line-of-sight vector in ecef */
cosel=cos(azel[1+i*2]);//高度角 cos
a[0]=sin(azel[i*2])*cosel;//OA*sin(方位角) A`A`` e
a[1]=cos(azel[i*2])*cosel;//n
a[2]=sin(azel[1+i*2]); //u
matmul("TN",3,1,3,1.0,E,a,0.0,e); //a是当地enu设计矩阵。enu转ecef的e设计矩阵
/* satellite velocity relative to receiver in ecef 卫星相对于接收机的速度 */
for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j];
/* range rate with earth rotation correction */
rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]-
rs[3+i*6]*rr[1]-rs[ i*6]*x[1]);
/* doppler residual 残差阵*/
v[nv]=-lam*obs[i].D[0]-(rate+x[3]-CLIGHT*dts[1+i*2]); //加负号 (v-vs)--＞-(vs-v)
/* design matrix 设计矩阵*/
for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0;
nv++;
}
return nv;
}
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
