void UnscentedTransform::compute (void)
  {
    std::vector<double> weights;
    std::vector<VectorXd> sigmaPoints_prior;
    std::vector<VectorXd> sigmaPoints_post;

    if (locked)
      throw new std::runtime_error("Unscented Transform. Called compute a second time - not allowed.");

    locked = true;

    try
    {
      // fill arrays: sigma points with associated weights
      generateSigmaPoints(sigmaPoints_prior, weights);

      // pop last element because its not for calculating the mean;
      // popped value will replace first element when calculating the
      // covariance
      double w0_cov = weights.back();
      weights.pop_back();
    
      // calculate mean y -----------------------------------
      for (int i = 0; i < sigmaPoints_prior.size(); i++)
      {
	// propagate the sigma points through the function and save
	// (needed for covariance) and covariance cannot be accumulated
	// here too because the mean is needed :(
	VectorXd post = transformer->transform(sigmaPoints_prior[i]);
	sigmaPoints_post.push_back(post);
    
	// accumulate to the approximated mean
	y += weights[i] * post;
      }

      // calculate covariances Py, Pxy ----------------------
      weights[0] = w0_cov;			// prepare weights
      VectorXd t(sigmaPoints_post[0].size());	// help vector (for transform)

      for (int i = 0; i < sigmaPoints_post.size(); i++)
      {
	t = sigmaPoints_post[i] - y;
	Py += weights[i] * t * t.transpose();
	Pxy += weights[i] * (sigmaPoints_prior[i] - x) * t.transpose();
      }
    } 
    catch (std::exception& e) 
    {
      std::string additionalInfo = "Computation of Unscented Transform failed. ";
      throw std::runtime_error(additionalInfo + e.what());
    }
  }
Example #2
0
void UKFPose2D::lineSensorUpdate(bool vertical, const Vector2f& reading, const Matrix2f& readingCov)
{
  generateSigmaPoints();

  // computeLineReadings
  Vector2f lineReadings[7];
  if(vertical)
    for(int i = 0; i < 7; ++i)
      lineReadings[i] = Vector2f(sigmaPoints[i].y(), sigmaPoints[i].z());
  else
    for(int i = 0; i < 7; ++i)
      lineReadings[i] = Vector2f(sigmaPoints[i].x(), sigmaPoints[i].z());

  // computeMeanOfLineReadings
  Vector2f lineReadingMean = lineReadings[0];
  for(int i = 1; i < 7; ++i)
    lineReadingMean += lineReadings[i];
  lineReadingMean *= 1.f / 7.f;

  // computeCovOfLineReadingsAndSigmaPoints
  Matrix2x3f lineReadingAndMeanCov = Matrix2x3f::Zero();
  for(int i = 0; i < 3; ++i)
  {
    const Vector2f d1 = lineReadings[i * 2 + 1] - lineReadingMean;
    lineReadingAndMeanCov += (Matrix2x3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished();
    const Vector2f d2 = lineReadings[i * 2 + 2] - lineReadingMean;
    lineReadingAndMeanCov += (Matrix2x3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished();
  }
  lineReadingAndMeanCov *= 0.5f;

  // computeCovOfLineReadingsReadings
  const Vector2f d = lineReadings[0] - lineReadingMean;
  Matrix2f lineReadingCov = (Matrix2f() << d * d.x(), d * d.y()).finished();
  for(int i = 1; i < 7; ++i)
  {
    const Vector2f d = lineReadings[i] - lineReadingMean;
    lineReadingCov += (Matrix2f() << d * d.x(), d * d.y()).finished();
  }
  lineReadingCov *= 0.5f;

  lineReadingMean.y() = Angle::normalize(lineReadingMean.y());
  const Matrix3x2f kalmanGain = lineReadingAndMeanCov.transpose() * (lineReadingCov + readingCov).inverse();
  Vector2f innovation = reading - lineReadingMean;
  innovation.y() = Angle::normalize(innovation.y());
  const Vector3f correction = kalmanGain * innovation;
  mean += correction;
  mean.z() = Angle::normalize(mean.z());
  cov -= kalmanGain * lineReadingAndMeanCov;
  Covariance::fixCovariance(cov);
}
Example #3
0
void UKFPose2D::landmarkSensorUpdate(const Vector2f& landmarkPosition, const Vector2f& reading, const Matrix2f& readingCov)
{
  generateSigmaPoints();

  // computeLandmarkReadings
  Vector2f landmarkReadings[7];
  for(int i = 0; i < 7; ++i)
  {
    Pose2f pose(sigmaPoints[i].z(), sigmaPoints[i].head<2>());
    Vector2f landmarkPosRel = pose.invert() * landmarkPosition; // TODO: optimize this
    landmarkReadings[i] = Vector2f(landmarkPosRel.x(), landmarkPosRel.y());
  }

  // computeMeanOfLandmarkReadings
  Vector2f landmarkReadingMean = landmarkReadings[0];
  for(int i = 1; i < 7; ++i)
    landmarkReadingMean += landmarkReadings[i];
  landmarkReadingMean *= 1.f / 7.f;

  // computeCovOfLandmarkReadingsAndSigmaPoints
  Matrix2x3f landmarkReadingAndMeanCov = Matrix2x3f::Zero();
  for(int i = 0; i < 3; ++i)
  {
    const Vector2f d1 = landmarkReadings[i * 2 + 1] - landmarkReadingMean;
    landmarkReadingAndMeanCov += (Matrix2x3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished();
    const Vector2f d2 = landmarkReadings[i * 2 + 2] - landmarkReadingMean;
    landmarkReadingAndMeanCov += (Matrix2x3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished();
  }
  landmarkReadingAndMeanCov *= 0.5f;

  // computeCovOfLandmarkReadingsReadings
  const Vector2f d = landmarkReadings[0] - landmarkReadingMean;
  Matrix2f landmarkReadingCov = Matrix2f::Zero();
  landmarkReadingCov << d * d.x(), d * d.y();
  for(int i = 1; i < 7; ++i)
  {
    const Vector2f d = landmarkReadings[i] - landmarkReadingMean;
    landmarkReadingCov += (Matrix2f() << d * d.x(), d * d.y()).finished();
  }
  landmarkReadingCov *= 0.5f;

  const Matrix3x2f kalmanGain = landmarkReadingAndMeanCov.transpose() * (landmarkReadingCov + readingCov).inverse();
  Vector2f innovation = reading - landmarkReadingMean;
  const Vector3f correction = kalmanGain * innovation;
  mean += correction;
  mean.z() = Angle::normalize(mean.z());
  cov -= kalmanGain * landmarkReadingAndMeanCov;
  Covariance::fixCovariance(cov);
}
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();
}
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;
}
Example #6
0
void UKFPose2D::poseSensorUpdate(const Vector3f& reading, const Matrix3f& readingCov)
{
  generateSigmaPoints();

  // computePoseReadings
  Vector3f poseReadings[7];
  for(int i = 0; i < 7; ++i)
    poseReadings[i] = sigmaPoints[i];

  // computeMeanOfPoseReadings
  Vector3f poseReadingMean = poseReadings[0];
  for(int i = 1; i < 7; ++i)
    poseReadingMean += poseReadings[i];
  poseReadingMean *= 1.f / 7.f;

  // computeCovOfPoseReadingsAndSigmaPoints
  Matrix3f poseReadingAndMeanCov = Matrix3f::Zero();
  for(int i = 0; i < 3; ++i)
  {
    const Vector3f d1 = poseReadings[i * 2 + 1] - poseReadingMean;
    poseReadingAndMeanCov += (Matrix3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished();
    const Vector3f d2 = poseReadings[i * 2 + 2] - poseReadingMean;
    poseReadingAndMeanCov += (Matrix3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished();
  }
  poseReadingAndMeanCov *= 0.5f;

  // computeCovOfPoseReadingsReadings
  Matrix3f poseReadingCov;
  const Vector3f d = poseReadings[0] - poseReadingMean;
  poseReadingCov << d * d.x(), d * d.y(), d * d.z();
  for(int i = 1; i < 7; ++i)
  {
    const Vector3f d = poseReadings[i] - poseReadingMean;
    poseReadingCov += (Matrix3f() << d * d.x(), d * d.y(), d * d.z()).finished();
  }
  poseReadingCov *= 0.5f;

  poseReadingMean.z() = Angle::normalize(poseReadingMean.z());
  const Matrix3f kalmanGain = poseReadingAndMeanCov.transpose() * (poseReadingCov + readingCov).inverse();
  Vector3f innovation = reading - poseReadingMean;
  innovation.z() = Angle::normalize(innovation.z());
  const Vector3f correction = kalmanGain * innovation;
  mean += correction;
  mean.z() = Angle::normalize(mean.z());
  cov -= kalmanGain * poseReadingAndMeanCov;
  Covariance::fixCovariance(cov);
}
Example #7
0
void UKFPose2D::motionUpdate(const Pose2f& odometryOffset, const Pose2f& filterProcessDeviation,
                             const Pose2f& odometryDeviation, const Vector2f& odometryRotationDeviation)
{
  generateSigmaPoints();

  // addOdometryToSigmaPoints
  for(int i = 0; i < 7; ++i)
  {
    Vector2f odo(odometryOffset.translation);
    odo.rotate(sigmaPoints[i].z());
    sigmaPoints[i] += Vector3f(odo.x(), odo.y(), odometryOffset.rotation);
  }

  // computeMeanOfSigmaPoints
  mean = sigmaPoints[0];
  for(int i = 1; i < 7; ++i)
    mean += sigmaPoints[i];
  mean *= 1.f / 7.f;

  // computeCovOfSigmaPoints
  const Vector3f d = sigmaPoints[0] - mean;
  cov << d * d.x(), d * d.y(), d * d.z();
  for(int i = 1; i < 7; ++i)
  {
    const Vector3f d = sigmaPoints[i] - mean;
    cov += (Matrix3f() << d * d.x(), d * d.y(), d * d.z()).finished();
  }
  cov *= 0.5f;
  Covariance::fixCovariance(cov);

  // addProcessNoise
  cov(0, 0) += sqr(filterProcessDeviation.translation.x());
  cov(1, 1) += sqr(filterProcessDeviation.translation.y());
  cov(2, 2) += sqr(filterProcessDeviation.rotation);

  Vector2f odo(odometryOffset.translation);
  odo.rotate(mean.z());
  cov(0, 0) += sqr(odo.x() * odometryDeviation.translation.x());
  cov(1, 1) += sqr(odo.y() * odometryDeviation.translation.y());
  cov(2, 2) += sqr(odometryOffset.rotation * odometryDeviation.rotation);
  cov(2, 2) += sqr(odo.x() * odometryRotationDeviation.x());
  cov(2, 2) += sqr(odo.y() * odometryRotationDeviation.y());
  ASSERT(cov(0, 1) == cov(1, 0) && cov(0, 2) == cov(2, 0) && cov(1, 2) == cov(2, 1));
  mean.z() = Angle::normalize(mean.z());
}
Example #8
0
void AngleEstimator::accSensorUpdate(const Vector3<>& acc, const Matrix3x3<>& measurementNoise)
{
  generateSigmaPoints();

  // apply measurement model
  sigmaPZacc[0] = accSensorModel(sigmaPX[0]);
  sigmaPZacc[1] = accSensorModel(sigmaPX[1]);
  sigmaPZacc[2] = accSensorModel(sigmaPX[2]);
  sigmaPZacc[3] = accSensorModel(sigmaPX[3]);
  sigmaPZacc[4] = accSensorModel(sigmaPX[4]);

  // update the state
  Vector3<> mean = meanOfSigmaPZacc();
  Matrix3x2<> covZX = covOfSigmaPZaccAndSigmaPX(mean);
  Matrix2x3<> kalmanGain = covZX.transpose() * (covOfSigmaPZacc(mean) + measurementNoise).invert();
  const Vector2<>& offset(kalmanGain * (acc - mean));
  x += offset;
  cov -= kalmanGain * covZX;
}
Example #9
0
void AngleEstimator::gyroSensorUpdate(const Vector2<>& gyro, const Matrix2x2<>& measurementNoise)
{
  generateSigmaPoints();

  // apply measurement model
  sigmaPZgyro[0] = gyroSensorModel(sigmaPX[0]);
  sigmaPZgyro[1] = gyroSensorModel(sigmaPX[1]);
  sigmaPZgyro[2] = gyroSensorModel(sigmaPX[2]);
  sigmaPZgyro[3] = gyroSensorModel(sigmaPX[3]);
  sigmaPZgyro[4] = gyroSensorModel(sigmaPX[4]);

  // update the state
  Vector2<> mean = meanOfSigmaPZgyro();
  Matrix2x2<> covZX = covOfSigmaPZgyroAndSigmaPX(mean);
  Matrix2x2<> kalmanGain = covZX.transpose() * (covOfSigmaPZgyro(mean) + measurementNoise).invert();
  const Vector2<>& offset(kalmanGain * (gyro - mean));
  x += offset;
  cov -= kalmanGain * covZX;
}
Example #10
0
void AngleEstimator::processUpdate(const Vector3<>& angleAxis, const Matrix2x2<>& processNoise)
{
  lastXInverse = x.invert();

  generateSigmaPoints();

  // apply dynamic model
  RotationMatrix rotation(angleAxis);
  sigmaPX[0] *= rotation;
  sigmaPX[1] *= rotation;
  sigmaPX[2] *= rotation;
  sigmaPX[3] *= rotation;
  sigmaPX[4] *= rotation;

  // update the state
  x = meanOfSigmaPX();
  cov = covOfSigmaPX(x);
  cov += processNoise;
}
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;
}
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;
}