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