计算GDOP

#include <iostream>
#include <fstream>
#include "..includeCPosition.h"
#include "..includeConstant.h"
#include "..includeData.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[4],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[4];
  int temp;
  XYZCoor SV[4];
  int SVNo[4];
  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
");
     exit(1);
      
}
  if((SVDOPFile=fopen("satdop.out","w"))==NULL)
    {
prinf("cannot open output file
");
     exit(1);
      
}
  if((SVPos=(SVPosStruct*)malloc(sizeof(SVPosStruct)*12))==NULL)
    {
prinf("not enough memory to allocate buffer
");
     exit(1);
      
}
  MaxSat=0;
  i=0;
  do
     {
 
      if(ReadSatPosFile(SVPosFile,(SVPos+i)))break;
      i++;
      if(i>=12)break;
      
}while(1);
  if(MaxSat<4)
    {
prinf("not enough satellite to compute DOP
");
     exit(1);
     
}
  num=fun(MaxSat)/(fun(MaxSat-4)*fun(4));
  fprinf(SVDOPFile,"SV Combinnation GDOP
");
  ri=1;
  array[1]=MaxSat;
  do
     {
 
      if(ri！=FOUR)
      if((ri+array[ri])>FOUR)
      {
array[ri+1]=array[ri]-1;
       ri++:
       
}
      else
     {
ri--;array[ri]--;
}
      else
     {
for(j=1;j<=FOUR;j++)
       {
SVNo[j-1]=(SVPos+array[j]-1)->prn;
        SV[j-1].X=(SVPos+array[j]-1)->pos.X
        SV[j-1].Y=(SVPos+array[j]-1)->pos.Y
        SV[j-1].Z=(SVPos+array[j]-1)->pos.Z;
        
}
      ComputeDOP2(ObsPos,SV,&DOP);
      fprinf(SVDOPFile,"%02d %02d-%02d-%02d %6.1f
",SVNo[0],SVNo[1],SVNo[2],SVNo[3],DOP.GDOP);
      if(array[FOUR]--1)
        {
ri--;array[ri]--;
}
      else
        {
array[ri]--;
}
       
}
       
}while(array[1]!=FOUR-1);
      fclose(SVPosFile);
      fclose(SVDOPFile);
      free(SVPos); 
  int fun(int n)
    {
 int i;
      int res;
      if(n<0)return0;
      res=1;
      for(i=1;i<=n;i++)res*=i;
      return res;
     
}
  int ReadSatPosFile(FILE*SVPosFile,SVPosStruct*SV)
    {
 char t1[30],t2[30],t3[30];
      if(fscanf(SVPosFile,"%d%s%s%s
",&SV->prn,t1,t2,t3))
      return 1;
      SV->pos.X=atof(t1);
      SV->pos.Y=atof(t2);
      SV->pos.Z=atof(t3);
      MaxSat++;
      if(MaxSat>=12)return 1;
      return 0;
     
}
  void ComputeDOP2(XYZCoor Obs.XYZCoor SV[4],DOPStract*DOP)
    {
 double PAI=3.1415926535898;
      double P0=PAI/180.0;
      int Row=4,Col=4;
      int n=Row;
      double Qp[4][4];
      double Qpt[4][4];
      double QptXQp[4][4]; 
      double Q[4][4];
      int i,j,k,ii,jj;
      double Temp;
      double b,max,A[4][4];
      int *z;
      for(i=0;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][0]=(SV[i].X-Obs.X)/Temp;
         Qp[i][1]=(SV[i].Y-Obs.Y)/Temp;
         Qp[i][2]=(SV[i].Z-Obs.Z)/Temp;
         Qp[i][3]=1.0;
         
}
      for(i=0;i<Row;i++)
      for(j=0;j<Col;j++)
         Qpt[i][j]=Qpt[j][i];
      for(i=0;i<Row;i++)
        {
for(j=0;j<Col;j++)
           {
Temp=0.0;
            for(k=0;k<4;k++)Temp=Qpt[i][k]*Qp[k][j];
            QptXQp[i][j]=Temp;
            
}
          
}
      z=(int*)malloc(sizeof(int)*2*n);
      for(i=0;i<Row;i++)
        {
for(j=0;j<Col;j++)
         A[i][j]=QptXQp[j][i];
         
}
      for(k=0;k<n;k++)
        {
max=0;
         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]=1;
         z[2*k]=ii;
         A[2*k+1]=jj
         if(ii!=k）
          {
for(j=0;j<n;j++)
            {
b=A[ii][j];A[ii][j]=A[k][j];A[k][j]=b;
             
}
           
}
         if(jj!=k）
          {
for(j=0;j<n;j++)
            {
b=A[j][jj];A[jj][j]=A[j][k];A[j][k]=b;
 
             
}
           
}
         for(j=0;j<n;j++)
           A[k][j]*=max;
         for(i=0;i<n;i++) 
          {
if(i!=k)
            {
max=A[i][k]; A[i][k]=0.0;
             for(j=0;j<n;j++) A[i][j]=A[i][j]-max*A[k][j];
             
}
           
}
         for(k=n-2;k>=0;k--)
          {
ii=z[2*k+1];jj=[2*k];
            if(ii!=k)
             {
for(j=0;j<n;j++)
               {
b=A[ii][j];A[ii][j]=A[k][j];A[k][j]=b;
                
}
              
}
            if(jj!=k)
             {
for(j=0;j<n;j++)
               {
b=A[j][ii];A[j][ii]=A[j][k];A[j][k]=b;
                
}
              
}
           
}
         for(i=0;i<Row;i++)
          {
for(j=0;j<Col;j++)
           Q[i][j]=A[i][j];
           
}
         free(z);
         Temp=0.0;for(i=0;i<n;i++)Temp+=Q[i][i];DOP->GDOP=sqrt(Temp);
      
}