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();
}
示例#2
0
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;
}
示例#3
0
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);
}
示例#4
0
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;
}
示例#5
0
文件: vector.cpp 项目: jorgeer/cpp
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;
}
示例#6
0
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));
}
示例#7
0
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;
}
示例#8
0
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);
}