Ejemplos de meanOfSigmaReadings en C++ (Cpp)

Ejemplo n.º 1

Archivo: InertiaSensorFilter.cpp Proyecto: MisterSquishy/nbites

void InertiaSensorFilter::readingUpdate(const Vector3<>& reading)
{
  generateSigmaPoints();

  for(int i = 0; i < 5; ++i)
    readingModel(sigmaPoints[i], sigmaReadings[i]);

  meanOfSigmaReadings();

//  PLOT("module:InertiaSensorFilter:expectedAccX", readingMean.x);
//  PLOT("module:InertiaSensorFilter:accX", reading.x);
//  PLOT("module:InertiaSensorFilter:expectedAccY", readingMean.y);
//  PLOT("module:InertiaSensorFilter:accY", reading.y);
//  PLOT("module:InertiaSensorFilter:expectedAccZ", readingMean.z);
//  PLOT("module:InertiaSensorFilter:accZ", reading.z);

  covOfSigmaReadingsAndSigmaPoints();
  covOfSigmaReadings();

  const Matrix2x3<float>& kalmanGain = readingsSigmaPointsCov.transpose() * (readingsCov + p.sensorCov).invert();
  const Vector2<>& innovation = kalmanGain * (reading - readingMean);
  x += innovation;
  cov -= kalmanGain * readingsSigmaPointsCov;
}

Ejemplo n.º 2

Archivo: InertialDataFilter.cpp Proyecto: Yanzqing/BHumanCodeRelease

void InertialDataFilter::readingUpdate(const Vector3f& reading)
{
  generateSigmaPoints();

  for(int i = 0; i < 5; ++i)
    readingModel(sigmaPoints[i], sigmaReadings[i]);

  meanOfSigmaReadings();

  PLOT("module:InertialDataFilter:expectedAccX", readingMean.x());
  PLOT("module:InertialDataFilter:accX", reading.x());
  PLOT("module:InertialDataFilter:expectedAccY", readingMean.y());
  PLOT("module:InertialDataFilter:accY", reading.y());
  PLOT("module:InertialDataFilter:expectedAccZ", readingMean.z());
  PLOT("module:InertialDataFilter:accZ", reading.z());

  const Matrix3x2f readingsSigmaPointsCov = covOfSigmaReadingsAndSigmaPoints();
  const Matrix3f readingsCov = covOfSigmaReadings();

  const Matrix3f sensorCov = accNoise.array().square().matrix().asDiagonal();
  const Matrix2x3f kalmanGain = readingsSigmaPointsCov.transpose() * (readingsCov + sensorCov).inverse();
  mean += kalmanGain * (reading - readingMean);
  cov -= kalmanGain * readingsSigmaPointsCov;
}