gpsfuncs.cpp

开源GPS源码

			  (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

*******************************************************************************/
#ifdef VCPP
void  dops( int nsl)
{
  double r,xls,yls,zls;
//  double det;
  int i;
  Matrix H(nsl,4),G(4,4);

  receiver.north.x=-cos(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
  receiver.north.y=-sin(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
  receiver.north.z= cos(rec_pos_llh.lat);
  receiver.east.x=-sin(rec_pos_llh.lon);
  receiver.east.y= cos(rec_pos_llh.lon);
//receiver.east.z=0.0;
  receiver.up.x=cos(rec_pos_llh.lon)*cos(rec_pos_llh.lat);
  receiver.up.y=sin(rec_pos_llh.lon)*cos(rec_pos_llh.lat);
  receiver.up.z=sin(rec_pos_llh.lat);
  for (i=1;i<=nsl;i++)
  {
//
//   Compute line of sight vectors
//
      xls=track_sat[i].x-rec_pos_xyz.x;
      yls=track_sat[i].y-rec_pos_xyz.y;
      zls=track_sat[i].z-rec_pos_xyz.z;

      r=sqrt(xls*xls+yls*yls+zls*zls);

      H(i,1)=(xls*receiver.north.x+yls*receiver.north.y+zls*receiver.north.z)/r;
      H(i,2)=(xls*receiver.east.x+yls*receiver.east.y)/r;
      H(i,3)=(xls*receiver.up.x+yls*receiver.up.y+zls*receiver.up.z)/r;
      H(i,4)=1.0;
  }

//  G=(H.transpose()*H).inverse();  //for Alberto's library
  G=(H.t()*H).i();                 // for Newmat library

  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));
//  gotoxy(1,24);
//  printf("->dops");
}
#endif

#ifdef BCPP
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;
//	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));
      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))/denom;// ddx=ndop^2
	 ddy=(a1*(k1*p1-l1*o1)+c1*(l1*m1-i1*p1)+d1*(i1*o1-k1*m1))/denom;// ddy=edop^2
	 ddz=(a1*(f1*p1-h1*n1)+b1*(h1*m1-e1*p1)+d1*(e1*n1-f1*m1))/denom;// ddz=vdop^2
	 ddt=(a1*(f1*k1-g1*j1)+b1*(g1*i1-e1*k1)+c1*(e1*j1-f1*i1))/denom;// ddt=tdop^2
    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);          // rss of ndop and edop
		 vdop=sqrt(ddz);
		 tdop=sqrt(ddt);
		 pdop=sqrt(ddx+ddy+ddz);      // rss of ndop, edop, and vdop
		 gdop=sqrt(ddx+ddy+ddz+ddt);  // rss of ndop, edop, vdop, and tdop
	 }
//	gotoxy(1,24);
//	printf("dops->");

}
#endif
#ifdef DJGPP
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;
//	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));
      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))/denom;// ddx=ndop^2
	 ddy=(a1*(k1*p1-l1*o1)+c1*(l1*m1-i1*p1)+d1*(i1*o1-k1*m1))/denom;// ddy=edop^2
	 ddz=(a1*(f1*p1-h1*n1)+b1*(h1*m1-e1*p1)+d1*(e1*n1-f1*m1))/denom;// ddz=vdop^2
	 ddt=(a1*(f1*k1-g1*j1)+b1*(g1*i1-e1*k1)+c1*(e1*j1-f1*i1))/denom;// ddt=tdop^2
    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);          // rss of ndop and edop
		 vdop=sqrt(ddz);
		 tdop=sqrt(ddt);
		 pdop=sqrt(ddx+ddy+ddz);      // rss of ndop, edop, and vdop
		 gdop=sqrt(ddx+ddy+ddz+ddt);  // rss of ndop, edop, vdop, and tdop
	 }
//	gotoxy(1,24);
//	printf("dops->");

}
#endif

/*******************************************************************************
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);
  chan[ch].Tropo=d_Trop;
//  if (el<(mask_angle-0.035))
//  {
//  		printf("ch=%d, el=%lf",ch,el*57.296);
//  		fprintf(kalm,"ch=%d, el=%lf",ch,el);
//  }
  //  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=141312.0-32768.0*phim-131072.0*phim*phim-65536.0*phim*phim*phim;
		amp=3.46e-8+7.45e-9*phim-1.19e-7*phim*phim+5.96e-8*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;
  chan[ch].Iono=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)
{
   char text[27];
     if ((in = fopen("ion_utc.dat", "rt")) == NULL)
     {
       printf("Cannot open ion_utc.dat file.\n");
     }
     else
     {
	handle=fileno(in);
	while (!feof(in))
	{
	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);