gps_nav.cpp

gps 软件接收机,用 c++ 语言实现

    
  //find Ttr_GPS
  //long double is used because of x86 FP registers having 80bits of precision
  //while in memory FP are stored with 64bits
  f64 lastDt = 0.0;
  f64 Ttr_GPS = inTtr_SV;
  f64 E_k = 0;
  for(;;)
  {
    f64 dt;
    calculateDeltaSVTimeAndEkFromGPSTime(Ttr_GPS,dt,E_k);

    Ttr_GPS = inTtr_SV - dt;

//debug    std::cerr 
//debug      << "Ttr_GPS Iter:"
//debug      << " " << lastDt - dt
//debug      << " " << static_cast<f64>(lastDt) - static_cast<f64>(dt)
//debug      << " " << static_cast<f64 volatile>(lastDt) - static_cast<f64 volatile>(dt)
//debug      << " " << Ttr_GPS
//debug      << "\n";
    if(std::fabs(lastDt - dt) == 0.0) break;
    lastDt = dt;
  }
  pos.t = Ttr_GPS;
//debug  std::cerr << "Ttr_GPS Done\n";
  
  //calculate Satelite position @ time Ttr_GPS
  f64 const t_k = limitTimeToValidRange(Ttr_GPS - t_oe_);

  //true anomaly
  f64 const cos_E_k = std::cos(E_k);
  f64 const sin_E_k = std::sin(E_k);
  f64 const v_k = std::atan2(
    std::sqrt(1.0 - e_ * e_) * sin_E_k,
    cos_E_k - e_);
  
  //argument of latitude
  f64 const phi_k = v_k + w_ * k_semi_circle_radians;

  //second harmonic perturbations
  f64 const sin_2phi_k = std::sin(2 * phi_k);
  f64 const cos_2phi_k = std::cos(2 * phi_k);
  f64 const delta_u_k = C_us_ * sin_2phi_k + C_uc_ * cos_2phi_k; //argument of latitude correction
  f64 const delta_r_k = C_rs_ * sin_2phi_k + C_rc_ * cos_2phi_k; //radius correction
  f64 const delta_i_k = C_is_ * sin_2phi_k + C_ic_ * cos_2phi_k; //inclination correction

  //corrected argument of latitude
  f64 const u_k = phi_k + delta_u_k;
  
  //corrected radius
  f64 const r_k = sqrtA_ * sqrtA_ * (1 - e_ * cos_E_k) + delta_r_k;
  
  //corrected inclination
  f64 const i_k = 
    i_0_ * k_semi_circle_radians + 
    delta_i_k + 
    IDOT_ * k_semi_circle_radians * t_k;

  //position in orbital plane
  f64 const xp = r_k * std::cos(u_k);
  f64 const yp = r_k * std::sin(u_k);

  //corrected longiture of ascending node
  f64 const omega_k = 
    omega_0_ * k_semi_circle_radians + 
    (dot_omega_ * k_semi_circle_radians - k_dot_omega_e) * t_k - 
    k_dot_omega_e * t_oe_;
  
  //convert to earth fixed coordinates
  f64 const cos_omega_k = std::cos(omega_k);
  f64 const sin_omega_k = std::sin(omega_k);
  f64 const yp_cos_i_k = yp * std::cos(i_k);
  
  pos.x = xp * cos_omega_k - yp_cos_i_k * sin_omega_k;
  pos.y = xp * sin_omega_k + yp_cos_i_k * cos_omega_k;
  pos.z = yp * std::sin(i_k);

  return pos;
}
//---------------------------------------------------------------------------
void
Ephemeris::preliminarySubFramesProcess()
{
  SubFrame1 const& psf1 = preliminarySF1_;
  SubFrame2 const& psf2 = preliminarySF2_;
  SubFrame3 const& psf3 = preliminarySF3_;
  
  //check if subframes 1 & 2 & 3 match
  if(((psf1.IODC_ & 0xff) == (psf2.sf2IODE_)) &&
     ((psf1.IODC_ & 0xff) == (psf3.sf3IODE_)))
  {
    static_cast<SubFrame1Data&>(*this) = psf1;
    static_cast<SubFrame2Data&>(*this) = psf2;
    static_cast<SubFrame3Data&>(*this) = psf3;
    ephemerisValid_ = true;
  }
}
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
//ChannelSetup::
//---------------------------------------------------------------------------
ChannelSetup::ChannelSetup(
  f64 const                inSampleRate,
  f64 const                inCarrierFreq,
  f64 const                inChipRate,
  u32 const                inPrnNumber)
: sampleRate_(inSampleRate)
, carrierFreq_(inCarrierFreq)
, chipRate_(inChipRate)
, prnNumber_(inPrnNumber)
{
}
//---------------------------------------------------------------------------
ChannelSetup::ChannelSetup(
  ChannelSetup const&      inOther)
{
  *this = inOther;
}
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
//Channel::
//---------------------------------------------------------------------------
Channel::Channel(
  f64 const                inSampleRate,
  f64 const                inCarrierFreq,
  f64 const                inChipRate,
  u32 const                inPrnNumber)
: signalTrack_(inSampleRate,inCarrierFreq,inChipRate,inPrnNumber)
, ephemeris_()
, lastSatTtr_(0.0)
{
}
//---------------------------------------------------------------------------
Channel::Channel(
  ChannelSetup const&      inSetup)
: signalTrack_(
   inSetup.sampleRate_,inSetup.carrierFreq_,
   inSetup.chipRate_,inSetup.prnNumber_)
, ephemeris_()
, lastSatTtr_()
{
}
//---------------------------------------------------------------------------
void
Channel::processData(
  f32 const*                inBegin,
  f32 const* const          inEnd,
  GPSCorrelatorMsgTrack::DoTrackDumpCallback const inCallback)
{
  //process block of data
  while(inBegin != inEnd)
  {
    GPSCorrelatorMsgTrack::DoTrackResult const result =
      signalTrack_.doTrack(inBegin,inEnd,inCallback);

    if(GPSCorrelatorMsgTrack::trMsgSubFrame == result)
      ephemeris_.processSubFrameRaw(signalTrack_.subFrameRaw_);
  }
}
//---------------------------------------------------------------------------
void
Channel::reset()
{  
  signalTrack_.reset();
  ephemeris_.reset();
  lastSatTtr_ = 0.0;
}
//---------------------------------------------------------------------------
void
Channel::reset(
  u32 const                inPrnNumber)
{
  signalTrack_.reset();
  ephemeris_.reset();
  lastSatTtr_ = 0.0;

  signalTrack_.prnNumberSet(inPrnNumber);
}
//---------------------------------------------------------------------------
void
Channel::start()
{
  signalTrack_.start();
}
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
void LeastSqFit(
  f64*                       ouDX,
  FourDPos&                  inPosition,
  std::vector<f64>&	         inSatX,
  std::vector<f64>&	         inSatY,
  std::vector<f64>&	         inSatZ,
  std::vector<f64>&	         inPRN)
{
  //solve interative least squares equations
  //dX = (H^T * H)^-1 * H^T * dP
  f64 HtH[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
  f64 HtdP[4] = {0,0,0,0};
  for(u32 i = 0; i < inSatX.size(); ++i)
  {
    f64 const dx = inPosition.x - inSatX[i];
    f64 const dy = inPosition.y - inSatY[i];
    f64 const dz = inPosition.z - inSatZ[i];
    f64 const r  = std::sqrt(dx*dx + dy*dy + dz*dz);
    f64 const dp = inPRN[i] * k_c - r;
    
    HtH[ 0] += dx * dx / (r * r);
    HtH[ 1] += dx * dy / (r * r);
    HtH[ 2] += dx * dz / (r * r);
    HtH[ 3] += dx / r;
    HtH[ 4] += dy * dx / (r * r);
    HtH[ 5] += dy * dy / (r * r);
    HtH[ 6] += dy * dz / (r * r);
    HtH[ 7] += dy / r;
    HtH[ 8] += dz * dx / (r * r);
    HtH[ 9] += dz * dy / (r * r);
    HtH[10] += dz * dz / (r * r);
    HtH[11] += dz / r;
    HtH[12] += dx / r;
    HtH[13] += dy / r;
    HtH[14] += dz / r;
    HtH[15] += 1;
    
    HtdP[0] += dp * dx / r;
    HtdP[1] += dp * dy / r;
    HtdP[2] += dp * dz / r;
    HtdP[3] += dp;
  }
  
  //HtHi = HtH^-1;
  f64 HtHi[16];
  
  Invert4x4Matrix(HtH,HtHi);
  
  for(u32 i = 0; i < 4; ++i) ouDX[i] = 0;
  
  for(u32 i = 0; i < 4; ++i)
  {
    ouDX[0] += HtHi[0*4 + i] * HtdP[i];
    ouDX[1] += HtHi[1*4 + i] * HtdP[i];
    ouDX[2] += HtHi[2*4 + i] * HtdP[i];
    ouDX[3] += HtHi[3*4 + i] * HtdP[i];
  }
}           
//---------------------------------------------------------------------------
bool findPosition(
  FourDPos&                  ioPosition,
  ChannelVector const&       inChannels)
{
  //find average Ttr(GPS)
  f64 satMaxTtx = 0.0;
  std::vector<u32> validSat;
  std::vector<FourDPos> satPos;

  //compute all valid satellite positions/times and average Ttx(GPS) time
  for(u32 i = 0; i < inChannels.size(); ++i)
    if(inChannels[i].ephemeris_.ephemerisValid())
    {
      validSat.push_back(i);
      satPos.push_back(
        inChannels[i].ephemeris_.findPos(inChannels[i].lastSatTtr_));
      satMaxTtx = std::max(satMaxTtx,satPos.back().t);
    }


  //some valid satelites  
  if(satPos.size() >= 4)
  {
    //add in average expected transit time
    satMaxTtx += k_satellite_orbital_radius_m / k_c; 
 
    //if user time is too far away from the estimated receive time
    //then reset user time to max satellite transmit time + estimated 
    //signal travel time
    if(std::fabs(satMaxTtx - ioPosition.t) > 1.0) ioPosition.t = satMaxTtx;
       
    std::vector<f64> satX(satPos.size());
    std::vector<f64> satY(satPos.size());
    std::vector<f64> satZ(satPos.size());
    std::vector<f64> PRN(satPos.size());
    for(;;)
    {
      for(u32 i = 0; i < satPos.size(); ++i)
      {
        f64 const x = satPos[i].x;
        f64 const y = satPos[i].y;
        f64 const z = satPos[i].z;
        f64 const t = satPos[i].t;

        //compute PRNs based on user and satellite times
        PRN[i] = ioPosition.t - t;
      
        //update PRNs for atmosphere 
        //Channel const& ch = inChannels[validSat[i]];
        //xxx

        //compute all ECI positions to userTime reference
        //section 20.3.3.4.3.3.2 GPS standard
        f64 const theta = k_dot_omega_e * (t - ioPosition.t);
        f64 const ct = std::cos(theta);
        f64 const st = std::sin(theta);
        satX[i] = x * ct - y * st;
        satY[i] = x * st + y * ct;
        satZ[i] = z;
      }
      
      f64 dX[4] = {0,0,0,0};
      LeastSqFit(dX,ioPosition,satX,satY,satZ,PRN);

      //update user position and time
      ioPosition.x += dX[0];
      ioPosition.y += dX[1];
      ioPosition.z += dX[2];
      ioPosition.t -= dX[3] / k_c;
      
      //if deltas small enough break out of loop
      f64 const posErr = std::sqrt(dX[0] * dX[0] + dX[1] * dX[1] + dX[2] * dX[2]);
      f64 const timeErr = dX[3] / k_c;  
      if((posErr < 1e-6) && (std::fabs(timeErr) < 1e-9)) break;
    }
    return true;    
  }
  else
  {
  	return false;
  }
}
//---------------------------------------------------------------------------