计算GDOP

#include <iostream>
#include <fstream>
#include "..\include\CPosition.h"
#include "..\include\Constant.h"
#include "..\include\Data.h"
#include<stdio.h>
#define MATHRES 1E-12
#define FOUR 4
typedf struct{

       int prn;
       XYZCoor pos;
}SVPosStruct;
int Maxsat;
int ReadSatPosFile(FILE*SVPosFile,SVPosStruct*SV);
int fun(int n);
void ComputeDOP2(XYZCoor Obs,XYZCoor SV[],DOPStruct*DOP);
void main()
{

  double a=6378137.0;
  double e2=0.00669342162297;
  double PAI=3.1415926535898;
  double P0=PAI/180.0;
  double N;
  XYZCoor ObsPos;
  int LatDeg,LonDeg,LatMin,LonMin;
  float LatSec,LonSec,H,B,L;
  FILE*SVPosFile,*SVDOPFile;
  int i,j,k,ri,num;
  int array[];
  int temp;
  XYZCoor SV[];
  int SVNo[];
  SVPosStruct*SVPos;
  DOPStruct DOP;
  prinf("Please Input local Lat(dd mm.mmmm):");
  scanf("%i %f",&LatDeg,&LatMin);
  B=((float)LatDeg+LatMin/60.0)*P0;
  prinf("Please Input local Lon(ddd mm.mmmm):");
  scanf("%i %f",&LonDeg,&LonMin);
  L=((float)LonDeg+LonMin/60.0)*P0;
  prinf("Please Input local Height(meter):");
  scanf("%f",&H);
  B=(LatDeg+LatMin/60.0+LatSec/3600.0)*P0;
  L=(LonDeg+LonMin/60.0+LonSec/3600.0)*P0;
  N=a/sqr(1.0-e2*sin(B)*sin(B));
  ObsPos.X=(N+H)*cos(B)*cos(L);
  ObsPos.Y=(N+H)*cos(B)*sin(L);
  ObsPos.Z=(N*(1.0-e2)+H)*sin(B);
  if((SVPosFile=fopen("satpos.out","r"))==NULL)
    {
prinf("cannot open input file\n");
     exit();

}
  if((SVDOPFile=fopen("satdop.out","w"))==NULL)
    {
prinf("cannot open output file\n");
     exit();

}
  if((SVPos=(SVPosStruct*)malloc(sizeof(SVPosStruct)*))==NULL)
    {
prinf("not enough memory to allocate buffer\n");
     exit();

}
  MaxSat=;
  i=;
  do
     {

      if(ReadSatPosFile(SVPosFile,(SVPos+i)))break;
      i++;
      if(i>=)break;

}while();
  if(MaxSat<)
    {
prinf("not enough satellite to compute DOP\n");
     exit();

}
  num=fun(MaxSat)/(fun(MaxSat-)*fun());
  fprinf(SVDOPFile,"SV Combinnation GDOP\n");
  ri=;
  array[]=MaxSat;
  do
     {

      if(ri！=FOUR)
      if((ri+array[ri])>FOUR)
      {
array[ri+]=array[ri]-;
       ri++:

}
      else
     {
ri--;array[ri]--;
}
      else
     {
for(j=;j<=FOUR;j++)
       {
SVNo[j-]=(SVPos+array[j]-)->prn;
        SV[j-].X=(SVPos+array[j]-)->pos.X
        SV[j-].Y=(SVPos+array[j]-)->pos.Y
        SV[j-].Z=(SVPos+array[j]-)->pos.Z;

}
      ComputeDOP2(ObsPos,SV,&DOP);
      fprinf(SVDOPFile,"%02d %02d-%02d-%02d %6.1f\n",SVNo[],SVNo[],SVNo[],SVNo[],DOP.GDOP);
      if(array[FOUR]--)
        {
ri--;array[ri]--;
}
      else
        {
array[ri]--;
}

}

}while(array[]!=FOUR-);
      fclose(SVPosFile);
      fclose(SVDOPFile);
      free(SVPos);
  int fun(int n)
    {
 int i;
      int res;
      if(n<)return0;
      res=;
      for(i=;i<=n;i++)res*=i;
      return res;

}
  int ReadSatPosFile(FILE*SVPosFile,SVPosStruct*SV)
    {
 char t1[],t2[],t3[];
      if(fscanf(SVPosFile,"%d%s%s%s\n",&SV->prn,t1,t2,t3))
      return ;
      SV->pos.X=atof(t1);
      SV->pos.Y=atof(t2);
      SV->pos.Z=atof(t3);
      MaxSat++;
      if(MaxSat>=)return ;
      return ;

}
  void ComputeDOP2(XYZCoor Obs.XYZCoor SV[],DOPStract*DOP)
    {
 double PAI=3.1415926535898;
      double P0=PAI/180.0;
      int Row=,Col=;
      int n=Row;
      double Qp[][];
      double Qpt[][];
      double QptXQp[][];
      double Q[][];
      int i,j,k,ii,jj;
      double Temp;
      double b,max,A[][];
      int *z;
      for(i=;i<Row;i++)
        {
Temp=sqrt((Obs.X-SV[i].X)*(Obs.X-SV[i].X)+(Obs.Y-SV[i].Y)*(Obs.X-SV[i].Y)+(Obs.Z-SV[i].Z)*(Obs.X-SV[i].Z));
         Qp[i][]=(SV[i].X-Obs.X)/Temp;
         Qp[i][]=(SV[i].Y-Obs.Y)/Temp;
         Qp[i][]=(SV[i].Z-Obs.Z)/Temp;
         Qp[i][]=1.0;

}
      for(i=;i<Row;i++)
      for(j=;j<Col;j++)
         Qpt[i][j]=Qpt[j][i];
      for(i=;i<Row;i++)
        {
for(j=;j<Col;j++)
           {
Temp=0.0;
            for(k=;k<;k++)Temp=Qpt[i][k]*Qp[k][j];
            QptXQp[i][j]=Temp;

}

}
      z=(int*)malloc(sizeof(int)**n);
      for(i=;i<Row;i++)
        {
for(j=;j<Col;j++)
         A[i][j]=QptXQp[j][i];

}
      for(k=;k<n;k++)
        {
max=;
         for(i=k;i<n;i++)
      for(j=k;j<n;j++)
        {
b=fabs(A[i][j]);
         if(max<b)
          {
ii=i;jj=j;max=b;

}

}
         if(max<MATHRES)
          {
free(z);
           prinf(The matrix isn't rank enough...");

}
         max=1.0/((A[ii][jj]));
         A[ii][jj]=;
         z[*k]=ii;
         A[*k+]=jj
         if(ii!=k）
          {
for(j=;j<n;j++)
            {
b=A[ii][j];A[ii][j]=A[k][j];A[k][j]=b;

}

}
         if(jj!=k）
          {
for(j=;j<n;j++)
            {
b=A[j][jj];A[jj][j]=A[j][k];A[j][k]=b;

}

}
         for(j=;j<n;j++)
           A[k][j]*=max;
         for(i=;i<n;i++)
          {
if(i!=k)
            {
max=A[i][k]; A[i][k]=0.0;
             for(j=;j<n;j++) A[i][j]=A[i][j]-max*A[k][j];

}

}
         for(k=n-;k>=;k--)
          {
ii=z[*k+];jj=[*k];
            if(ii!=k)
             {
for(j=;j<n;j++)
               {
b=A[ii][j];A[ii][j]=A[k][j];A[k][j]=b;

}

}
            if(jj!=k)
             {
for(j=;j<n;j++)
               {
b=A[j][ii];A[j][ii]=A[j][k];A[j][k]=b;

}

}

}
         for(i=;i<Row;i++)
          {
for(j=;j<Col;j++)
           Q[i][j]=A[i][j];

}
         free(z);
         Temp=0.0;for(i=;i<n;i++)Temp+=Q[i][i];DOP->GDOP=sqrt(Temp);

}