void UKFSample::lineSensorUpdate(bool vertical, const Vector2f& reading, const Matrix2x2f& 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;
lineReadingMean = lineReadings[0];
for(int i = 1; i < 7; ++i)
lineReadingMean += lineReadings[i];
lineReadingMean *= 1.f / 7.f;
// computeCovOfLineReadingsAndSigmaPoints
Matrix2x3f lineReadingAndMeanCov;
for(int i = 0; i < 3; ++i)
{
Vector2f d1 = lineReadings[i * 2 + 1] - lineReadingMean;
lineReadingAndMeanCov += Matrix2x3f(d1 * l[i].x, d1 * l[i].y, d1 * l[i].z);
Vector2f d2 = lineReadings[i * 2 + 2] - lineReadingMean;
lineReadingAndMeanCov += Matrix2x3f(d2 * -l[i].x, d2 * -l[i].y, d2 * -l[i].z);
}
lineReadingAndMeanCov *= 0.5f;
// computeCovOfLineReadingsReadings
Matrix2x2f lineReadingCov;
Vector2f d = lineReadings[0] - lineReadingMean;
lineReadingCov = Matrix2x2f(d * d.x, d * d.y);
for(int i = 1; i < 7; ++i)
{
Vector2f d = lineReadings[i] - lineReadingMean;
lineReadingCov += Matrix2x2f(d * d.x, d * d.y);
}
lineReadingCov *= 0.5f;
lineReadingMean.y = normalize(lineReadingMean.y);
const Matrix3x2f kalmanGain = lineReadingAndMeanCov.transpose() * (lineReadingCov + readingCov).invert();
Vector2f innovation = reading - lineReadingMean;
innovation.y = normalize(innovation.y);
const Vector3f correction = kalmanGain * innovation;
mean += correction;
mean.z = normalize(mean.z);
cov -= kalmanGain * lineReadingAndMeanCov;
}
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 UKFSample::landmarkSensorUpdate(const Vector2<>& landmarkPosition, const Vector2f& reading, const Matrix2x2f& readingCov)
{
generateSigmaPoints();
// computeLandmarkReadings
Vector2f landmarkReadings[7];
for(int i = 0; i < 7; ++i)
{
Pose2D pose(sigmaPoints[i].z, sigmaPoints[i].x, sigmaPoints[i].y);
Vector2<> 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;
for(int i = 0; i < 3; ++i)
{
Vector2f d1 = landmarkReadings[i * 2 + 1] - landmarkReadingMean;
landmarkReadingAndMeanCov += Matrix2x3f(d1 * l[i].x, d1 * l[i].y, d1 * l[i].z);
Vector2f d2 = landmarkReadings[i * 2 + 2] - landmarkReadingMean;
landmarkReadingAndMeanCov += Matrix2x3f(d2 * -l[i].x, d2 * -l[i].y, d2 * -l[i].z);
}
landmarkReadingAndMeanCov *= 0.5f;
// computeCovOfLandmarkReadingsReadings
Vector2f d = landmarkReadings[0] - landmarkReadingMean;
Matrix2x2f landmarkReadingCov(d * d.x, d * d.y);
for(int i = 1; i < 7; ++i)
{
Vector2f d = landmarkReadings[i] - landmarkReadingMean;
landmarkReadingCov += Matrix2x2f(d * d.x, d * d.y);
}
landmarkReadingCov *= 0.5f;
const Matrix3x2f kalmanGain = landmarkReadingAndMeanCov.transpose() * (landmarkReadingCov + readingCov).invert();
Vector2f innovation = reading - landmarkReadingMean;
const Vector3f correction = kalmanGain * innovation;
mean += correction;
mean.z = normalize(mean.z);
cov -= kalmanGain * landmarkReadingAndMeanCov;
}