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