gpsfuncs.cpp

此程序为GPS软件接收机的源码

		ms[1][i]=(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-x)/r;
		ms[2][i]=(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-y)/r;
		ms[3][i]=(track_sat[i].z-z)/r;
		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];   for completeness, the matrix is symmetric
		 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][1]=dd[1][2];
		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][1]=dd[1][3];
//		dd[3][2]=a1*(o1*h1-g1*p1)+e1*(c1*p1-o1*d1)+m1*(d1*g1-c1*h1);
		dd[3][2]=dd[2][3];
		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][1]=dd[1][4];
//		dd[4][2]=a1*(g1*l1-h1*k1)-e1*(c1*l1-d1*k1)+i1*(c1*h1-d1*g1);
		dd[4][2]=dd[2][4];
//		dd[4][3]=a1*(j1*h1-f1*l1)+e1*(b1*l1-d1*j1)+i1*(d1*f1-b1*h1);
		dd[4][3]=dd[3][4];
		dd[4][4]=a1*(f1*k1-g1*j1)+b1*(g1*i1-e1*k1)+c1*(e1*j1-f1*i1);

		if ( denom<=0.0 )
		{
		  result.x=1.0;      // something went wrong
		  result.y=1.0;      // set solution to near center of earth
		  result.z=1.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]);
//		 printf("mag=%lf\n",correct_mag);
		 }
  } while ( correct_mag > 0.01 && correct_mag < 1.e8 && nits < 10);
  if (out_debug)
  {
	 fprintf(stream," t=%20.10lf\n",t);
	 for (i=1;i<=nsl;i++)
	 {
		alpha=(t-dt[i])*omegae;// was dt[i]-t
		fprintf(stream,"%i,%20.10lf,%20.10lf,%20.10lf,%20.10lf\n",i,alpha,
				track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha),
				track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha),
				track_sat[i].z);
	 }
  }
  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;
  }
  for (i=1;i<=4;i++)
  {
	  br[i]=0.0;
	  for (j=1;j<=nsl;j++)br[i]+=bm[j]*pm[i][j];
  }
  result.xv=br[1]+y*omegae;    //  get rid of earth
  result.yv=br[2]-x*omegae;    //  rotation rate
  result.zv=br[3];
  result.df=br[4]/c*1000000.0; // frequency error in parts per million (ppm)
//	gotoxy(1,24);
//	printf("pos_vel_time->");
  return(result);
}

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

RETURNS  None.

PARAMETERS
			nsl  int

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 (rss of ndop & edop)
    vdop = vertical dilution of precision
    tdop = time dilution of precision
	 pdop = position dilution of precision (rss of vdop & hdop)
    gdop = geometric dilution of precision (rss of pdop & tdop)

WRITTEN BY
	Clifford Kelley

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

void  dops( int nsl)
{
  double r,rxy,x,y,z;
  int i;
  Matrix  H(nsl,4), G(4,4);
  ecef north,east,up;
//	gotoxy(1,24);
//	printf("->dops");
  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));
	  H(i,1)=((track_sat[i].x-x)*north.x+(track_sat[i].y-y)*north.y+(track_sat[i].z-z)*north.z)/r;
	  H(i,2)=((track_sat[i].x-x)*east.x+(track_sat[i].y-y)*east.y)/r;
	  H(i,3)=((track_sat[i].x-x)*up.x+(track_sat[i].y-y)*up.y+(track_sat[i].z-z)*up.z)/r;
	  H(i,4)=1.0;
  }

//	gotoxy(1,24);
//	printf("dops->");
	 G=(H.transpose()*H).inverse();

	 hdop=sqrt(G(1,1)+G(2,2));
	 vdop=sqrt(G(3,3));
	 tdop=sqrt(G(4,4));
	 pdop=sqrt(G(1,1)+G(2,2)+G(3,3));
	 gdop=sqrt(G(1,1)+G(2,2)+G(3,3)+G(4,4));
}

/*******************************************************************************
FUNCTION tropo_iono(float az, float el, double gps_time)
RETURNS  signal time delay due to troposphere and ionosphere (single frequency)

PARAMETERS
			az         float
			el         float
			gps_time   double

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

WRITTEN BY
	Clifford Kelley

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

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

  //  Try a simple troposphere model
//	gotoxy(1,24);
//	printf("->tropo_iono");
  if (current_loc.hae>200000.0) alt_factor=0.0;
  else if (current_loc.hae<0.0) alt_factor=1.0;
  else alt_factor=exp(-current_loc.hae*1.33e-4);
  if (m_tropo==1) d_Trop=2.47/(sin(el)+.0121)*alt_factor/c;
  else d_Trop=0.0;
//  if (out_kalman==1) fprintf(kalm,",az= %12.6f, el=%12.6f, tropo= %12.10lf",
//                             az,el,d_Trop);
  tropo[ch]=d_Trop;
  if (d_Trop<0.0) printf("el=%lf, hae=%lf",el,current_loc.hae);
  //  Use an ionosphere model

  if (m_iono==1)
  {
	 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=43200.0*lambdai+gps_time-int((43200.0*lambdai+gps_time)/86400.)*86400.;
	 if (t<0.0) t=t+86400.0;
	 phim=phi+0.064*cos((lambdai-1.617)*pi);
//
//  If available from the nav message use its Ionosphere model
//
	 if (b0 != 0.0 && al0 != 0.0)
	 {
		per=b0+b1*phim+b2*phim*phim+b3*phim*phim*phim;
		amp=al0+al1*phim+al2*phim*phim+al3*phim*phim*phim;
	 }
//
// else try this set of default iono model parameters
//
	 else
	 {
		per=b0+b1*phim+b2*phim*phim+b3*phim*phim*phim;
		amp=al0+al1*phim+al2*phim*phim+al3*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);
	 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 d_Ion=0.0;
  iono[ch]=d_Ion;
//	gotoxy(1,24);
//	printf("tropo_iono->");
  return(d_Trop+d_Ion);
}

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

PARAMETERS None.

PURPOSE
	This function reads the broadcast ionospheric correction model and the
	gps time to UTC conversion parameters from "ion_utc.dat" which is
	created by the program when the data has been read from the satellites

WRITTEN BY
	Clifford Kelley

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

void  read_ion_utc(void)
{
     if ((in = fopen("ion_utc.dat", "rt")) == NULL)
     {
       printf("Cannot open ion_utc.dat file.\n");
     }
     else
     {
	handle=fileno(in);
	while (!eof(handle))
	{
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&al0);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&al1);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&al2);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&al3);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&b0);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&b1);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&b2);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&b3);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&a0);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&a1);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&dtls);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&tot);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&WNt);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&WNlsf);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&DN);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&dtlsf);
	}
     }
     fclose(in);
}

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

PARAMETERS None.

PURPOSE
	This function reads the almanac parameters from "current.alm" which is
	created by the program when the data has been read from the satellites

WRITTEN BY
	Clifford Kelley

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

void read_almanac(void)
{
  int   id,health,week;
  float eccen,rinc,rras,sqra;
  float ratoa,aopg,rma,af0,af1,toa;

  if ((in = fopen("current.alm", "rt")) == NULL)
  {
	  printf("Cannot open currrent.rcv file.\n");
	  for (id=1;id<=32;id++)
	  {
		  gps_alm[id].week=gps_week-1;
		  gps_alm[id].inc=1.0;
	  }
  }
  else
  {
	  status=warm_start;
	  handle=fileno(in);
	  while (!eof(handle))
	  {
		  fscanf(in,"%45c",&header);
		  fscanf(in,"%27c",&text);
	 fscanf(in,"%d",&id);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%i",&health);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&eccen);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&toa);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&rinc);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&rras);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&sqra);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&ratoa);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&aopg);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&rma);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&af0);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%f",&af1);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%i",&week);