GICI学习笔记-RTK

添加测量值：

bool RtkEstimator::addGnssMeasurementAndState(
    const GnssMeasurement& measurement_rov, 
    const GnssMeasurement& measurement_ref)
{
  // Get prior states
  if (!spp_estimator_->addGnssMeasurementAndState(measurement_rov)) {
    return false;
  }
  if (!spp_estimator_->estimate()) {
    return false;
  }
  Eigen::Vector3d position_prior = spp_estimator_->getPositionEstimate();
  Eigen::Vector3d velocity_prior = spp_estimator_->getVelocityEstimate();
  curState().status = GnssSolutionStatus::Single;

  // Set to local measurement handle
  curGnssRov() = measurement_rov;
  curGnssRov().position = position_prior;
  curGnssRef() = measurement_ref;

  // Erase duplicated phases, arrange to one observation per phase
  gnss_common::rearrangePhasesAndCodes(curGnssRov());
  gnss_common::rearrangePhasesAndCodes(curGnssRef());

  // Form double difference pair
  // 按照最高高度角选取基准卫星，
  GnssMeasurementDDIndexPairs index_pairs = gnss_common::formPhaserangeDDPair(
    curGnssRov(), curGnssRef(), gnss_base_options_.common);

  // Cycle-slip detection
  if (!isFirstEpoch()) {
    cycleSlipDetectionSD(lastGnssRov(), lastGnssRef(), 
      curGnssRov(), curGnssRef(), gnss_base_options_.common);
  }
  
  // Add parameter blocks
  double timestamp = curGnssRov().timestamp;
  curState().timestamp = timestamp;
  // position block
  BackendId position_id = addGnssPositionParameterBlock(curGnssRov().id, position_prior);
  curState().id = position_id;
  curState().id_in_graph = position_id;
  // ambiguity blocks
  addSdAmbiguityParameterBlocks(curGnssRov(), 
    curGnssRef(), index_pairs, curGnssRov().id, curAmbiguityState());
  if (rtk_options_.estimate_velocity) {
    // velocity block
    addGnssVelocityParameterBlock(curGnssRov().id, velocity_prior);
    // frequency block
    int num_valid_doppler_system = 0;
    addFrequencyParameterBlocks(curGnssRov(), curGnssRov().id, num_valid_doppler_system);
  }
  
  // Add pseudorange residual blocks
  int num_valid_satellite = 0;
  addDdPseudorangeResidualBlocks(curGnssRov(), 
    curGnssRef(), index_pairs, curState(), num_valid_satellite);

  // Check if insufficient satellites
  if (!checkSufficientSatellite(num_valid_satellite, 0)) {
    // erase parameters in current state
    eraseGnssPositionParameterBlock(curState());
    eraseAmbiguityParameterBlocks(curAmbiguityState());
    if (rtk_options_.estimate_velocity) {
      eraseGnssVelocityParameterBlock(curState());
      eraseFrequencyParameterBlocks(curState());
    }

    return false;
  }
  num_satellites_ = num_valid_satellite;

  // Add phaserange residual blocks
  addDdPhaserangeResidualBlocks(curGnssRov(), curGnssRef(), index_pairs, curState());

  // Add doppler residual blocks
  if (rtk_options_.estimate_velocity) {
    addDopplerResidualBlocks(curGnssRov(), curState(), num_valid_satellite);
  }

  // Add relative errors
  if (!isFirstEpoch()) {
    if (!rtk_options_.estimate_velocity) {
      // position
      addRelativePositionResidualBlock(lastState(), curState());
    }
    else {
      // position and velocity
      addRelativePositionAndVelocityBlock(lastState(), curState());
      // frequency
      addRelativeFrequencyBlock(lastState(), curState());
    }
    // ambiguity
    addRelativeAmbiguityResidualBlock(
      lastGnssRov(), curGnssRov(), lastAmbiguityState(), curAmbiguityState());
  }

  // Compute DOP
  updateGdop(curGnssRov(), index_pairs);

  return true;
}

 

单差对：

// Form single difference pseudorange pair
GnssMeasurementSDIndexPairs formPseudorangeSDPair(
                            const GnssMeasurement& measurement_rov, 
                            const GnssMeasurement& measurement_ref,
                            const GnssCommonOptions& options)
{
    GnssMeasurementSDIndexPairs index_pairs;
    //循环流动站测量值，确定可用的measurement_rov

    ...

// 双循环，外层循环为measurement_rov，内层循环为measurement_ref，
  // Find valid matches in measurement_ref
  for (auto index : indexes_1) 
  {
    auto& satellite = measurement_rov.satellites.at(index.prn);
    auto observation = satellite.observations.at(index.code_type);
    for (auto& sat : measurement_ref.satellites) {
      auto& satellite_2 = sat.second;

        //寻找流动站和基站的共视卫星
      if (satellite_2.prn != satellite.prn) continue;

      if (!checkObservationValid(measurement_ref, 
          GnssMeasurementIndex(satellite.prn, index.code_type), 
          ObservationType::Pseudorange)) {
        continue;
      }
       // 找到共视卫星后存入SD对index_pairs
      index_pairs.push_back(GnssMeasurementSDIndexPair(
        index, GnssMeasurementIndex(satellite.prn, index.code_type)));
    }
  }
}

 

模糊度相关：

//rtk_estimator.cpp
bool RtkEstimator::estimate() 
{
   ...
 // solve
    AmbiguityResolution::Result ret = ambiguity_resolution_->solveRtk(
      curState().id, curAmbiguityState().ids, 
      ambiguity_covariance, gnss_measurement_pairs_.back());
   ...
}

//ambiguity_resolution_differential.cpp
AmbiguityResolution::Result AmbiguityResolution::solveRtk(
             const BackendId& epoch_id, 
             const std::vector<BackendId>& ambiguity_ids,
             const Eigen::MatrixXd& ambiguity_covariance,
             const std::pair<GnssMeasurement, GnssMeasurement>& measurements)
{
   ...
    int num_fixed = trySolveLanes(
      lane_groups_.at(i), min_percentage_fixation, use_rounding);
   ...
}

//ambiguity_resolution.cpp
// Try to solve ambiguity by all partial AR methods
int AmbiguityResolution::trySolveLanes(
  const std::vector<size_t>& indexes, 
  const double min_percentage_fixation,
  const bool use_rounding)
{
  ...
    num_fixed = solveLanes(indexes, min_percentage_fixation, 
      method, use_rounding, skip_full);
  ...
}

//ambiguity_resolution.cpp
// Solve ambiguities
int AmbiguityResolution::solveLanes(
  const std::vector<size_t>& indexes, 
  const double min_percentage_fixation,
  const DelMethod method,
  const bool use_rounding,
  const bool skip_full)
{
  ...
  if (sqrt(active_float_covariance(num_active - 1, num_active - 1)) < 0.25)
  if (solveAmbiguityLambda(active_float_ambiguities, 
       active_float_covariance, options_.ratio, fixed_ambiguities, ratio)) break;
  ...
}

//ambiguity_common.cpp
// Solve integer ambiguity by LAMBDA
bool solveAmbiguityLambda(const Eigen::VectorXd& float_ambiguities,
                          const Eigen::MatrixXd& covariance, 
                          const double ratio_threshold, 
                          Eigen::VectorXd& fixed_ambiguities,
                          double& ratio)
{
   ...
  // Lambda search
  lambda(float_ambiguities.size(), 2, float_ambiguities.data(), 
         covariance.data(), fixed_template.data(), residuals.data());
   ...
}

//rtlib lambda.c
extern int lambda(int n, int m, const double *a, const double *Q, double *F,
                  double *s)
{
    int info;
    double *L,*D,*Z,*z,*E;
    
    if (n<=0||m<=0) return -1;
    L=zeros(n,n); D=mat(n,1); Z=eye(n); z=mat(n,1); E=mat(n,m);
    
    /* LD factorization */
    if (!(info=LD(n,Q,L,D))) {
        
        /* lambda reduction */
        reduction(n,L,D,Z);
        matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */
        
        /* mlambda search */
        if (!(info=search(n,m,L,D,z,E,s))) {
            
            info=solve("T",Z,E,n,m,F); /* F=Z'\E */
        }
    }
    free(L); free(D); free(Z); free(z); free(E);
    return info;
}