gici-open学习日记(6)：GNSS图优化——SPP

GINS初始化时的SPP调用

在GINS初始化时，涉及到了好多层的addMeasurement，首先是MultiSensorEstimating::handleTimePropagationSensors函数里会调用总估计器的addMeasurement

void MultiSensorEstimating::handleTimePropagationSensors(EstimatorDataCluster& data)
{
  // only support IMU
  CHECK(estimatorDataIsImu(data));    // LOG记录信息
  if (estimator_) estimator_->addMeasurement(data);
  /* 这部分不关注的代码 */
}

这里还是以RTK/IMU/视觉组合为例，在这个函数里面又会去调用下一层初始化器的addMeasurement

bool RtkImuCameraRrrEstimator::addMeasurement(const EstimatorDataCluster& measurement)
{
  /* 这部分不关注的代码 */
    if (gnss_imu_initializer_->addMeasurement(measurement)) { // 这里的data应该是IMU和GNSS可能共存的
      gnss_imu_initializer_->estimate();                      // GNSS/IMU初始化
  /* 这部分不关注的代码 */
}

然后进去了又调用了下一层估计器的addMeasurement，这里就是RTK估计器的addMeasurement

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

然后在这里，又调用了RtkEstimator::addGnssMeasurementAndState函数

bool RtkEstimator::addMeasurement(const EstimatorDataCluster& measurement)
{
	/* 这部分不关注的代码 */
    return addGnssMeasurementAndState(rov, ref);  // TODO
	/* 这部分不关注的代码 */
}

在这个函数里，又调用了SPP估计器的addGnssMeasurementAndState

bool RtkEstimator::addGnssMeasurementAndState(
    const GnssMeasurement& measurement_rov, 
    const GnssMeasurement& measurement_ref)
{
  /* 这部分不关注的代码 */
  if (!spp_estimator_->addGnssMeasurementAndState(measurement_rov)) {
  /* 这部分不关注的代码 */
}

在这里，才最后加到SPP里

correctCodeBias码偏差改正

原理

关于GNSS差分码偏差，网上有很多详细的资料，也可以参考manual的 3.4.1 以及 3.4.2 两节中的对应部分，这里推荐一篇博客GNSS差分码偏差（DCB）原理学习与数据下载地址

GNSS差分码偏差（DCB，Differential Code Bias）是由不同类型的GNSS信号在卫星和接收机不同通道产生的时间延迟（硬件延迟/码偏差）差异，按照频率相同或者不同又可以细分为频内偏差（例如GPS P1-C1）和频间偏差（例如GPS P1-P2）。
由于GNSS卫星钟差基准通常定义在某一指定频率（如BDS-2 B3）或某两个频率的无电离层组合（如GPS P1/P2和Galileo E1/E5a）伪距观测量上，因此使用不同频率不同 观测量组合时，必须引入差分码偏差参数进行改正。
以GPS 双频C1-P2数据为例，进行无电离层组合时需加入P1-C1 DCB值将基准修正到GPS无电离层组合的钟差基准。

代码

void GnssEstimatorBase::correctCodeBias(
  GnssMeasurement& measurement, const bool accept_coarse)
{
  CodeBiasPtr code_bias = measurement.code_bias;
  for (auto& sat : measurement.satellites) {
    Satellite& satellite = sat.second;        // 选择卫星
    std::string prn = satellite.prn;          // 卫星的prn号
    for (auto& obs : satellite.observations) {
      int code = obs.first;                   // 码(C1C C1W之类的码)
      Observation& observation = obs.second;  // 观测值
      if (observation.pseudorange == 0.0) continue;
      double bias = code_bias->getCodeBias(prn, code, accept_coarse); // 得到差分码偏差
      if (bias == 0.0) {
        // code bias not availible
        observation.pseudorange = 0.0;
      }
      else {
        observation.pseudorange += bias;
      }
    }
  }
}

其中，getCodeBias函数就是根据prn号和码标志来得到对应的改正值。
事实上如果是PPP的话这里应该会更复杂？因为IFB和电离层延迟是相互影响的，所以我们一般的做法是作为待估参数进行估计，代码中涉及到PPP的部分这里就不再关注了。

computeIonosphereDelay计算电离层延迟

原理

电离层延迟是影响GNSS定位一个关键因素，事实上电离层延迟的影响非常明显，尤其在中低纬度地区的午后时间，电离层活动十分活跃，对定位的影响十分明显。GICI-LIB里对电离层的改正主要涉及到了四种IonoType

1. None
   这种应该是延迟计算失败了，之后如果是这种的话应该是会跳出的
2. Broadcast
   广播星历改正，这里用到的是很经典的Klobuchar模型，广播星历是会包含对应的电离层参数，利用模型计算得到对应的电离层延迟，这里放一张《GPS理论、算法与应用 第3版》相关内容的截图供参考，详细的原理和计算可以参考这本书，或者我们当年上课用的教材《GNSS原理及应用》
   
3. DualFrequency
   这个指得就是通过观测量的线性组合消除电离层误差，也就是消电离层组合(Ionosphere-Free (IF) combination)，关于具体为什么这个组合可以消除电离层延迟，需要结合GNSS观测方程来具体描述，这里不详细记录，组合的计算公式很简单
   
4. Augmentation
   对着manual来看应该是使用其他模型的方法，像SBAS、GIM模型等等
   
   电离层改正常用的一般就是Klobuchar模型和消电离层组合两种办法，如果是PPP的话可能会涉及到作为输入GIM模型、参数估计等等，那样要对电离层本身要有更多的认知(本科毕设做的相关东西的我并不想再看这个了)，这里就不再赘述了。

代码

这个函数只是进行了电离层延迟的计算，还没有涉及到改正。具体函数的前面部分是一些相关的判断

    auto& satellite = sat.second;
    char system = satellite.getSystem();
    std::vector<Observation> dual_frequency_observations;
    std::vector<Observation> valid_observations;

    if (satellite.ionosphere_type == IonoType::Augmentation) continue;    // 外部模型涉及到输入，所以这里跳出
    CHECK(satellite.ionosphere == 0.0);

    if (!gnss_common::useSystem(gnss_base_options_.common, satellite.getSystem()) || // 确保系统和卫星的有效性
        !gnss_common::useSatellite(gnss_base_options_.common, satellite.prn)) continue;

接下来的一段是判断电离层延迟能不能用双频计算，所以要用到base frequency

double ionosphere_delay;
    IonoType type = IonoType::None;
    for (auto obs : satellite.observations) {
      if (!checkObservationValid(measurement,
          GnssMeasurementIndex(satellite.prn, obs.first))) continue;
      
      // Add to valid
      bool found = false;
      for (auto& it : valid_observations) {
        if (checkEqual(it.wavelength, obs.second.wavelength)) { // 波长
          found = true; break;
        }
      }
      if (!found) valid_observations.push_back(obs.second);

      // only use bases frequencies to apply dual-frequency ionosphere calculation, 
      // or it may contain large IFB
      CodeBias::BaseFrequencies bases = measurement.code_bias->getBase(); // base frequency
      std::pair<int, int> base_pair = bases.at(system);
      int phase_id_base_lhs = 
        gnss_common::getPhaseID(system, base_pair.first);
      int phase_id_base_rhs = 
        gnss_common::getPhaseID(system, base_pair.second);
      int phase_id = 
        gnss_common::getPhaseID(system, obs.first);
      /* 观测值必须和base frequency中至少一个相同 */
      if (phase_id != phase_id_base_lhs && phase_id != phase_id_base_rhs) {
        continue;
      }

      // ensure different frequencies
      found = false;
      for (auto& it : dual_frequency_observations) {
        if (checkEqual(it.wavelength, obs.second.wavelength)) {
          found = true; break;
        }
      }
      if (!found) dual_frequency_observations.push_back(obs.second);
    }

第一种就是用双频计算电离层延迟

    bool computed = false;
    if (!use_single_frequency && dual_frequency_observations.size() > 1) 
    {
      // use the maximum and minimum wavelengths
      std::sort(dual_frequency_observations.begin(), 
                dual_frequency_observations.end(),
        [](Observation& lhs, Observation& rhs) 
        { return lhs.wavelength < rhs.wavelength; });

      // check error ratio
      double ratio = fabs(1.0 / (1.0 - square(
        dual_frequency_observations.back().wavelength / 
        dual_frequency_observations.front().wavelength)));
      if (ratio < 3.0) {
        // compute 
        double ionosphere = gnss_common::ionosphereDualFrequency(
          dual_frequency_observations.front(), dual_frequency_observations.back());
        ionosphere_delay = gnss_common::ionosphereConvertToBase(
          ionosphere, dual_frequency_observations.front().wavelength);
        type = IonoType::DualFrequency;
        computed = true;
      }
    }

第二种就是用广播星历模型了

    if (!computed) {
      double timestamp = measurement.timestamp;
      Eigen::Vector3d position = measurement.position;
      if (checkZero(position)) {
        // For initial SPP
        ionosphere_delay = 0.0;
      }
      else {
        double azimuth = gnss_common::satelliteAzimuth(
          satellite.sat_position, position);
        double elevation = gnss_common::satelliteElevation(
          satellite.sat_position, position);
        double wavelength = valid_observations[0].wavelength;
        Eigen::VectorXd iono_parameters = measurement.ionosphere_parameters;
        ionosphere_delay = gnss_common::ionosphereBroadcast(timestamp, position, // 广播星历模型计算电离层
          azimuth, elevation, wavelength, iono_parameters);
        ionosphere_delay = gnss_common::ionosphereConvertToBase(
          ionosphere_delay, wavelength);
      }
      type = IonoType::Broadcast;
    }

计算对流层延迟

这个部分没有单独的函数完成，而是在PseudorangeError类中进行的，里面主要用到了Saastamoinen模型和GMF投影模型，这里还是从《GPS理论、算法与应用 第3版》相关内容进行截图供参考，详细的原理和计算可以参考这本书，或者我们当年上课用的教材《GNSS原理及应用》

加入图结构的ParameterBlock

位置先验addGnssPositionParameterBlock

这个没什么好说的，就是把当前坐标作为先验信息加到了图结构graph_中，如果没有的话应该就是用初值加

BackendId GnssEstimatorBase::addGnssPositionParameterBlock(
  const int32_t id, const Eigen::Vector3d& prior)
{
  BackendId position_id = createGnssPositionId(id);
  std::shared_ptr<PositionParameterBlock> position_parameter_block =  // 创建参数块
    std::make_shared<PositionParameterBlock>(prior, position_id.asInteger());
  CHECK(graph_->addParameterBlock(position_parameter_block));   // 加到图中
  return position_id;
}

钟差addClockParameterBlocks

先是添加系统间的钟差参数，因为这个差值是和系统相关的，所以一种系统添加一次就可以了。但是在这里，因为传入的只有三个参数，也就意味着prior这个参数一直是空的，所以相当于没有先验，clock_init一直是0

  for (size_t i = 0; i < getGnssSystemList().size(); i++) 
  {
    const char system = getGnssSystemList()[i];               // 对系统进行遍历
    BackendId clock_id = createGnssClockId(system, id);       // 所以这个里面是系统间的钟差
    if (gnss_common::useSystem(gnss_base_options_.common, system) && 
        !graph_->parameterBlockExists(clock_id.asInteger()))  // 确保之前没有添加过
    {
      Eigen::Matrix<double, 1, 1> clock_init;
      clock_init.setZero();
      if (prior.find(system) != prior.end()) clock_init[0] = prior.at(system);
      std::shared_ptr<ClockParameterBlock> clock_parameter_block = 
        std::make_shared<ClockParameterBlock>(clock_init, clock_id.asInteger());
      CHECK(graph_->addParameterBlock(clock_parameter_block));
    }
  }

接下来分别看用了哪几个系统，以及系统对应的观测数

  std::map<char, int> system_observation_cnt;
  for (size_t i = 0; i < getGnssSystemList().size(); i++) { // 先看用了哪几个系统
    const char system = getGnssSystemList()[i];
    if (!gnss_common::useSystem(gnss_base_options_.common, system)) continue;
    system_observation_cnt.insert(std::make_pair(system, 0));
  }
  for (auto& sat : measurement.satellites)                  // 再统计每个系统的观测数
  {
    auto& satellite = sat.second;
    char system = satellite.getSystem();
    if (!gnss_common::useSystem(gnss_base_options_.common, system)) continue;
    for (auto obs : satellite.observations) {
      if (checkObservationValid(measurement, 
          GnssMeasurementIndex(satellite.prn, obs.first))) {
        system_observation_cnt.at(system)++;
      }
    }
  }

然后把全部的有效观测都设置了钟差作为参数加了进去，同时也添加了残差

  for (auto it_system : system_observation_cnt) // 这里的遍历会对所有观测都进行遍历
  {
    char system = it_system.first;
    bool valid = it_system.second;
    if (valid) {
      num_valid_system++;
    }

    BackendId clock_id = createGnssClockId(system, id);
    Eigen::VectorXd measurement = Eigen::VectorXd::Zero(1);
    if (prior.find(system) != prior.end()) measurement[0] = prior.at(system);
    Eigen::MatrixXd information = Eigen::MatrixXd::Identity(1, 1) * 1e-6;
    std::shared_ptr<ClockError> clock_error = 
      std::make_shared<ClockError>(measurement, information);
    graph_->addResidualBlock(clock_error, nullptr,  // 残差和参数块在一个语句中加入了
      graph_->parameterBlockPtr(clock_id.asInteger()));
  }

添加伪距残差addPseudorangeResidualBlocks

**这个是重要的部分，进行伪距残差模块的添加。**函数里分为了非精密定位模式和精密定位两种模式，这里看的是非精密定位的模式(即利用模型改正大气延迟，也不考虑接收机的IFB)。

• 大气改正是在Evlaute里进行的，不是在这里进行的，这里只是加入参数块和残差块

  CHECK(!(use_single_frequency && is_verbose_model_));
  num_valid_satellite = 0;
  const BackendId parameter_id = state.id;  // 外部是让curGNSS()和curState()的ID一致了

  // None precise mode. 
  // Do not estimate atmosphere, we correct them by models or measurements. 
  // Ignore IFBs, because they are not significant for meter-level positioning. 
  if (!is_verbose_model_) 
  {
    for (auto& sat : measurement.satellites) 
    {
      const Satellite& satellite = sat.second;          // 卫星的信息：位置、速度等
      std::vector<Observation> observations_frequency;  // TODO 好像没用到啊
      char system = satellite.getSystem();

      if (!gnss_common::useSystem(gnss_base_options_.common, system) || 
          !gnss_common::useSatellite(gnss_base_options_.common, satellite.prn)) continue;
      if (satellite.ionosphere_type == IonoType::None) continue;

      // Add residuals
      bool has_valid = false;
      for (auto obs : satellite.observations) {
        if (!checkObservationValid(measurement, 
          GnssMeasurementIndex(satellite.prn, obs.first))) continue;
        
        // check single frequency
        if (use_single_frequency) {
          CodeBias::BaseFrequencies bases = measurement.code_bias->getBase();
          std::pair<int, int> base_pair = bases.at(system);
          if (system == 'C') base_pair.first = CODE_L2I;  // use B1I for BDS
          int phase_id_base = gnss_common::getPhaseID(system, base_pair.first);
          int phase_id = gnss_common::getPhaseID(system, obs.first);
          if (phase_id_base != phase_id) continue;
        }

        BackendId clock_id = createGnssClockId(satellite.getSystem(), measurement.id);

        // position in ECEF for standalone 
        ceres::ResidualBlockId residual_id;
        if (parameter_id.type() == IdType::gPosition) {
          is_state_pose_ = false;
          std::shared_ptr<PseudorangeError<3, 1>> pseudorange_error = 
            std::make_shared<PseudorangeError<3, 1>>(measurement,   // measurement包含误差项
            GnssMeasurementIndex(satellite.prn, obs.first), 
            gnss_base_options_.error_parameter);
          residual_id = graph_->addResidualBlock(pseudorange_error, 
            huber_loss_function_ ? huber_loss_function_.get() : nullptr,
            graph_->parameterBlockPtr(parameter_id.asInteger()),
            graph_->parameterBlockPtr(clock_id.asInteger()));
        }
        // pose in ENU for fusion
        else {
          is_state_pose_ = true;
          BackendId pose_id = state.id_in_graph;
          std::shared_ptr<PseudorangeError<7, 3, 1>> pseudorange_error =  // ENU下是七维
            std::make_shared<PseudorangeError<7, 3, 1>>(measurement,      // measurement包含误差项
            GnssMeasurementIndex(satellite.prn, obs.first), 
            gnss_base_options_.error_parameter);
          pseudorange_error->setCoordinate(coordinate_);
          residual_id = graph_->addResidualBlock(pseudorange_error, 
            huber_loss_function_ ? huber_loss_function_.get() : nullptr,
            graph_->parameterBlockPtr(pose_id.asInteger()), 
            graph_->parameterBlockPtr(gnss_extrinsics_id_.asInteger()),
            graph_->parameterBlockPtr(clock_id.asInteger()));
        }
        
        has_valid = true;
      }
      if (has_valid) num_valid_satellite++;
    }

spp_estimator_->estimate()进行SPP解算

RTK解算的时候，spp_estimator_->estimate()的调用是在spp_estimator_->addGnssMeasurementAndState(measurement_rov)之后的。这里只是记录了调用了Ceres库进行优化的流程，具体的残差、雅可比计算和求解并不在这里

  if (!spp_estimator_->addGnssMeasurementAndState(measurement_rov)) {
    return false;
  }
  if (!spp_estimator_->estimate()) {
    return false;
  }

函数首先调用了optimize()进行了优化

  if (gnss_base_options_.use_outlier_rejection)
  while (1)
  {
    optimize(); // 执行优化
    // reject outlier
    if (!rejectPseudorangeOutlier(curState(), 
        gnss_base_options_.reject_one_outlier_once)) break;
  }
  // Optimize without FDE
  else {
    optimize();
  }

如果DOP值太高就删掉参数块

  // DOP值太高就把当前的都删掉
  bool dop_valid = true;
  curGnss().position = getPositionEstimate(curState());
  if (!checkZero(curGnss().position)) {
    updateGdop(curGnss());
    if (gdop_ > gnss_base_options_.common.max_gdop) {
      LOG(INFO) << "High GDOP! Our tolerant is " 
        << gnss_base_options_.common.max_gdop << " in maximum, but current GDOP is "
        << gdop_ << "!";
      // erase parameters
      Graph::ParameterBlockCollection parameters = graph_->parameters();
      for (auto parameter : parameters) graph_->removeParameterBlock(parameter.first);
      states_.clear(); 
      dop_valid = false;
    }
  }

接下来计算残差和雅可比，同时计算卡方进行，但是卡方太高的话只是给了信息，而不是认为解算失败

int n_residual = 0;
  int n_parameter = graph_->parameters().size();
  double chi_square = 0.0;
  auto residual_blocks = graph_->residuals();
  for (auto residual_block : residual_blocks) {
    std::shared_ptr<ErrorInterface> interface = residual_block.error_interface_ptr;
    ErrorType type = interface->typeInfo();
    if (!(type == ErrorType::kPseudorangeError)) continue;  // 伪距残差
    double residual[1];
    n_residual++;
    graph_->problem()->EvaluateResidualBlock(residual_block.residual_block_id, 
      false, nullptr, residual, nullptr); // 计算残差和雅可比
    chi_square += square(residual[0]);
  }
  int chisqr_degree = n_residual - n_parameter - 1;
  if (chisqr_degree > 0 && chi_square > chisqr[n_residual - n_parameter - 1]) {
    LOG(INFO) << "High chi-square! Chi-square is " << chi_square
      << ". Chi-square threshold is " << chisqr[n_residual - n_parameter - 1] << ".";
  }

如果要估计速度的话，就用到了多普勒观测值，加入对应的速度和频率参数块。为了加速优化，因为之前已经优化完位置、钟差等了，就先把这些从节点删除，认为是常值，然后去优化速度和频率

  if (spp_options_.estimate_velocity && dop_valid) 
  {
    // Add parameter blocks
    int num_valid_doppler_system = 0;
    // velocity block
    addGnssVelocityParameterBlock(curGnss().id);
    // frequency block
    addFrequencyParameterBlocks(curGnss(), curGnss().id, num_valid_doppler_system);

    // Add doppler residual blocks
    int num_valid_doppler_satellite = 0;
    addDopplerResidualBlocks(curGnss(), curState(), num_valid_doppler_satellite, true);
    if (!checkSufficientSatellite(
        num_valid_doppler_satellite, num_valid_doppler_system, false)) {
      eraseGnssVelocityParameterBlock(curState());
      eraseFrequencyParameterBlocks(curState());
      has_velocity_estimate_ = false;
    }
    else {
      // To speed-up the optimization, we do the following things:
      // erase all pseudorange residuals
      erasePseudorangeResidualBlocks(curState());
      // set position and clock parameters as constant
      graph_->setParameterBlockConstant(curState().id.asInteger());
      for (auto system : getGnssSystemList()) {
        BackendId clock_id = changeIdType(curState().id, IdType::gClock, system);
        if (graph_->parameterBlockExists(clock_id.asInteger())) {
          graph_->setParameterBlockConstant(clock_id.asInteger());
        }
      }

      optimize();
      has_velocity_estimate_ = true;
    }
  }

PseudorangeError伪距残差

在SPP中主要涉及的就是伪距残差，之前的几个函数都只是SPP优化的流程，具体的雅可比和残差的计算都涉及在PseudorangeError的类之中，注释给出了不同坐标框架(Global/ENU)以及不同定位精度模式(non-precise/precise)的几种残差定义的区别

pseudorange error
// The candidate parameter setups are:
// Group 1: P1. receiver position in ECEF (3), P2. receiver clock (1)
// Group 2: P1. body pose in ENU (7), P2. relative position from body to receiver
// in body frame (3), P3. receiver clock (1)
// Group 3: Group 1 + P3. IFB, P4. troposphere delay (1), P5. ionosphere delay (1)
// Group 4: Group 2 + P4. IFB, P5. troposphere delay (1), P6. ionosphere delay (1)

构造函数中判断了具体是哪个Group的形式，同时设置了信息矩阵，设置信息矩阵就是设置的一些协方差的值进行的。这里就不详细分析，主要看一下残差和雅可比的计算。

EvaluateWithMinimalJacobians

1. 对框架的判断：分别是ECEF和ENU两种，同时这里也把钟差提取出来了

  if (!is_estimate_body_)     // 判断是ECEF还是ENU
  {
    t_WR_ECEF = Eigen::Map<const Eigen::Vector3d>(parameters[0]);
    clock = parameters[1][0]; // ECEF的话就只有位置和钟差
  }
  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]);      // 相对位置

    // clock
    clock = parameters[2][0]; // 钟差

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

2. 固体潮改正：GICI好像没有固体潮的选项，默认都进行了改正

  double timestamp = measurement_.timestamp;
  Eigen::Vector3d tide = gnss_common::solidEarthTide(timestamp, t_WR_ECEF);
  t_WR_ECEF += tide;  // 固体潮改正

3. 大气延迟：大气延迟分为不作为参数估计(直接模型)和作为参数估计两种，主要看一下不作为参数估计的情况。（对流层是在这里计算得到的，而电离层是在前面计算得到的）

if (!is_estimate_atmosphere_) 
  {
    // Inter-Frequency Bias (IFB)
    ifb = 0.0;

    // troposphere hydro-static delay
    troposphere_delay = gnss_common::troposphereSaastamoinen( // 计算对流程干延迟
      timestamp, t_WR_ECEF, elevation);
    // troposphere wet delay
    if (measurement_.troposphere_wet != 0.0) {                // 湿延迟如果有就用GMF进行投影
      gnss_common::troposphereGMF(timestamp, t_WR_ECEF, elevation, nullptr, &gmf_wet);
      troposphere_delay += measurement_.troposphere_wet * gmf_wet;
    }

    // ionosphere
    if (satellite_.ionosphere != 0.0 && 
        satellite_.ionosphere_type == IonoType::Augmentation) {
      ionosphere_delay = satellite_.ionosphere;
      ionosphere_delay = gnss_common::ionosphereConvertFromBase(
        ionosphere_delay, observation_.wavelength);
    }
    else if (satellite_.ionosphere != 0.0 && 
             satellite_.ionosphere_type == IonoType::DualFrequency) {
      ionosphere_delay = satellite_.ionosphere;
      ionosphere_delay = gnss_common::ionosphereConvertFromBase(
        ionosphere_delay, observation_.wavelength);
    }
    else {
      ionosphere_delay = gnss_common::ionosphereBroadcast(timestamp, t_WR_ECEF, 
          azimuth, elevation, observation_.wavelength, measurement_.ionosphere_parameters);
    }
  }

4. 观测模型构建：根据前面得到的那些误差量，就可以构建最终的模型方程了

  // Get estimate derivated measurement
  double pseudorange_estimate = rho + clock -            // 误差改正
    satellite_.sat_clock + ifb + troposphere_delay + ionosphere_delay;

  // Compute error
  double pseudorange = observation_.pseudorange;
  Eigen::Matrix<double, 1, 1> error =                   // 残差
    Eigen::Matrix<double, 1, 1>(pseudorange - pseudorange_estimate);

  // weigh it
  Eigen::Map<Eigen::Matrix<double, 1, 1> > weighted_error(residuals);
  weighted_error = square_root_information_ * error;    // 设置权重

5. 计算雅可比：这里要结合manual的3.4.1小节，关于位置、钟差、电离层等变量的雅可比矩阵在manual里都有对应的说明，可以对着manual推一下(其实也不用怎么推，相比预积分形式简单的很)。这里就不贴具体代码了

• 里面每一个雅可比块都有一个JN_minimal_mapped的变量，还没想明白这个变量是干吗用的