• RTKLIB中estpos()函数之——rescode()函数


    最小二乘原理:x=(v^T *v)^-1 * (v^T * H)

    x:即为接收机位置

    rescode()函数就是求出v和H,为下一步用最小二乘法求解接收机位置做准备

    先介绍rescode()函数形参变量名:

    obs:当前历元的观测数据         n:观测数            nav :导航数据          opt :选择设置           sol:结果

    rs:卫星的位置和速度(6*1)   vare:卫星的位置和速度协方差矩阵

    dts:卫星的钟差和钟漂              svh:卫星的健康标志(-1:表示此卫星不可用)         azel:卫星的方位角和高度角

    iter:迭代次数                            v:伪距残差         var:伪距残差的协方差矩阵             H:伪距观测方程线性化后未知数前面的系数阵

    :第一次计算,因为不知道rover的位置,所以位置赋的是0;

           此函数跟求载波残差的zdres()函数类似

     1 /* pseudorange residuals -----------------------------------------------------*/
     2 static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
     3                    const double *dts, const double *vare, const int *svh,
     4                    const nav_t *nav, const double *x, const prcopt_t *opt,
     5                    double *v, double *H, double *var, double *azel, int *vsat,
     6                    double *resp, int *ns)
     7 {
     8     double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P;
     9     int i,j,nv=0,sys,mask[4]={0};
    10     
    11     trace(3,"resprng : n=%d
    ",n);
    12     
    13     for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3];
    14     
    15     ecef2pos(rr,pos);
    16     
    17     for (i=*ns=0;i<n&&i<MAXOBS;i++) {
    18         vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;
    19         
    20         if (!(sys=satsys(obs[i].sat,NULL))) continue;
    21         
    22         /* reject duplicated observation data */ 
    23         if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) {
    24             trace(2,"duplicated observation data %s sat=%2d
    ",
    25                   time_str(obs[i].time,3),obs[i].sat);
    26             i++;
    27             continue;
    28         }
    29         /* geometric distance/azimuth/elevation angle */
    30         if ((r=geodist(rs+i*6,rr,e))<=0.0||                       /*  根据接收机位置和卫星位置计算出卫星距离接收机的距离储存在rs[0-2]   */
    31             satazel(pos,e,azel+i*2)<opt->elmin) continue;         /*  把计算出卫星的方位角和高度角储存在变量azel中   */       
    32         
    33         /* psudorange with code bias correction */                /*  对量测的伪距进行码间偏差改正   */
    34         if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue;        /*  疑问:为什么只用一个伪距   */
    35         
    36         /* excluded satellite? */
    37         if (satexclude(obs[i].sat,svh[i],opt)) continue;          /*  排除星历不可用(svh=-1)、配置文件中没包括的卫星系统的卫星;opt->exsats???  */
    38         
    39         /* ionospheric corrections */
    40         if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2,
    41                       iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) continue;
    42         
    43         /* tropospheric corrections */
    44         if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
    45                       iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {
    46             continue;
    47         }
    48         /* pseudorange residual */
    49         v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);                /*   最关键的一步,用前面计算的星地距离r和经改正后的伪距观测值P,做差求得v*/
    50         
    51         /* design matrix */
    52         for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0);   /*   根据伪距观测方程线性化,求线性化后的未知数前面的系数阵  */
    53         
    54         /* time system and receiver bias offset correction */
    55         if      (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;}
    56         else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;}
    57         else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}
    58         else mask[0]=1;
    59         
    60         vsat[i]=1; resp[i]=v[nv]; (*ns)++;
    61         
    62         /* error variance */
    63         var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
    64         
    65         trace(4,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f
    ",obs[i].sat,
    66               azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1]));
    67     }
    68     /* constraint to avoid rank-deficient */
    69     for (i=0;i<4;i++) {
    70         if (mask[i]) continue;
    71         v[nv]=0.0;
    72         for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;
    73         var[nv++]=0.01;
    74     }
    75     return nv;
    76 }
  • 相关阅读:
    必读:Spark与kafka010整合
    6 个开源的家庭自己主动化工具 | Linux 中国
    承上 DBlink 与 SCN | 新增视图找出外部 SCN 跳变
    NetBeans使用Consolas中文乱码的解决
    IDEA community + Gradle + Gretty 调试 servlet 应用 + war包部署到tomcat
    解决ubuntu下IntelliJ IDEA无法锁定到启动器的问题
    virtualbox安装android6.0并设置分辨率为1920x1080x32
    Ubuntu安装Sqlite报错:No module named 'ConfigParser'
    Linux常用指令笔记
    解决Warning Couldn't flush user prefs: java.util.prefs.BackingStoreException: Couldn't get file lock.
  • 原文地址:https://www.cnblogs.com/y-z-h/p/13864873.html
Copyright © 2020-2023  润新知