RTKLIB学习（2）

码伪距单点定位 ：estpos(pntpos.c)

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)

功能说明：

通过伪距实现绝对定位，计算出接收机的位置和钟差，顺带返回实现定位后每颗卫星的{方位角、仰角}、定位时有效性、定位后伪距残差。

参数说明：

/*------------------------------------------------------------
函数参数，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)
*********************************************************/

• 处理过程：

（1）将sol->rr的前 3项赋值给 x数组。

（2）开始迭代定位计算，首先调用rescode函数，计算在当前接收机位置和钟差值的情况下，定位方程的右端部分 v(nv*1)、几何矩阵 H(NX*nv)、此时所得的伪距残余的方差 var、所有观测卫星的 azel{方位角、仰角}、定位时有效性vsat、定位后伪距残差resp、参与定位的卫星个数 ns和方程个数 nv。
（3）确定方程组中方程的个数要大于未知数的个数。
（4）以伪距残余的标准差的倒数作为权重，对H和v分别左乘权重对角阵，得到加权之后的H和v。
（5）调用lsq函数，根据
得到当前 x的修改量和定位误差协方差矩阵中的权系数阵。
（6）将 5中求得的 x加入到当前x值中，得到更新之后的x值。
（7）如果 5中求得的修改量小于截断因子(目前是1e-4)，则将 6中得到的 x值作为最终的定位结果，对 sol的相应参数赋值，之后再调用valsol函数确认当前解是否符合要求（伪距残余小于某个 值和GDOP小于某个门限值）。否则，进行下一次循环。
（8） 如果超过了规定的循环次数，则输出发散信息后，返回 0。

rescode(pntpos.c)

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)

功能说明：

计算在当前接收机位置和钟差值的情况下，定位方程的右端部分v(nv*1)、几何矩阵 H(NX*nv)、此时所得的伪距残余的方差var、所有观测卫星的azel{方位角、仰角}、定位时有效性vsat、定位后伪距残差 resp、参与定位的卫星个数ns和方程个数nv。

参数说明：

/*------------------------------------------------------
*函数参数，17个
int      iter      I    迭代次数
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
double   *x        I   本次迭代开始之前的定位值
prcopt_t *opt      I   processing options
double   *v        O   定位方程的右端部分，伪距残余
double   *H        O   定位方程中的几何矩阵
double   *var      O   参与定位的伪距残余方差
double   *azel     O   对于当前定位值，每一颗观测卫星的 {方位角、高度角}
int      *vsat     O   每一颗观测卫星在当前定位时是否有效
double   *resp     O   每一颗观测卫星的伪距残余， (P-(r+c*dtr-c*dts+I+T))
int      *ns       O   参与定位的卫星的个数
返回类型：
int                O   定位方程组的方程个数

处理流程

(1) 将之前得到的定位解信息赋值给rr和dtr数组，以进行关于当前解的伪距残余的相关计算。
(2) 调用ecef2pos函数，将上一步中得到的位置信息由ECEF转化为大地坐标系。
(3) 将 vsat、azel和 resp数组置 0，因为在前后两次定位结果中，每颗卫星的上述信息都会发生变化。
(4) 调用 satsys函数，验证卫星编号是否合理及其所属的导航系统。
(5) 检测当前观测卫星是否和下一个相邻数据重复。是，则 i++后继续下一次循环；否，则进入下一步。
(6) 调用 geodist函数，计算卫星和当前接收机位置之间的几何距离和 receiver-to-satellite方向的单位向量。然后检验几何距离是否 >0。
(7) 调用 satazel函数，计算在接收机位置处的站心坐标系中卫星的方位角和仰角，检验仰角是否 截断值。
(8) 调用 prange函数，得到伪距值。
(9) 可以在处理选项中事先指定只选用哪些导航系统或卫星来进行定位，这是通过调用satexclude函数完成的。
(10)调用 ionocorr函数，计算电离层延时(m)。
(11)上一步中所得的电离层延时是建立在 L1信号上的，当使用其它频率信号时，依据所用信号频组中第一个频率的波长与 L1波长的关系，对上一步得到的电离层延时进行修正。
(12)调用 tropcorr函数，计算对流层延时(m)。
(13)由 计算出此时的伪距残余。
(14) 组装几何矩阵，前 3行为 6中计算得到的视线单位向量的反向，第 4行为 1，其它行为 0。
(15) 将参与定位的卫星的定位有效性标志设为 1，给当前卫星的伪距残余赋值，参与定位的卫星个数 ns加 1.
(16) 调用 varerr函数，计算此时的导航系统误差（可能会包括 IFLC选项时的电离层延时），然后累加计算用户测距误差(URE)。
(17) 为了防止亏秩，人为的添加了几组观测方程。

lsq.c

int lsq(const double *A, const double *y, 
int n, int m, double *x, double *Q)

参数说明

/* least square estimation y = A^T * x--------------
* 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)
*---------------------------------------------------------*/

调用函数

info=lsq(H,v,NX,nv,dx,Q)

处理流程

（1）首先计算右半部分
（2）再计算左半部分括号里面的值
（3）计算Q矩阵的逆Q^-1，但仍存储在Q中，最后再右乘Ay，得到x的值。