void ObstacleCombinator::rotateMatrix(Matrix2x2<>& matrix, const float angle)
{
const float cosine = cos(angle);
const float sine = sin(angle);
const Matrix2x2<> rotationMatrix(cosine, -sine, sine, cosine);
matrix = (rotationMatrix * matrix) * rotationMatrix.transpose();
}
void UKF::update(const Vector2<> measurement,
const Matrix2x2<>& Q,
const CameraMatrix& theCameraMatrix,
const CameraInfo& theCameraInfo,
const ImageCoordinateSystem& theImageCoordinateSystem)
{
Matrix2x2<> L = Covariance::choleskyDecomposition(covariance);
std::vector<Vector2<> > sigmaPoints(5);
sigmaPoints[0] = (*h)(mean, theCameraMatrix, theCameraInfo, theImageCoordinateSystem);
for(int i = 0; i < 2; i++)
{
sigmaPoints[i + 1] = (*h)(mean + L[i], theCameraMatrix, theCameraInfo, theImageCoordinateSystem);
sigmaPoints[i + 3] = (*h)(mean - L[i], theCameraMatrix, theCameraInfo, theImageCoordinateSystem);
}
Vector2<> meanH = (sigmaPoints[0] + sigmaPoints[1] + sigmaPoints[2] + sigmaPoints[3] + sigmaPoints[4]) / 5.0f;
Matrix2x2<> covH;
for(int i = 0; i < 5; i++)
{
sigmaPoints[i] -= meanH;
covH[0][0] += sigmaPoints[i][0] * sigmaPoints[i][0];
covH[1][1] += sigmaPoints[i][1] * sigmaPoints[i][1];
const float cov01 = sigmaPoints[i][0] * sigmaPoints[i][1];
covH[0][1] += cov01;
covH[1][0] += cov01;
}
covH /= 2.0f;
Matrix2x2<> covHx;
for(int i = 1; i < 5; i++)
{
const float s = i > 2 ? -1.0f : 1.0f;
const int Lcol = i % 2 == 1 ? 0 : 1;
covHx[0][0] += sigmaPoints[i][0] * s * L[Lcol][0];
covHx[1][1] += sigmaPoints[i][1] * s * L[Lcol][1];
covHx[1][0] += sigmaPoints[i][0] * s * L[Lcol][1];
covHx[0][1] += sigmaPoints[i][1] * s * L[Lcol][0];
}
covHx /= 2.0f;
const Matrix2x2<> K = covHx.transpose() * (covH + Q).invert();
const Vector2<> innovation = measurement - meanH;
mean += K * innovation;
covariance -= K * covHx;
}
const Matrix2x2 orthogonalize(const Matrix2x2& m)
{
// TODO: In the worst case scenario, this could construct a matrix where
// one or both rows are incorrectly set to Vector2(1.0f, 0.0f) because of
// how normalize(const Vector2&) is implemented. Should this function check
// for division by zero instead of relying on the default behavior of
// vector normalization?
const Vector2 x = normalize(m.row(0));
Vector2 y = m.row(1);
// extract the part that is parallel to x and normalize
y -= x * dot(y, x);
y = normalize(y);
return Matrix2x2(x, y);
}
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;
}
Vector2 operator*(const Vector2 &lhs, const Matrix2x2 &rhs)
{
Vector2 product = Vector2();
double sum;
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 2; j++)
{
sum = product.get(i) + rhs.get(i, j) * lhs.get(j);
product.set(i, sum);
}
}
return product;
}
Vector2 operator*(const Matrix2x2 &lhs, const Vector2 &rhs) {
return Vector2(lhs.get(0, 0) * rhs.get(0) + lhs.get(0, 1) * rhs.get(1),
lhs.get(1, 0) * rhs.get(0) + lhs.get(1, 1) * rhs.get(1));
}
Matrix2x2 operator*(const Matrix2x2 &lhs, const Matrix2x2 &rhs) {
Matrix2x2 matrix = Matrix2x2();
matrix.set(0, 0, lhs.get(0, 0) * rhs.get(0, 0) + lhs.get(0, 1) * rhs.get(1, 0));
matrix.set(0, 1, lhs.get(0, 0) * rhs.get(0, 1) + lhs.get(0, 1) * rhs.get(1, 1));
matrix.set(1, 0, lhs.get(1, 0) * rhs.get(0, 0) + lhs.get(1, 1) * rhs.get(1, 0));
matrix.set(1, 1, lhs.get(1, 0) * rhs.get(0, 1) + lhs.get(1, 1) * rhs.get(1, 1));
return matrix;
}
bool operator==(const Matrix2x2 &lhs, const Matrix2x2 &rhs) {
return lhs.get(0, 0) == rhs.get(0, 0) && lhs.get(0, 1) == rhs.get(0, 1)
&& lhs.get(1, 0) == rhs.get(1, 0) && lhs.get(1, 1) == rhs.get(1, 1);
}