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());
}
}
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);
}
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;
}
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);
}
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());
}
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;
}
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;
}
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;
}