gici-open学习日记(7):GNSS图优化——RTK

news2024/12/25 12:19:53

gici-open学习日记——GNSS RTK图优化

  • 前言
  • 初始化RTK的调用
  • `rearrangePhasesAndCodes`
  • 双差构造`formPhaserangeDDPair`
  • 周跳探测`cycleSlipDetectionSD`
  • 添加参数块
    • 模糊度参数部分`addSdAmbiguityParameterBlocks`
    • 添加双差伪距残差`addDdPseudorangeResidualBlocks`
    • 添加双差相位残差`addDdPhaserangeResidualBlocks`
    • 添加多普勒观测残差`addDopplerResidualBlocks`
    • 其他一些参数块的添加
  • 伪距双差残差`PseudorangeErrorDD`
    • `EvaluateWithMinimalJacobians`
  • 相位双差残差`PseudorangeErrorDD`
    • `EvaluateWithMinimalJacobians`
  • 多普勒残差`DopplerError`
    • `EvaluateWithMinimalJacobians`
  • 添加相对残差
  • 更新DOP值`updateGdop`

前言

自己其实很久没有看这个框架了,包括后续代码好像也更新了不少,不过正好过一阵可能就要准备找工作了,这里就当简单把RTK的知识稍微看一下吧。如果和最新版本的代码有冲突的地方还请以最新版本为主

初始化RTK的调用

这个的调用流程和SPP是一样的,只不过相当于是在SPP调用的上一层,具体在GnssImuInitializer::addMeasurement函数里

bool GnssImuInitializer::addMeasurement(const EstimatorDataCluster& measurement)
{
    /* 这部分不关注的代码 */
    if (sub_gnss_estimator_->addMeasurement(*measurement.gnss)) {
    /* 这部分不关注的代码 */
}

事实上,在进行具体的RTK解算之前,需要进行SPP提供给RTK解算的初值,所以才会在RtkEstimator::addGnssMeasurementAndState函数的最前面先进行SPP观测的添加以及SPP的解算

关于RtkEstimator::addGnssMeasurementAndState这个函数,大概的流程在gici-open学习日记(5):runMeasurementAddin函数引申——初始化这篇文章的“GNSS/INS初始化补充1:RtkEstimator流程”这里梳理过,所以这里不再梳理具体的流程,重点关注具体的参数块添加和解算

rearrangePhasesAndCodes

我的理解是这个函数是删除重复的相位,让每个观测值只对应一个相位的函数。这里面的核心是把载波相位和期对应的伪距码对应起来,如果不对应的情况,就把伪距观测值根据不同的DCB对应到同一个基础下,我在原函数的基础上加了一点点注释,就不特别详细地展开记录了。

void rearrangePhasesAndCodes(GnssMeasurement& measurement, bool accept_coarse)
{
  CodeBiasPtr code_bias = measurement.code_bias;
  for (auto& sat : measurement.satellites) {
    std::string prn = sat.first;
    char system = prn[0];
    Satellite& satellite = sat.second;
    std::unordered_map<int, Observation>& observations = satellite.observations;
    std::unordered_map<int, Observation> arranged_observations;   // 重新排序的观测
    for (auto it = observations.begin(); it != observations.end(); it++) {
      std::pair<int, Observation> arranged_observation;
      int code = it->first;
      Observation observation = it->second;
      int phase_id = getPhaseID(system, code);
      int default_code = 0;
#define MAP(S, P, C) \
  if (system == S && phase_id == P) { default_code = C; }
  PHASE_CHANNEL_TO_DEFAULT_CODE;  // 为每个载波相位分配默认的码
#undef MAP
      bool can_add = true;
      // do not need to arrange
      if (default_code == code) arranged_observation = *it;
      // arrange to default code
      else {
        double bias = code_bias->getCodeBias(prn, code, accept_coarse);
        double default_bias = 
          code_bias->getCodeBias(prn, default_code, accept_coarse);
        // do not have code bias
        if (bias == 0.0 || default_code == 0.0) {
          // consider DCBs in the same frequency are zeros
          if (accept_coarse) {
            arranged_observation = std::make_pair(default_code, observation);
          } 
          // cannot arrange
          else {
            // pass
            can_add = false;
          }
        }
        // use code bias to arrange
        else {
          observation.pseudorange += bias - default_bias; // 相当于把伪距观测值也对齐到默认码下面
          arranged_observation = std::make_pair(default_code, observation);
        }
      }
      // add to observations
      if (can_add) {
        if (arranged_observations.find(default_code) == arranged_observations.end()) {
          arranged_observations.insert(arranged_observation);
        }
      }
    }
    satellite.observations = arranged_observations;
  }
}

双差构造formPhaserangeDDPair

双差的公式manual里有给出了,这里不再详细记录
在这里插入图片描述
formPhaserangeDDPair进行了观测值双差的构建,其调用的参数有三个

const GnssMeasurement& measurement_rov, 	// 流动站观测值
const GnssMeasurement& measurement_ref,		// 基准站观测值
const GnssCommonOptions& options			// GNSS基本的配置选项

函数先对接收机之间做了一次单差

  GnssMeasurementSDIndexPairs sd_pairs = formPhaserangeSDPair(
    measurement_rov, measurement_ref, options); // 先对接收机间做一个单差

接下来先定义了5个变量,不过有一个目前来看是没有用到的。代码的话就是针对每一个变量进行赋值

   /**
   * prn_to_number_phases 卫星和载波相位数对应起来
   * prn_to_index         每一个卫星prn所对应的在单差sd_pairs的索引
   * prn_to_phases        prn和载波相位对应起来
   * system_to_num_phases 卫星系统和载波相位的对应,每一种系统挑出载波观测最多的以可卫星      
   */
  std::map<char, int> system_to_num_phases;
  std::multimap<char, double> system_to_phases; // FIXME 没用到
  std::map<std::string, int> prn_to_number_phases;
  std::multimap<std::string, double> prn_to_phases;
  std::multimap<std::string, int> prn_to_indexes;

然后// Find base satellites for each system and phases,这里就是为形成双差挑选一颗基础卫星,操作和RTKLIB是一样的,也是找出卫星高度角最大的。

  std::map<char, std::string> system_to_base_prn;
  for (size_t i = 0; i < getGnssSystemList().size(); i++) {
    char system = getGnssSystemList()[i];

    double max_elevation = 0.0;
    for (size_t j = 0; j < sd_pairs.size(); j++) {
      if (sd_pairs[j].rov.prn[0] != system) continue;

      // we only select satellites with max phase number
      if (prn_to_number_phases.at(sd_pairs[j].rov.prn) != 
          system_to_num_phases.at(system)) continue;

      double elevation = satelliteElevation(  // 相对参考站的卫星高度角
        measurement_ref.getSat(sd_pairs[j].ref).sat_position, 
        measurement_ref.position);
      if (max_elevation < elevation) {
        system_to_base_prn[system] = sd_pairs[j].rov.prn;
        max_elevation = elevation;          // 找到最大的卫星高度角
      }
    }
  }

最后就是形成双差了,这里也是只是形成了双差的结构,但没有完成具体的双差计算

  GnssMeasurementDDIndexPairs dd_pairs;
  for (size_t i = 0; i < sd_pairs.size(); i++) {
    char system = sd_pairs[i].rov.prn[0]; 
    std::string prn = sd_pairs[i].rov.prn;
    std::string prn_base = system_to_base_prn.at(system);

    if (prn == prn_base) continue;  // 同一颗卫星无法形成双差

    for (auto it = prn_to_indexes.lower_bound(prn_base); 
         it != prn_to_indexes.upper_bound(prn_base); it++) {
      GnssMeasurementSDIndexPair& sd_pair_base = sd_pairs[it->second];
      int phase_id_base = getPhaseID(system, sd_pair_base.rov.code_type);
      int phase_id = getPhaseID(system, sd_pairs[i].rov.code_type);
      if (phase_id_base == phase_id) {
        dd_pairs.push_back(GnssMeasurementDDIndexPair(  // 形成了双差结构
          sd_pairs[i].rov, sd_pairs[i].ref, sd_pair_base.rov, sd_pair_base.ref));
        break;
      }
    }
  }

周跳探测cycleSlipDetectionSD

周跳的精确探测可以确保正确的模糊度参数化,包括整周模糊度的搜索准则,所以也是RTK解算的一个重要部分。

  1. GICI和RTKLIB一样,也是在一上来利用LLI对周跳进行探测。LLI(Loss of Lock Indicator)表示失锁标识符,和接收机板卡的性能有关系。但在实际GNSS处理中,有些板卡也会把不是周跳通过LLI探测成周跳,周跳的过探测对后边的固定率影响也比较大,所以这个选项是可以根据自己的需要进行修改的,不过那都是很特殊的需求了。
  cycleSlipDetectionLLI(measurement_rov_pre, measurement_rov_cur);
  cycleSlipDetectionLLI(measurement_ref_pre, measurement_ref_cur);

关于cycleSlipDetectionLLI函数,就是根据当前历元的LLI标志和上一历元LLI与当前历元LLI是否一致判断的。

void cycleSlipDetectionLLI(GnssMeasurement& measurement_pre, 
                           GnssMeasurement& measurement_cur)
{
  for (auto& sat : measurement_cur.satellites) {  // 先根据当前历元的LLI判断有没有周跳
    for (auto& obs : sat.second.observations) {
      Observation& observation = obs.second;
      if (observation.LLI & 1) {
        observation.slip = true;
        continue;
      }

      // detect slip by parity unknown flag transition in LLI
      uint8_t LLI_cur = observation.LLI;          // 再根据两个历元的LLI是否一样判断
      auto it_sat = measurement_pre.satellites.find(sat.first);
      if (it_sat == measurement_pre.satellites.end()) continue;
      auto it_obs = it_sat->second.observations.find(obs.first);
      if (it_obs == it_sat->second.observations.end()) continue;
      uint8_t LLI_pre = it_obs->second.LLI;
      if (((LLI_pre & 2) && !(LLI_cur & 2)) || (!(LLI_pre & 2) && (LLI_cur & 2))) {
        observation.slip = true;
#if LOG_CYCLE_SLIP
        LOG(INFO) << "Detected cycle slip by LLI at " << sat.second.prn << ".";
#endif
      }
    }
  }
}
  1. 接下来分别生成了上一历元与当前历元的单差观测值,这里不仅构造了GnssMeasurementSDIndexPairs单差类型的变量,还完成了具体单差观测值的计算。

  2. 然后根据GF组合进行了周跳探测

Under not disturbed ionospheric conditions, the geometry-free combination performs as a very precise and smooth test signal,
driven by the ionospheric refraction. Although, for instance, the jump produced by a simultaneous one-cycle slip in both signals is smaller in this combination than in the original signals (λ2-λ1 =5.4cm), it can provide reliable detection even for small jumps

在这里插入图片描述
代码也很简单,就是分别计算得到了上一历元和当前历元的GF值,然后作差和阈值比较,大于阈值就认为发生了周跳。具体的计算内容在

cycleSlipDetectionGF(measurement_sd_pre, measurement_sd_cur, options.gf_sd_slip_thres);
  1. 根据时间间隔判断是否周跳
    这个的核心就在于cycleSlipDetectionTimeGap中的一行代码
cycleSlipDetectionTimeGap(measurement_sd_pre, measurement_sd_cur, options.period * 1.5);

// cycleSlipDetectionTimeGap函数中
if (measurement_cur.timestamp - measurement_pre.timestamp < max_time_gap) {
    return;
  }

如果时间间隔大于设置的阈值,然后接下来又在代码中判断确认是单频率,就直接认为发生了周跳

  1. 最后就是周跳的汇总了,当流动站和参考站中有一个发生周跳的时候,就会认为当前的历元发生了周跳
  for (size_t i = 0; i < pairs_cur.size(); i++) {
    Observation& observation_sd = measurement_sd_cur.getObs(pairs_cur[i].rov);
    Observation& observation_rov = measurement_rov_cur.getObs(pairs_cur[i].rov);
    Observation& observation_ref = measurement_ref_cur.getObs(pairs_cur[i].ref);
    observation_rov.slip |= observation_sd.slip;
    observation_ref.slip |= observation_sd.slip;
  }

添加参数块

模糊度参数部分addSdAmbiguityParameterBlocks

  addSdAmbiguityParameterBlocks(curGnssRov(), 
    curGnssRef(), index_pairs, curGnssRov().id, curAmbiguityState());

前几个参数很好理解,就是当前的流动站、基准站、双差以及ID,最后一个参数得到的是模糊度状态容器ambiguity_states_(定义在gnss_estimator_base.h里)的最后一个变量,这里只是加入到参数块之中,具体的解算也在后面

  1. 函数上来先利用之前形成的双差关系得到了一些变量,在后边可能会用到
const Satellite& satellite = // 先利用双差得到了一些之后可能会用的变量
      measurement_rov.getSat(index_pair.rov);
// 。。。这里省略了一些变量
  1. 然后根据ID判断在图优化中是否已经存在了
    double code = index_pair.rov.code_type;
    double wavelength = observation.wavelength; // FIXME 没用到啊
    double phase_id = gnss_common::getPhaseID(system, code);
    BackendId ambiguity_id = createGnssAmbiguityId(satellite.prn, phase_id, id);
    CHECK(!graph_->parameterBlockExists(ambiguity_id.asInteger())) 
      << "Ambiguity parameter for " << satellite.prn << " in phase " 
      << phase_id << " has already been added!";
  1. 接下来用伪距和载波相位观测值给了个初值,其实原理就是用两个观测方程减一下就得到了
    在这里插入图片描述
  • TODO:事实上前面定义了波长的变量,按理说这里给初值用这个变量好一点,不过这里没有用到。我觉得用一下是不是比较好?
    double phaserange = observation.phaserange; // 这里利用伪距和载波观测给了个初值
    double pseudorange = observation.pseudorange;
    double phaserange_ref = observation_ref.phaserange;
    double pseudorange_ref = observation_ref.pseudorange;
    double ambiguity = phaserange - phaserange_ref -
                      (pseudorange - pseudorange_ref);
    // TODO ambiguity /= wavelength;
  1. 接下来就是构建模糊度参数并加到图中了
    Eigen::Matrix<double, 1, 1> init;
    init[0] = ambiguity;
    std::shared_ptr<AmbiguityParameterBlock> ambiguity_parameter_block = 
      std::make_shared<AmbiguityParameterBlock>(init, ambiguity_id.asInteger());
    CHECK(graph_->addParameterBlock(ambiguity_parameter_block));
    state.ids.push_back(ambiguity_id);
  1. 因为形成双差是需要一颗base_satellite的,所以接下来就是对这个卫星再进行一遍类似的操作
    BackendId ambiguity_base_id =   // 这里为作为基础的那颗卫星添加参数块
      createGnssAmbiguityId(satellite_base.prn, phase_id, id);
    if (!graph_->parameterBlockExists(ambiguity_base_id.asInteger())) 
    {
      phaserange = observation_base.phaserange;
      pseudorange = observation_base.pseudorange;
      phaserange_ref = observation_ref_base.phaserange;
      pseudorange_ref = observation_ref_base.pseudorange;
      ambiguity = phaserange - phaserange_ref - 
                        (pseudorange - pseudorange_ref);
      // TODO 同理,这里是不是也可以利用上波长信息作为初值?
      /**
       * wavelength = observation_base.wavelength;
       * ambiguity /= wavelength;
        */
      Eigen::Matrix<double, 1, 1> init_base;
      init_base[0] = ambiguity;
      std::shared_ptr<AmbiguityParameterBlock> ambiguity_base_parameter_block = 
        std::make_shared<AmbiguityParameterBlock>(
          init_base, ambiguity_base_id.asInteger());
      CHECK(graph_->addParameterBlock(ambiguity_base_parameter_block));
      state.ids.push_back(ambiguity_base_id);
  1. 最后添加模糊度的残差
      addAmbiguityResidualBlock(ambiguity_base_id, init_base[0], 	// 在构建误差的时候会执行setInformation的操作
        gnss_base_options_.error_parameter.initial_ambiguity);

添加双差伪距残差addDdPseudorangeResidualBlocks

这个函数也是进行了参数块的添加,关于参数块的核心计算步骤不在这个函数里,也是分为了ECEF和ENU两种框架下

// 这里用ENU框架下的代码举例子
      else {
        is_state_pose_ = true;  // ENU框架下
        BackendId pose_id = state.id_in_graph;
        std::shared_ptr<PseudorangeErrorDD<7, 3>> pseudorange_error = 
          std::make_shared<PseudorangeErrorDD<7, 3>>( // 主要是进行构建这个残差
          measurement_rov, measurement_ref, 
          index_pair.rov, index_pair.ref, index_pair.rov_base, index_pair.ref_base,
          gnss_base_options_.error_parameter); 
        pseudorange_error->setCoordinate(coordinate_);
        graph_->addResidualBlock(pseudorange_error, 
          huber_loss_function_ ? huber_loss_function_.get() : nullptr,
          graph_->parameterBlockPtr(pose_id.asInteger()), 
          graph_->parameterBlockPtr(gnss_extrinsics_id_.asInteger()));
      }

添加双差相位残差addDdPhaserangeResidualBlocks

这个和上一个函数没啥区别,不过是相位观测的话残差的维度会变多(模糊度),这里就不再重复记录了

  // Add phaserange residual blocks
  addDdPhaserangeResidualBlocks(curGnssRov(), curGnssRef(), index_pairs, curState()); // 相位双差残差

添加多普勒观测残差addDopplerResidualBlocks

这个其实和前两个也没什么不同,不过就是添加进去的残差信息换成多普勒观测量的了,关于多普勒的残差因子可以看manual的3.4.3节,这里代码也没有什么特别的东西,不再重复记录

  if (rtk_options_.estimate_velocity) {
    addDopplerResidualBlocks(curGnssRov(), curState(), num_valid_satellite);          // 多普勒残差
  }

其他一些参数块的添加

其实剩下添加的一些残差块都差不多,核心都在于残差构建的地方,那就不在具体的添加残差的函数里了,所以这里就不一一详细记录了,只是大概列一下都加了哪些

  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());
  }

伪距双差残差PseudorangeErrorDD

关于双差伪距残差的几种形式,注释里也给出了比较详细的解释

// Group 1: P1. receiver position in ECEF (3)
// Group 2: P1. body pose in ENU (7), P2. relative position from body to receiver
// in body frame (3)
// Group 3: Group 1 + P2. troposphere delay at rov (1), P3. troposphere delay at ref (1)
// P4. ionosphere delay (1), P5. ionosphere delay of base satellite (1)
// Group 4: Group 2 + P3. troposphere delay at rov (1), P4. troposphere delay at ref (1),
// P5. ionosphere delay (1), P6. ionosphere delay of base satellite (1)

构造函数也是判断了具体是哪种形式,以及调用setInfoemation设置了信息矩阵

EvaluateWithMinimalJacobians

残差和雅可比矩阵的计算在这个函数里

  1. 首先是得到了在ECEF下的坐标
  if (!is_estimate_body_)   // ECEF框架下
  {
    t_WR_ECEF = Eigen::Map<const Eigen::Vector3d>(parameters[0]);
  }
  else 
  {
    // pose in ENU frame
    t_WS_W = Eigen::Map<const Eigen::Vector3d>(&parameters[0][0]);
    q_WS = Eigen::Map<const Eigen::Quaterniond>(&parameters[0][3]);

    // relative position
    t_SR_S = Eigen::Map<const Eigen::Vector3d>(parameters[1]);

    // receiver position
    Eigen::Vector3d t_WR_W = t_WS_W + q_WS * t_SR_S;

    if (!coordinate_) {
      LOG(FATAL) << "Coordinate not set!";
    }
    if (!coordinate_->isZeroSetted()) {
      LOG(FATAL) << "Coordinate zero not set!";
    }
    t_WR_ECEF = coordinate_->convert(t_WR_W, GeoType::ENU, GeoType::ECEF);
  }
  1. 然后计算了卫星到接收机的距离,因为是双差所以也是分为base satellite和使用到的另一颗
  double rho_rov = gnss_common::satelliteToReceiverDistance(  // 接收机和卫星的距离
    satellite_rov_.sat_position, t_WR_ECEF);
  double rho_ref = gnss_common::satelliteToReceiverDistance(
    satellite_ref_.sat_position, measurement_ref_.position);
  double rho_rov_base = gnss_common::satelliteToReceiverDistance( // 接收机和base卫星的距离
    satellite_rov_base_.sat_position, t_WR_ECEF);
  double rho_ref_base = gnss_common::satelliteToReceiverDistance(
    satellite_ref_base_.sat_position, measurement_ref_.position);
  1. 然后和距离一样,计算对应的高度角
  double elevation_rov = gnss_common::satelliteElevation(
    satellite_rov_.sat_position, t_WR_ECEF);
  double elevation_ref = gnss_common::satelliteElevation(
    satellite_ref_.sat_position, measurement_ref_.position);
  double elevation_rov_base = gnss_common::satelliteElevation(
    satellite_rov_base_.sat_position, t_WR_ECEF);
  double elevation_ref_base = gnss_common::satelliteElevation(
    satellite_ref_base_.sat_position, measurement_ref_.position);
  1. 在短基线的情况下,认为大气延迟都被双差模型消除了,就不再计算大气相关的延迟量了;而如果选择估计大气延迟,就要计算对应的延迟量
  if (!is_estimate_atmosphere_) 
  {
    // We think all atmosphere delays are eliminated by double-difference
  }
  else
  { 
    // use estimated atomspheric delays
    double zwd_rov = 0.0, zwd_ref = 0.0;
    double dionosphere_delay_cur = 0.0, dionosphere_delay_base = 0.0;
    if (!is_estimate_body_) {
      zwd_rov = parameters[1][0];
      zwd_ref = parameters[2][0];
      dionosphere_delay_cur = parameters[3][0];
      dionosphere_delay_base = parameters[4][0];
    }
    else {
      zwd_rov = parameters[2][0];
      zwd_ref = parameters[3][0];
      dionosphere_delay_cur = parameters[4][0];
      dionosphere_delay_base = parameters[5][0];
    }

    // troposphere hydro-static delay
    double zhd_rov = gnss_common::troposphereSaastamoinen(
      timestamp, t_WR_ECEF, PI / 2.0);
    double zhd_ref = gnss_common::troposphereSaastamoinen(
      timestamp, t_WR_ECEF, PI / 2.0);
    double zhd_rov_base = gnss_common::troposphereSaastamoinen(
      timestamp, t_WR_ECEF, PI / 2.0);
    double zhd_ref_base = gnss_common::troposphereSaastamoinen(
      timestamp, t_WR_ECEF, PI / 2.0);
    // mapping
    gnss_common::troposphereGMF(  // 投影函数模型
      timestamp, t_WR_ECEF, elevation_rov, &gmf_hydro_rov, &gmf_wet_rov);
    gnss_common::troposphereGMF(
      timestamp, t_WR_ECEF, elevation_ref, &gmf_hydro_ref, &gmf_wet_ref);
    gnss_common::troposphereGMF(
      timestamp, t_WR_ECEF, elevation_rov_base, &gmf_hydro_rov_base, &gmf_wet_rov_base);
    gnss_common::troposphereGMF(
      timestamp, t_WR_ECEF, elevation_ref_base, &gmf_hydro_ref_base, &gmf_wet_ref_base);
    dtroposphere_delay = zhd_rov * gmf_hydro_rov - zhd_ref * gmf_hydro_ref - 
      zhd_rov_base * gmf_hydro_rov_base + zhd_ref_base * gmf_hydro_ref_base + 
      zwd_rov * gmf_wet_rov - zwd_ref * gmf_wet_ref - 
      zwd_rov * gmf_wet_rov_base + zwd_ref * gmf_wet_ref_base;

    // ionosphere 
    dionosphere_delay_cur = gnss_common::ionosphereConvertFromBase(
      dionosphere_delay_cur, observation_rov_.wavelength);
    dionosphere_delay_base = gnss_common::ionosphereConvertFromBase(
      dionosphere_delay_base, observation_rov_base_.wavelength);
    dionosphere_delay = dionosphere_delay_cur - dionosphere_delay_base;
  }
  1. 接下来就是计算残差以及赋权了
  double dpseudorange_estimate = rho_rov - rho_ref - rho_rov_base + rho_ref_base
 + dtroposphere_delay + dionosphere_delay;

  // Compute error
  double dpseudorange = observation_rov_.pseudorange - observation_ref_.pseudorange -
    observation_rov_base_.pseudorange + observation_ref_base_.pseudorange;
  Eigen::Matrix<double, 1, 1> error = 
    Eigen::Matrix<double, 1, 1>(dpseudorange - dpseudorange_estimate);
    
  Eigen::Map<Eigen::Matrix<double, 1, 1> > weighted_error(residuals);
  weighted_error = square_root_information_ * error;  // 利用在构造函数中得到的权重进行赋权的操作
  1. 计算雅可比矩阵,对应着manual的3.8.3节中的第一个残差,具体的雅可比计算在3.4.1节中的Formulation 11形式里,待估参数形式:
    在这里插入图片描述
  • 残差形式:

在这里插入图片描述
在这里插入图片描述

  • 雅可比的形式:
    在这里插入图片描述
  • 具体的J0以及J1:

在这里插入图片描述
在这里插入图片描述

相位双差残差PseudorangeErrorDD

相位双差主要比伪距残差多了一维模糊度的参数,但是具体形式的分类和伪距是一样的,也是根据是否估计大气延迟进行分类的。在具体的构造函数中,选择了残差的形式并利用setInfomation设置了信息矩阵以及对应的权重。

EvaluateWithMinimalJacobians

这里和伪距残差的部分也没什么不同,多的模糊度的部分雅可比对应到manual里的公式(3.80)又是-1和1
在这里插入图片描述

    Eigen::Matrix<double, 1, 1> J_amb = -Eigen::MatrixXd::Identity(1, 1); // 模糊度对应的雅可比部分
    Eigen::Matrix<double, 1, 1> J_amb_base = Eigen::MatrixXd::Identity(1, 1);

剩下的和伪距残差没有特别大的区别,对应3.8.3中的第二个残差,具体可以参考manual的3.4.2小节,这里就不再详细记录了

多普勒残差DopplerError

如果需要对速度进行估计的情况下就会用到多普勒观测量,这个时候也就需要计算对应的多普勒残差。多普勒的残差只根据框架分为了ECEF框架下和ENU框架下两种形式,构造函数则是可以选择是否设置角速度的值,如果选择设置角度速的值,就会在执行完正常的构造函数后再给角速度赋值

template<int... Ns>
DopplerError<Ns ...>::DopplerError(
                       const GnssMeasurement& measurement,
                       const GnssMeasurementIndex index,
                       const GnssErrorParameter& error_parameter,
                       const Eigen::Vector3d& angular_velocity)
  : DopplerError(measurement, index, error_parameter)
{
  angular_velocity_ = angular_velocity;
}

关于具体调用哪种形式,也是根据框架来的,如果是ECEF框架下就不会设置,反之就会设置这个值

// gnss_estimator_base.cpp
      if (parameter_id.type() == IdType::gPosition) {
        /* 无关的代码 */
        std::shared_ptr<DopplerError<3, 3, 1>> doppler_error = 
          std::make_shared<DopplerError<3, 3, 1>>(measurement, 
          GnssMeasurementIndex(satellite.prn, obs.first), 
          gnss_base_options_.error_parameter);
        /* 无关的代码 */
      }
      // pose in ENU for fusion
      else {
        /* 无关的代码 */
        std::shared_ptr<DopplerError<7, 9, 3, 1>> doppler_error = 
          std::make_shared<DopplerError<7, 9, 3, 1>>(measurement, 
          GnssMeasurementIndex(satellite.prn, obs.first), 
          gnss_base_options_.error_parameter, angular_velocity);
        /* 无关的代码 */
      }

EvaluateWithMinimalJacobians

残差和雅可比的计算也是对照着手册来就可以了,对应着manual残差中的第三部分,具体到3.4.3小节,在ENU融合框架的形式下:

  • 待估参数的形式为:
    在这里插入图片描述
  • 残差的计算:
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
  • 具体的雅可比计算
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

添加相对残差

相对残差的概念在manual 3.7.3有介绍
在这里插入图片描述
关于这部分的代码可以在RelativeConstErrorRelativeIntegrationError找到,比如对应到公式(3.136)的这个

  RelativeIntegrationError(const Eigen::Matrix<double, Dim, Dim>& psd, double dt) { // 设置权阵
    Eigen::MatrixXd covariance = Eigen::MatrixXd::Zero(2*Dim, 2*Dim);
    covariance.topLeftCorner(Dim, Dim) = psd * pow(dt, 3) / 3.0;
    covariance.topRightCorner(Dim, Dim) = psd * pow(dt, 2) / 2.0;
    covariance.bottomLeftCorner(Dim, Dim) = psd * pow(dt, 2) / 2.0;
    covariance.bottomRightCorner(Dim, Dim) = psd * dt;
    setCovariance(covariance);
    dt_ = dt;
  }

更新DOP值updateGdop

DOP (dilution of precision)值,代表着精度因子,是用来判断卫星相对于观测者的几何位置所造成误差(卫星几何效应)的单位,会随卫星和接受器间的相对移动而变化,通常值越低代表误差越小。在GICI中,DOP值的计算是在函数gnss_common::computeDops

  inline void updateGdop(
    const GnssMeasurement& measurement_rov, const GnssMeasurementDDIndexPairs& indexes) {
    gdop_ = gnss_common::computeDops(
      measurement_rov, indexes, gnss_base_options_.common).x();
  }

这个函数核心的计算部分是调用了RTKLIB的dops函数

dops(ns, azel, degreeToRad(options.min_elevation), Dops.data());

关于这部分的原理,其实很简单,就只是一种精度的评价方式
在这里插入图片描述
在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/1865046.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

springcloud-gateway 路由加载流程

问题 Spring Cloud Gateway版本是2.2.9.RELEASE&#xff0c;原本项目中依赖服务自动发现来自动配置路由到微服务的&#xff0c;但是发现将spring.cloud.gateway.discovery.locator.enabledfalse 启动之后Gateway依然会将所有微服务自动注册到路由中&#xff0c;百思不得其解&a…

手把手从零开始搭建远程访问服务

远程访问服务工具——FRP frp 是一个能够实现内网穿透的高性能的反向代理应用&#xff0c;支持 TCP、UDP、HTTP、HTTPS 等多种协议。可以将内网服务以安全、便捷的方式通过具有公网的服务器来转发。 资源链接 根据自己服务型号和操作系统来选取对应的文件&#xff0c;不知道的…

汽车EDI: BMW EDI项目案例

宝马集团是全世界成功的汽车和摩托车制造商之一&#xff0c;旗下拥有BMW、MINI和Rolls-Royce三大品牌&#xff1b;同时提供汽车金融和高档出行服务。作为一家全球性公司&#xff0c;宝马集团在14个国家拥有31家生产和组装厂&#xff0c;销售网络遍及140多个国家和地区。 本文主…

mitt通信

一、mitt介绍 mitt是一款轻量级的组件通信插件(大小仅为200字节左右) 二、mitt安装 npm install --save mitt三、使用 1.在组件中使用 import mitt from mitt //创建mitt实例 const emitter mitt()// 监听事件 emitter.on(foo, e > console.log(foo, e) )// 通过通配符监…

09. Java ThreadLocal 的使用

1. 前言 本节内容主要是对 ThreadLocal 进行深入的讲解&#xff0c;具体内容点如下&#xff1a; 了解 ThreadLocal 的诞生&#xff0c;以及总体概括&#xff0c;是学习本节知识的基础&#xff1b;了解 ThreadLocal 的作用&#xff0c;从整体层面理解 ThreadLocal 的程序作用&…

VC++开发积累——vc++6.0中删除函数的方法,右键,Delete

目录 引出插曲&#xff1a;删除函数的方法多行注释的实现代码输入的自动提示搜索出来&#xff0c;标记和取消标记跳转到上一步的位置 ctrl TAB 总结其他规范和帮助文档创建第一个Qt程序对象树概念信号signal槽slot自定义信号和槽1.自定义信号2.自定义槽3.建立连接4.进行触发 自…

千呼新零售2.0-OCR拍照识别采购单

千呼新零售2.0系统是零售行业连锁店一体化收银系统&#xff0c;包括线下收银线上商城连锁店管理ERP管理商品管理供应商管理会员营销等功能为一体&#xff0c;线上线下数据全部打通。 适用于商超、便利店、水果、生鲜、母婴、服装、零食、百货、宠物、中医养生、大健康等连锁店…

Python 实现Excel转TXT,或TXT文本导入Excel

Excel是一种具有强大的数据处理和图表制作功能的电子表格文件&#xff0c;而TXT则是一种简单通用、易于编辑的纯文本文件。将Excel转换为TXT可以帮助我们将复杂的数据表格以文本的形式保存&#xff0c;方便其他程序读取和处理。而将TXT转换为Excel则可以将文本文件中的数据导入…

AI引领创意潮流:高效生成图片,参考图助力,一键保存到指定文件夹

在这个数字与创意交融的时代&#xff0c;我们迎来了AI绘画的新纪元。借助先进的AI技术&#xff0c;我们不仅能够高效生成图片&#xff0c;还能在参考图的启发下&#xff0c;激发无限创意&#xff0c;让您的想象力在数字世界中自由翱翔。 首助编辑高手软件中的魔法智能绘图板块&…

PMP证书在国内已经泛滥了,大家怎么看?

目前&#xff0c;越来越多的人获得了PMP证书。自1999年PMP引入中国以来&#xff0c;全国累计PMP考试人数接近60万人次&#xff0c;通过PMP认证的人数约为42万人。虽然这个数据看起来很大&#xff0c;但绝对不能说是过多。 首先&#xff0c;PMP在中国并不普遍。根据美国项目管理…

解决go语言对接s3的SDK上传文件遇到的问题

先看正确的配置 问题1 配置文件中的OssEndpoint 不管是minio还是oss需要带上http://或者https:// 否则会出现这个问题 operation error S3: PutObject, exceeded maximum number of attempts, 3, https response error StatusCode: 0, RequestID: , HostID: , request send …

qt报错:“QtRunWork”任务返回了 false,但未记录错误。

qt报错&#xff1a;“QtRunWork”任务返回了 false&#xff0c;但未记录错误。 说明情况一 说明 这个报错可能的原因有很多&#xff0c;这里只写一种&#xff0c;以后遇到再进行补充。 情况一 如果 Q_OBJECT 宏未正确处理&#xff0c;通常会出现类似的错误。 要使用信号与槽…

视频汇聚平台LntonCVS视频集中存储平台技术解决方案

安防视频监控技术是一种利用各种监控设备捕捉实时画面&#xff0c;并将其传输至监控中心或数据存储设备的技术。随着科技的不断进步&#xff0c;监控视频技术也在不断改进&#xff0c;应用领域也在不断扩展。 然而&#xff0c;尽管技术进步&#xff0c;当前视频监控技术仍然面临…

PointCloudLib-特征(Features)-全局对齐空间分布 (GASD) 描述符

全局对齐空间分布 (GASD) 描述符 本文档介绍用于高效对象识别和姿势估计的全局对齐空间分布 ([GASD]) 全局描述符。 GASD 基于对表示对象实例的整个点云的参考系的估计,该参考系用于将其与规范坐标系对齐。之后,根据对齐点云的 3D 点的空间分布方式计算对齐点云的描述符…

最新!计算机类SCI期刊全名单!你想发的顶刊都在这里

【SciencePub学术】近日&#xff0c;2023JCR正式发布&#xff0c;最受瞩目就是各类期刊的最新影响因子排名&#xff0c;本期&#xff0c;小编对计算机类的期刊做了一个整理&#xff0c;供计算机方向的研究学者们参考&#xff01; 来源&#xff1a;WOS数据库官网 完整名单 ※ 本…

新手选择代理IP时这几点误区一定要避开!

在选择代理IP时&#xff0c;许多用户可能会因为对代理IP的认识不足或受到一些误导&#xff0c;而陷入一些常见的误区。这些误区不仅可能导致用户无法达到预期的效果&#xff0c;还可能带来一些不必要的风险。下面&#xff0c;IPIDEA代理IP就与大家一同分析在选择代理IP时需要避…

《Attention is all you need》通俗解读,彻底理解版:part1

最近在更新 Transformer 的技术专栏&#xff0c;如果你关注我的公众号的话&#xff0c;会发现有不少文章都打上了“Transformer最后一公里”的标签。 打上标签的文章要么是介绍 Transformer 技术的&#xff0c;要么是介绍学习Transformer 所需要的背景知识的&#xff0c;比如这…

typescript学习回顾(二)

今天来分享一下ts的基础&#xff0c;如何使用ts&#xff0c;以及ts具体的作用&#xff0c;如何去约束我们开发中常见的一些数据的&#xff0c;最后做一个小练习来巩固对ts基础的掌握程度。 类型约束 如何加类型约束呢 变量、函数的参数、函数的返回值位置加上:类型 比如 //约…

AI绘画Stable diffusion的SDXL模型超详细讲解,针不错!(含实操教程)

大家好&#xff0c;我是画画的小强 朋友们好&#xff0c;今天分享的是Stable diffusion的SDXL模型以及相关实操。 与之前的SD1.5大模型不同&#xff0c;这次的SDXL在架构上采用了“两步走”的生图方式&#xff1a; 以往SD1.5大模型&#xff0c;生成步骤为 Prompt → Base → …

时序分析(二):input delay分析

一、IO接口分析基本模型 数据按照同步方式可分为系统同步和源同步方式两种。所谓系统同步指发送端和接收端共用一个时钟源&#xff1b;源同步指发送端提供数据同步时钟&#xff0c;接收端根据该时钟进行数据接收。现在多数通信中使用源同步方式&#xff0c;例如以太网、ADC等。…