Function TuneErrorModel

applications/IV19_GNSS.cpp:168–272

@brief Use a GMM to estimate the error distribution of a factor * * @param Graph reference to the factor graph object * @param Config reference to the factor graph config object that specifies the motion model * @return nothing * */

*
*/
void TuneErrorModel(libRSF::FactorGraph &Graph,
                    libRSF::FactorGraphConfig &Config)
{
  if(Config.GNSS.ErrorModel.TuningType != libRSF::ErrorModelTuningType::None)
  {
    /** compute resudiuals of the factor graph */
    std::vector<double> ErrorData;
    Graph.computeUnweightedError(libRSF::FactorType::Pseudorange3_ECEF, ErrorData);

    if(Config.GNSS.ErrorModel.TuningType == libRSF::ErrorModelTuningType::EM)
    {
      /** fill empty GMM */
      libRSF::GaussianMixture<1> GMM;

      if(GMM.getNumberOfComponents() == 0)
      {
        GMM.initSpread(GMM_N, 10);
      }

      /** call the EM algorithm */
      libRSF::GaussianMixture<1>::EstimationConfig GMMConfig;
      GMMConfig.EstimationAlgorithm = libRSF::ErrorModelTuningType::EM;
      GMMConfig.RemoveSmallComponents = false;
      GMMConfig.MergeSimiliarComponents = false;
      GMM.estimate(ErrorData, GMMConfig);

      /** remove offset of the first "LOS" component */
      GMM.removeOffset();

      /** apply error model */
      if(Config.GNSS.ErrorModel.MixtureType == libRSF::ErrorModelMixtureType::SumMix)
      {
        libRSF::SumMix1 NewSMModel(GMM);
        Graph.setNewErrorModel(libRSF::FactorType::Pseudorange3_ECEF, NewSMModel);
      }
      else if(Config.GNSS.ErrorModel.MixtureType == libRSF::ErrorModelMixtureType::MaxMix)
      {
        libRSF::MaxMix1 NewMMModel(GMM);
        Graph.setNewErrorModel(libRSF::FactorType::Pseudorange3_ECEF, NewMMModel);
      }

    }
    else if(Config.GNSS.ErrorModel.TuningType == libRSF::ErrorModelTuningType::VBI)
    {
      /** initialize GMM */
      static libRSF::GaussianMixture<1> GMMAdaptive;
      libRSF::GaussianComponent<1> Component;

      if(GMMAdaptive.getNumberOfComponents() == 0)
      {
        GMMAdaptive.initSpread(1, 10);
      }

      /** calculate statistics for GMM initialization*/
      std::vector<double> v = ErrorData;
      double sum = std::accumulate(v.begin(), v.end(), 0.0);
      double mean = sum / v.size();