C++ (Cpp) covOfSigmaPointsの例

コード例 #1

ファイル: InertialDataFilter.cpp プロジェクト: Yanzqing/BHumanCodeRelease

void InertialDataFilter::predict(const RotationMatrix& rotationOffset)
{
  generateSigmaPoints();

  // update sigma points
  for(int i = 0; i < 5; ++i)
    sigmaPoints[i].rotation *= rotationOffset;

  // get new mean and cov
  meanOfSigmaPoints();
  covOfSigmaPoints();

  // add process noise
  cov += processNoise.array().square().matrix().asDiagonal();
}

コード例 #2

ファイル: InertiaSensorFilter.cpp プロジェクト: MisterSquishy/nbites

void InertiaSensorFilter::predict(const RotationMatrix& rotationOffset)
{
  generateSigmaPoints();

  // update sigma points
  for(int i = 0; i < 5; ++i)
    sigmaPoints[i].rotation *= rotationOffset;

  // get new mean and cov
  meanOfSigmaPoints();
  covOfSigmaPoints();

  // add process noise
  cov.c[0].x += p.processCov.c[0].x;
  cov.c[1].y += p.processCov.c[1].y;
}