Function AverageMeasurement

src/Resampling.cpp:65–103

  }

  Data AverageMeasurement(const std::vector<Data> &Input)
  {
    Data Output;

    /** catch empty vectors */
    if(Input.empty())
    {
      PRINT_ERROR("There is no Measurement!");
      return Output;
    }

    /** return single element vectors directly */
    if(Input.size() == 1)
    {
      return Input.at(0);
    }

    /** create vectors */
    double Time = 0;
    Vector Mean (Input.front().getMean().size());
    Mean.setZero();
    Vector Info (Input.front().getCovarianceDiagonal().size());
    Info.setZero();
    for (const Data & Measurement : Input)
    {
      Time += Measurement.getTimestamp();
      Mean += Measurement.getMean();
      Info += Measurement.getCovarianceDiagonal().cwiseInverse();
    }
    Mean /= Input.size();
    Time /= Input.size();

    Output = Input.back();
    Output.setTimestamp(Time);
    Output.setMean(Mean);
    Output.setCovarianceDiagonal(Info.cwiseInverse());

    return Output;
  }

}