ad_core.c

国内外搜集的GPS程序代码

ad_core.c

double ls(fp,a,b,p,ltpl,n,m) 
FILE *fp;
double a[Nlow],b[N],p[N],ltpl;
long n,m;
{
	long i,j,k,ichol,igaussj;
        char *str;
	double sum,atest[Nlow];
/* 为控制而输出 */
/* 将向量 b to 放入p，矩阵 a to 放入atest */
        for (i=0;i<n;i++) p[i] = b[i];
        for (i=0;i<n*(n+3)/2;i++) atest[i] = a[i];
/* 矩阵求逆，解方程 */
        switch(Inv_n)
        { case 1: if (chol_inv(fp,a,b,n)==1)
           {  fprintf(fp,"Function chol_inv failed\n");
              ichol = 1;  }
          break; } 
/* 计算 m0 (表示和数)和精度向量 p */
        if (m == n) {k = m - 1; if (m==1) k=1;}
        else {if (m > n) k = m - n;
        else fprintf(fp,"Error by input m in ls\n");}
        if (ltpl <= 0.0) fprintf(fp,"Error occurred due to ltpl value in ls\n");
        for (sum=0.0,i=0;i<n;i++) sum += p[i]*b[i];
        sum = sqrt(fabs(ltpl - sum)/(double)k);
        for (i=0;i<n;i++) p[i] = sum*sqrt(a[i*(i+3)/2]);
/* 输出结果 */
        return(sum); }
int chol_inv(fp,a,b,n)
FILE *fp;
double a[Nlow],b[N];
int n;
 {	int i,j,k,ichol;
    char *str;
	double sum,atest[Nlow],p[N],a1[N][N],b1[N],x[N],atest1[Nlow];
/*为试验注解输入矩阵 */ 
        for (i=0;i<n;i++) 
         { for (j=0;j<=i;j++)  
           {  k=i*(i+1)/2+j;
              atest[k] = a[k]; }
           b1[i] = b[i]; }
/* 将矩阵下三角 a 二维存储 a1  */
        for (i=0;i<n;i++) 
         { for (j=0;j<=i;j++) 
           { a1[i][j] = a[i*(i+1)/2+j];
             a1[j][i] = a[i*(i+1)/2+j]; }
         }
/* 为检查打印输入， 分解和求解矩阵方程 */
	   for (i=0;i<n;i++) 
         { for (j=i;j<n;j++) 
           { for (sum=a1[i][j],k=i-1;k>=0;k--) sum -= a1[i][k]*a1[j][k];
	        if (i == j) 
             { if (sum <= 0.0) printf("Choldc failed\n");
		      p[i]=sqrt(sum); } 
              else a1[j][i]=sum/p[i]; } }
	   for (i=0;i<n;i++) 
         { for (sum=b1[i],k=i-1;k>=0;k--) sum -= a1[i][k]*x[k];
		  x[i]=sum/p[i];	}
	   for (i=n-1;i>=0;i--) 
         { for (sum=x[i],k=i+1;k<n;k++) sum -= a1[k][i]*x[k];
		  x[i]=sum/p[i];	}
/* 试验输出 ， 计算 L (下三角) 矩阵 */
	   for (i=0;i<n;i++) 
          { for (j=i;j<n;j++) 
             { for (sum=a[j*(j+1)/2+i],k=i-1;k>=0;k--) 
               sum -= a[i*(i+1)/2+k]*a[j*(j+1)/2+k];
	           if (i == j) 
                {	if (sum <= 0.0) 
                  {  fprintf(fp,"Function chol_inv failed");
                     ichol=1;
                     goto loop; }
		         p[i]=sqrt(sum); } 
               else a[j*(j+1)/2+i]=sum/p[i]; }}
        for (i=0;i<n;i++) a[i*(i+3)/2] = p[i];
/* 检查分解 */
        str = "Matrix of L*LT (in chol_inv)";
	   for (i=0;i<n;i++) 
         { for (j=0;j<=i;j++) {
	      for (sum=0.0,k=0;k<=j;k++) 
           sum += a[i*(i+1)/2+k]*a[j*(j+1)/2+k];
           atest1[i*(i+1)/2+j] = sum; }}
/* 为转换将 E 放入 atest1  */
        for (i=0;i<n;i++) 
         { for (j=0;j<=i;j++) 
           { k=i*(i+1)/2+j;
             atest1[k] = a[k];
             if(i==j) atest1[k] = a[k]; } }
/* 计算L矩阵的求逆矩阵 */
	    for (j=0;j<n;j++) 
          { p[j]=1.0/atest1[j*(j+3)/2]; 
            atest1[j*(j+3)/2] = p[j]; 
            for (k=j-1;k>=0;k--) atest1[j*(j+1)/2+k] *= p[j];
	       for (i=j+1;i<n;i++) 
             {for (k=0;k<j;k++) atest1[i*(i+1)/2+k] -= 
              atest1[j*(j+1)/2+k]*atest1[i*(i+1)/2+j];
              k=j;
              atest1[i*(i+1)/2+k] = -atest1[i*(i+1)/2+k]*p[j]; }}
/* 检查 Inv(L)*L */
/* 计算并打印L*LT的求逆矩阵 */
	    for (i=0;i<n;i++) 
          { for (j=0;j<=i;j++) 
             { for (sum=0.0,k=i;k<n;k++) 
               sum += atest1[k*(k+1)/2+i]*atest1[k*(k+1)/2+j];
               a[i*(i+1)/2+j] = sum; }}
/* 单位矩阵试验： E = Inv(a)*a ，下三角中相关矩阵，计算并打印结果 */
	    for (i=0;i<n;i++) 
          { for (sum=0.0,j=0;j<n;j++) 
             { if (i >=j ) sum += a[i*(i+1)/2+j]*b[j];
               else sum += a[j*(j+1)/2+i]*b[j]; }
           p[i] = sum;  }
/* 将结果放入 b 并返回 */
        for (i=0;i<n;i++) b[i]=p[i];
        loop: return(ichol);}