gpsfuncs.c

C写的用软件无线电实现的GPS模拟程序

   f1=0.;g1=0.;h1=0.;
//   i1=0.;j1=0.;
   k1=0.;l1=0.;
//   m1=0.;n1=0.;o1=0.;p1=0.;
//   m1=0.;n1=0.;
   p1=0.;
   for (k=1;k<=nsl;k++)
   {
     a1+=ms[1][k]*ms[1][k];
     b1+=ms[1][k]*ms[2][k];
     c1+=ms[1][k]*ms[3][k];
     d1+=ms[1][k]*ms[4][k];
//       e1+=ms[2][k]*ms[1][k];
     f1+=ms[2][k]*ms[2][k];
     g1+=ms[2][k]*ms[3][k];
     h1+=ms[2][k]*ms[4][k];
//       i1+=ms[3][k]*ms[1][k];
//       j1+=ms[3][k]*ms[2][k];
     k1+=ms[3][k]*ms[3][k];
     l1+=ms[3][k]*ms[4][k];
//       m1+=ms[1][k];
//       n1+=ms[2][k];
//       o1+=ms[3][k];
     p1+=ms[4][k];
    }
    o1=l1; m1=d1; n1=h1; e1=b1; i1=c1; j1=g1;
/*
    SOLVE FOR THE MATRIX INVERSE
*/
    denom = (k1*p1-l1*o1) * (a1*f1-b1*e1) + (l1*n1-j1*p1) * (a1*g1-c1*e1) + 
            (j1*o1-k1*n1) * (a1*h1-d1*e1) + (l1*m1-i1*p1) * (c1*f1-b1*g1) + 
            (i1*o1-k1*m1) * (d1*f1-b1*h1) + (i1*n1-j1*m1) * (c1*h1-d1*g1);
    dd[1][1] = f1*(k1*p1-l1*o1)+g1*(l1*n1-j1*p1)+h1*(j1*o1-k1*n1);
    dd[1][2] = e1*(l1*o1-k1*p1)+g1*(i1*p1-l1*m1)+h1*(k1*m1-i1*o1);
    dd[1][3] = e1*(j1*p1-n1*l1)-i1*(f1*p1-n1*h1)+m1*(f1*l1-j1*h1);
    dd[1][4] = e1*(n1*k1-j1*o1)+i1*(f1*o1-n1*g1)+m1*(j1*g1-f1*k1);
    dd[2][1] = b1*(l1*o1-k1*p1)+j1*(c1*p1-d1*o1)+n1*(d1*k1-c1*l1);
    dd[2][2] = a1*(k1*p1-l1*o1)+c1*(l1*m1-i1*p1)+d1*(i1*o1-k1*m1);
    dd[2][3] = a1*(l1*n1-j1*p1)+i1*(b1*p1-n1*d1)+m1*(j1*d1-b1*l1);
    dd[2][4] = a1*(j1*o1-n1*k1)-i1*(b1*o1-n1*c1)+m1*(b1*k1-c1*j1);
    dd[3][1] = b1*(g1*p1-h1*o1)-f1*(c1*p1-o1*d1)+n1*(c1*h1-d1*g1);
    dd[3][2] = a1*(o1*h1-g1*p1)+e1*(c1*p1-o1*d1)+m1*(d1*g1-c1*h1);
    dd[3][3] = a1*(f1*p1-h1*n1)+b1*(h1*m1-e1*p1)+d1*(e1*n1-f1*m1);
    dd[3][4] = a1*(n1*g1-f1*o1)+e1*(b1*o1-c1*n1)+m1*(c1*f1-b1*g1);
    dd[4][1] = b1*(h1*k1-g1*l1)+f1*(c1*l1-d1*k1)+j1*(d1*g1-c1*h1);
    dd[4][2] = a1*(g1*l1-h1*k1)-e1*(c1*l1-d1*k1)+i1*(c1*h1-d1*g1);
    dd[4][3] = a1*(j1*h1-f1*l1)+e1*(b1*l1-d1*j1)+i1*(d1*f1-b1*h1);
    dd[4][4] = a1*(f1*k1-g1*j1)+b1*(g1*i1-e1*k1)+c1*(e1*j1-f1*i1);

    if ( denom <= 0.0 )
    {
      result.x=0.0;      // something went wrong
      result.y=0.0;      // set solution to center of earth
      result.z=0.0;
      result.dt=0.0;
    }
    else
    {
      for (i=1; i<=4; i++)
      {
        for ( j=1; j<=4; j++) 
          dd[i][j] = dd[i][j]/denom;
      }
      for (i=1; i<=nsl; i++)
      {
        for (j=1; j<=4; j++)
        {
          pm[j][i] = 0.0;
          for ( k=1; k<=4; k++)
            pm[j][i] += dd[j][k]*ms[k][i];
        }
      }
      for (i=1;i<=4;i++)
      {
        br[i]=0.0;
        for ( j=1; j<=nsl; j++)
          br[i] += bm[j]*pm[i][j];
      }
      nits++;
      x = x+br[1];
      y = y+br[2];
      z = z+br[3];
      t = t-br[4]/c;
//   printf("%lf,%lf,%lf,%20.10lf\n",x,y,z,t);
//   printf("%lf,%lf,%lf,%lf\n",br[1],br[2],br[3],br[4]);
      correct_mag=sqrt(br[1]*br[1]+br[2]*br[2]+br[3]*br[3]);
    }
  } while ( correct_mag > 0.01 && correct_mag < 1.e8 && nits < 10);
  result.x  = x;
  result.y  = y;
  result.z  = z;
  result.dt = t;

//
//  Now for Velocity
//

  for (i=1;i<=nsl;i++)
  {
    alpha = (dt[i]-t)*omegae;
    r = sqrt( pow( track_sat[i].x * cos(alpha) - track_sat[i].y * sin(alpha)-x, 2) +
              pow( track_sat[i].y * cos(alpha) + track_sat[i].x * sin(alpha)-y, 2) +
              pow( track_sat[i].z-z, 2));
    bm[i] = ((track_sat[i].x * cos(alpha) - track_sat[i].y * sin(alpha)-x) * d_sat[i].x +
             (track_sat[i].y * cos(alpha) + track_sat[i].x * sin(alpha)-y) * d_sat[i].y +
             (track_sat[i].z-z) * d_sat[i].z)/r - meas_dop[i] * lambda;
//    fprintf(out," %d meas_dop= %f bm= %f\n",i,meas_dop[i],bm[i]);
  }
  for (i=1; i<=4; i++)
  {
    br[i]=0.0;
    for (j=1;j<=nsl;j++)
      br[i] += bm[j]*pm[i][j];
  }
//  fprintf(out," %f  %f  %f  %f\n",pm[1][1],pm[1][2],pm[1][3],pm[1][4]);
//  fprintf(out," %f  %f  %f  %f\n",pm[2][1],pm[2][2],pm[2][3],pm[2][4]);
//  fprintf(out," %f  %f  %f  %f\n",pm[3][1],pm[3][2],pm[3][3],pm[3][4]);
//  fprintf(out," %f  %f  %f  %f\n",pm[4][1],pm[4][2],pm[4][3],pm[4][4]);
  result.xv = br[1] + y*omegae;    //  get rid of earth
  result.yv = br[2] - x*omegae;    //  rotation rate
  result.zv = br[3];
//  fprintf(out,"vel  x=%f,y=%f,z=%f\n",result.xv,result.yv,result.zv);
  result.df = br[4] / c * 1000000.0;
  return (result);
}

/*******************************************************************************
FUNCTION dops( int nsl)

RETURNS  None.

PARAMETERS None.

PURPOSE

  This routine computes the dops

INPUTS:
    sat_location[nsl][3] Array of satellite locations in ECEF when the signal was sent
    nsl      number of satellites used
    receiver position

OUTPUTS:
    hdop = horizontal dilution of precision
    vdop = vertical dilution of precision
    tdop = time dilution of precision
    pdop = position dilution of precision
    gdop = geometric dilution of precision (rss of pdop & tdop)

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

void  dops( int nsl)
{
  double ddx,ddy,ddz,ddt,r,rxy,ms[5][13],x,y,z;
  double a1,b1,c1,d1,e1,f1,g1,h1,i1,j1,k1,l1,m1,n1,o1,p1,denom;
  double msx,msy,msz;
  int i,k;
  ECEF north,east,up;

  x   = rec_pos_xyz.x;
  y   = rec_pos_xyz.y;
  z   = rec_pos_xyz.z;
  r   = sqrt(x*x+y*y+z*z);
  rxy = sqrt(x*x+y*y);

  north.x = -x/rxy*z/r;
  north.y = -y/rxy*z/r;
  north.z =  rxy/r;
  east.x  = -y/rxy;
  east.y  =  x/rxy;
//east.z=0.0; just for completeness
  up.x    = x/r;
  up.y    = y/r;
  up.z    = z/r;

  for (i=1;i<=nsl;i++)
  {
//
//   Compute line of sight vectors
//
    r=sqrt(pow(track_sat[i].x-x,2)+
    pow(track_sat[i].y-y,2)+
    pow(track_sat[i].z-z,2));
    ms[1][i]=(track_sat[i].x-x)/r;
    ms[2][i]=(track_sat[i].y-y)/r;
    ms[3][i]=(track_sat[i].z-z)/r;
    ms[4][i]=1.0;
  }
  for (i=1;i<=nsl;i++)
  {
    msx=ms[1][i]*north.x+ms[2][i]*north.y+ms[3][i]*north.z;
    msy=ms[1][i]*east.x+ms[2][i]*east.y;
    msz=ms[1][i]*up.x+ms[2][i]*up.y+ms[3][i]*up.z;
    ms[1][i]=msx;ms[2][i]=msy;ms[3][i]=msz;
    ms[4][i]=1.0;
  }
  a1=0.;b1=0.;c1=0.;d1=0.;
//  e1=0.;
  f1=0.;g1=0.;h1=0.;
//  i1=0.;j1=0.;
  k1=0.;l1=0.;
//  m1=0.;
//  n1=0.;
//  o1=0.;
  p1=0.;

  for (k=1; k<=nsl; k++)
  {
    a1+=ms[1][k]*ms[1][k];
    b1+=ms[1][k]*ms[2][k];
    c1+=ms[1][k]*ms[3][k];
    d1+=ms[1][k]*ms[4][k];
//   e1+=ms[2][k]*ms[1][k];
    f1+=ms[2][k]*ms[2][k];
    g1+=ms[2][k]*ms[3][k];
    h1+=ms[2][k]*ms[4][k];
//   i1+=ms[3][k]*ms[1][k];
//   j1+=ms[3][k]*ms[2][k];
    k1+=ms[3][k]*ms[3][k];
    l1+=ms[3][k]*ms[4][k];
//   m1+=ms[1][k];
//   n1+=ms[2][k];
//   o1+=ms[3][k];
    p1+=ms[4][k];
  }
  o1=l1; m1=d1; n1=h1; e1=b1; i1=c1; j1=g1;
//
//     SOLVE FOR THE DIAGONALS OF THE MATRIX INVERSE
//
  denom=(k1*p1-l1*o1)*(a1*f1-b1*e1)+(l1*n1-j1*p1)*(a1*g1-c1*e1)+(j1*o1-k1*n1)*(a1*h1-d1*e1)
       +(l1*m1-i1*p1)*(c1*f1-b1*g1)+(i1*o1-k1*m1)*(d1*f1-b1*h1)+(i1*n1-j1*m1)*(c1*h1-d1*g1);
  ddx=f1*(k1*p1-l1*o1)+g1*(l1*n1-j1*p1)+h1*(j1*o1-k1*n1);
  ddy=a1*(k1*p1-l1*o1)+c1*(l1*m1-i1*p1)+d1*(i1*o1-k1*m1);
  ddz=a1*(f1*p1-h1*n1)+b1*(h1*m1-e1*p1)+d1*(e1*n1-f1*m1);
  ddt=a1*(f1*k1-g1*j1)+b1*(g1*i1-e1*k1)+c1*(e1*j1-f1*i1);
  if ( denom<=0.0 )
  {
    hdop=1.e6;      // something went wrong
    vdop=1.e6;      // set dops to a large number
    tdop=1.e6;
    pdop=1.e6;
    gdop=1.e6;
  }
  else
  {
    hdop=sqrt((ddx+ddy)/denom);
    vdop=sqrt(ddz/denom);
    tdop=sqrt(ddt/denom);
    pdop=sqrt(vdop*vdop+hdop*hdop);
    gdop=sqrt(pdop*pdop+tdop*tdop);
  }
//     return(void);
}

/*******************************************************************************
FUNCTION tropo_iono()
RETURNS  None.

PARAMETERS None.

PURPOSE
  This function corrects the pseudoranges with a tropospheric model
  and the broadcast ionospheric message corrections.

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

double tropo_iono( float az, float el, double gps_time)
{
  double d_Trop,d_Ion,psi,phi,lambdai,phim,per,x,F,amp,t;

//  Try a troposphere model
//  if (current_loc.hae>7010.) d_Trop=(4.75/sin(el+.01745))/c;
  d_Trop = 2.47/(sin(el)+.0121)*exp(-current_loc.hae*1.33e-4)/c;
//  If available from the nav message use its Ionosphere model
  if ( Iono.b0 != 0.0 && Iono.al0 != 0.0)
  {
    psi = 0.0137/(el/pi+0.11)-.022;
    phi = current_loc.lat/pi+psi*cos(az);
    if ( phi > 0.416) 
      phi = 0.416;
    else if (phi < -0.416 )
      phi = -0.416;
    lambdai = current_loc.lon/pi+psi*sin(az)/cos(phi*pi);
    t       = (int)(43200.*lambdai+gps_time)%86400L;
    if ( t < 0.0) 
      t = t + 86400.0;
    phim    = phi+0.064*cos((lambdai-1.617)*pi);
    per     = Iono.b0 + 
              Iono.b1*phim + 
              Iono.b2*phim*phim + 
              Iono.b3*phim*phim*phim;
    if ( per <72000.0 ) 
      per = 72000.0;
    x       = 2.*pi*(t-50400.)/per;
    F       = 1.0+16.0*pow(0.53-el/pi,3);
    amp     = Iono.al0 + 
              Iono.al1 * phim + 
              Iono.al2 * phim*phim + 
              Iono.al3 * phim*phim*phim;

    if ( amp < 0.0 ) 
      amp = 0.0;

    if (fabs(x) < 1.5707)  
      d_Ion=F*(5.0e-9+amp*(1.0-x*x/2.+x*x*x*x/24.0));
    else
      d_Ion=F*5.0e-9;
  }
// else try this
  else
  {
    d_Ion=16.33/sin(sqrt(el*el+0.1255))/c;
  }
//  fprintf(stream,"d_trop=%le,d_ion=%le,az=%f,el=%f\n",d_Trop,d_Ion,az,el);
  return (d_Trop + d_Ion);
}

/*******************************************************************************
FUNCTION read_ion_utc()
RETURNS  None.

PARAMETERS None.

PURPOSE
  This function 

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

void  read_ion_utc( void)
{
  char infile[] = "ion_utc.dat";
  char *tmpstr;
  FILE *in;

  tmpstr = conmalloc( strlen( OGSDataDir) + strlen( infile) + 1);
  strcpy( tmpstr, OGSDataDir);
  strcat( tmpstr, infile);

  if (( in = fopen( tmpstr, "rt")) == NULL)
  {
    printf( "error opening %s.\n", tmpstr);
    exit(-1);
  }
  else
  {
    while ( !feof( in))                // GB: replaced eof() by feof()
    {
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Iono.al0);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Iono.al1);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Iono.al2);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Iono.al3);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Iono.b0);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Iono.b1);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Iono.b2);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Iono.b3);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Utc.a0);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Utc.a1);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Utc.dtls);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Utc.tot);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Utc.WNt);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Utc.WNlsf);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Utc.DN);
      fscanf( in, "%27c", &text);
      fscanf( in, "%f", &Utc.dtlsf);
    }
  }
  fclose( in);
  if ( tmpstr)
    free( tmpstr);
  return;
}

/*******************************************************************************
FUNCTION read_almanac()
RETURNS  None.

PARAMETERS None.

PURPOSE
  This function 

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

void read_almanac( void)
{
  int   id, health, week;
  float eccen, rinc, rras, sqra;
  float ratoa, aopg, rma, af0, af1, toa;
  char infile[] = "current.alm";
  char *buf;
  FILE *in;