TEST(FixedMeasurementVectorTest, SigmaPointDeltas) {
MyStateVector test_state;
MyMeasurementVector test_measurement;
test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
test_state.set_field<Altitude>(1000);
MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;
MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
(MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());
MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points =
test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);
MyMeasurementVector mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);
MyMeasurementVector::SigmaPointDeltas<MyStateVector> sigma_point_deltas, target_sigma_point_deltas;
target_sigma_point_deltas << 0, 0, 0, 0, 0, 0, 0, 0, -8.314, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8.314, 0, 0,
0, 0, 0, 0, 0, 0, 0, 8.314, 0, 0, 0, 0, 0, 0, 0, 0, 0, -8.314, 0, 0, 0,
-2.306, -2.306, -2.306, -2.306, -2.306, -2.306, -2.306, 12.682, 12.682, -2.306, -2.306, -2.306, -2.306, -2.306, -2.306, -2.306, -2.306, 12.682, 12.682, -2.306, -2.306,
0, 0, 0, 0, 3.606, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3.606, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 3.606, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3.606, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 3.606, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3.606, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0.043, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.043,
-1.8375, 10.542, 14.959, 19.376, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375, 1.7082, -2.7086, -7.126, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375, -1.8375;
sigma_point_deltas = mean_measurement.calculate_sigma_point_deltas<MyStateVector>(measurement_sigma_points);
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(0), sigma_point_deltas.col(0));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(1), sigma_point_deltas.col(1));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(2), sigma_point_deltas.col(2));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(3), sigma_point_deltas.col(3));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(4), sigma_point_deltas.col(4));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(5), sigma_point_deltas.col(5));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(6), sigma_point_deltas.col(6));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(7), sigma_point_deltas.col(7));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(8), sigma_point_deltas.col(8));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(9), sigma_point_deltas.col(9));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(10), sigma_point_deltas.col(10));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(11), sigma_point_deltas.col(11));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(12), sigma_point_deltas.col(12));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(13), sigma_point_deltas.col(13));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(14), sigma_point_deltas.col(14));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(15), sigma_point_deltas.col(15));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(16), sigma_point_deltas.col(16));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(17), sigma_point_deltas.col(17));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(18), sigma_point_deltas.col(18));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(19), sigma_point_deltas.col(19));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(20), sigma_point_deltas.col(20));
}
TEST(FixedMeasurementVectorTest, SigmaPointGenerationWithInput) {
MyStateVector test_state;
MyMeasurementVector test_measurement;
test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
test_state.set_field<Altitude>(1000);
MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;
MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
(MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());
MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points, target_sigma_points;
target_sigma_points << 1, 1, 1, 1, 1, 1, 1, 1, -7.313, 1, 1, 1, 1, 1, 1, 1, 1, 1, 9.313, 1, 1,
2, 2, 2, 2, 2, 2, 2, 10.313, 2, 2, 2, 2, 2, 2, 2, 2, 2, -6.313, 2, 2, 2,
-6.8, -6.8, -6.8, -6.8, -6.8, -6.8, -6.8, 8.188, 8.188, -6.8, -6.8, -6.8, -6.8, -6.8, -6.8, -6.8, -6.8, 8.188, 8.188, -6.8, -6.8,
1, 1, 1, 1, 4.606, 1, 1, 1, 1, 1, 1, 1, 1, 1, -2.606, 1, 1, 1, 1, 1, 1,
0, 0, 0, 0, 0, 3.606, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3.606, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 3.606, 0, 0, 0, 0, 0, 0, 0, 0, 0, -3.606, 0, 0, 0, 0,
89.3, 89.3, 89.3, 89.3, 89.3, 89.3, 89.3, 89.3, 89.3, 89.3, 89.257, 89.3, 89.3, 89.3, 89.3, 89.3, 89.3, 89.3, 89.3, 89.3, 89.343,
8.575, 20.954, 25.371, 29.788, 8.575, 8.575, 8.575, 8.575, 8.575, 8.575, 8.575, 12.121, 7.704, 3.287, 8.575, 8.575, 8.575, 8.575, 8.575, 8.575, 8.575;
measurement_sigma_points = test_measurement.calculate_sigma_point_distribution<MyStateVector>(
sigma_points, UKF::Vector<3>(1, 2, 3));
EXPECT_VECTOR_EQ(target_sigma_points.col(0), measurement_sigma_points.col(0));
EXPECT_VECTOR_EQ(target_sigma_points.col(1), measurement_sigma_points.col(1));
EXPECT_VECTOR_EQ(target_sigma_points.col(2), measurement_sigma_points.col(2));
EXPECT_VECTOR_EQ(target_sigma_points.col(3), measurement_sigma_points.col(3));
EXPECT_VECTOR_EQ(target_sigma_points.col(4), measurement_sigma_points.col(4));
EXPECT_VECTOR_EQ(target_sigma_points.col(5), measurement_sigma_points.col(5));
EXPECT_VECTOR_EQ(target_sigma_points.col(6), measurement_sigma_points.col(6));
EXPECT_VECTOR_EQ(target_sigma_points.col(7), measurement_sigma_points.col(7));
EXPECT_VECTOR_EQ(target_sigma_points.col(8), measurement_sigma_points.col(8));
EXPECT_VECTOR_EQ(target_sigma_points.col(9), measurement_sigma_points.col(9));
EXPECT_VECTOR_EQ(target_sigma_points.col(10), measurement_sigma_points.col(10));
EXPECT_VECTOR_EQ(target_sigma_points.col(11), measurement_sigma_points.col(11));
EXPECT_VECTOR_EQ(target_sigma_points.col(12), measurement_sigma_points.col(12));
EXPECT_VECTOR_EQ(target_sigma_points.col(13), measurement_sigma_points.col(13));
EXPECT_VECTOR_EQ(target_sigma_points.col(14), measurement_sigma_points.col(14));
EXPECT_VECTOR_EQ(target_sigma_points.col(15), measurement_sigma_points.col(15));
EXPECT_VECTOR_EQ(target_sigma_points.col(16), measurement_sigma_points.col(16));
EXPECT_VECTOR_EQ(target_sigma_points.col(17), measurement_sigma_points.col(17));
EXPECT_VECTOR_EQ(target_sigma_points.col(18), measurement_sigma_points.col(18));
EXPECT_VECTOR_EQ(target_sigma_points.col(19), measurement_sigma_points.col(19));
EXPECT_VECTOR_EQ(target_sigma_points.col(20), measurement_sigma_points.col(20));
}
TEST(StateVectorTest, SigmaPointMean) {
MyStateVector test_state;
test_state.set_field<LatLon>(UKF::Vector<2>(-37.8136, 144.9631));
test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
test_state.set_field<Altitude>(10);
MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;
MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
(MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());
EXPECT_VECTOR_EQ(test_state, MyStateVector::calculate_sigma_point_mean(sigma_points));
}
TEST(StateVectorTest, SigmaPointDeltas) {
MyStateVector test_state;
test_state.set_field<LatLon>(UKF::Vector<2>(-37.8136, 144.9631));
test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
test_state.set_field<Altitude>(10);
MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;
MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
(MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());
MyStateVector test_mean = MyStateVector::calculate_sigma_point_mean(sigma_points);
MyStateVector::SigmaPointDeltas sigma_point_deltas, target_sigma_point_deltas;
target_sigma_point_deltas << 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 3.464, 0, 0, 0, 0, 0, 0, 0, 0, -3.464;
sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points);
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(0), sigma_point_deltas.col(0));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(1), sigma_point_deltas.col(1));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(2), sigma_point_deltas.col(2));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(3), sigma_point_deltas.col(3));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(4), sigma_point_deltas.col(4));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(5), sigma_point_deltas.col(5));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(6), sigma_point_deltas.col(6));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(7), sigma_point_deltas.col(7));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(8), sigma_point_deltas.col(8));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(9), sigma_point_deltas.col(9));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(10), sigma_point_deltas.col(10));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(11), sigma_point_deltas.col(11));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(12), sigma_point_deltas.col(12));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(13), sigma_point_deltas.col(13));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(14), sigma_point_deltas.col(14));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(15), sigma_point_deltas.col(15));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(16), sigma_point_deltas.col(16));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(17), sigma_point_deltas.col(17));
EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(18), sigma_point_deltas.col(18));
}
TEST(StateVectorTest, SigmaPointGeneration) {
MyStateVector test_state;
test_state.set_field<LatLon>(UKF::Vector<2>(45, 135));
test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
test_state.set_field<Altitude>(10);
MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;
MyStateVector::SigmaPointDistribution sigma_points, target_sigma_points;
target_sigma_points << 45, 48.464, 45, 45, 45, 45, 45, 45, 45, 45, 41.536, 45, 45, 45, 45, 45, 45, 45, 45,
135, 135, 138.46, 135, 135, 135, 135, 135, 135, 135, 135, 131.54, 135, 135, 135, 135, 135, 135, 135,
1, 1, 1, 4.464, 1, 1, 1, 1, 1, 1, 1, 1, -2.464, 1, 1, 1, 1, 1, 1,
2, 2, 2, 2, 5.464, 2, 2, 2, 2, 2, 2, 2, 2, -1.464, 2, 2, 2, 2, 2,
3, 3, 3, 3, 3, 6.464, 3, 3, 3, 3, 3, 3, 3, 3, -0.464, 3, 3, 3, 3,
0, 0, 0, 0, 0, 0, 0.866, 0, 0, 0, 0, 0, 0, 0, 0, -0.866, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.866, 0, 0, 0, 0, 0, 0, 0, 0, -0.866, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.866, 0, 0, 0, 0, 0, 0, 0, 0, -0.866, 0,
1, 1, 1, 1, 1, 1, 0.5, 0.5, 0.5, 1, 1, 1, 1, 1, 1, 0.5, 0.5, 0.5, 1,
10, 10, 10, 10, 10, 10, 10, 10, 10, 13.464, 10, 10, 10, 10, 10, 10, 10, 10, 6.536;
sigma_points = test_state.calculate_sigma_point_distribution((covariance *
(MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());
EXPECT_VECTOR_EQ(target_sigma_points.col(0), sigma_points.col(0));
EXPECT_VECTOR_EQ(target_sigma_points.col(1), sigma_points.col(1));
EXPECT_VECTOR_EQ(target_sigma_points.col(2), sigma_points.col(2));
EXPECT_VECTOR_EQ(target_sigma_points.col(3), sigma_points.col(3));
EXPECT_VECTOR_EQ(target_sigma_points.col(4), sigma_points.col(4));
EXPECT_VECTOR_EQ(target_sigma_points.col(5), sigma_points.col(5));
EXPECT_VECTOR_EQ(target_sigma_points.col(6), sigma_points.col(6));
EXPECT_VECTOR_EQ(target_sigma_points.col(7), sigma_points.col(7));
EXPECT_VECTOR_EQ(target_sigma_points.col(8), sigma_points.col(8));
EXPECT_VECTOR_EQ(target_sigma_points.col(9), sigma_points.col(9));
EXPECT_VECTOR_EQ(target_sigma_points.col(10), sigma_points.col(10));
EXPECT_VECTOR_EQ(target_sigma_points.col(11), sigma_points.col(11));
EXPECT_VECTOR_EQ(target_sigma_points.col(12), sigma_points.col(12));
EXPECT_VECTOR_EQ(target_sigma_points.col(13), sigma_points.col(13));
EXPECT_VECTOR_EQ(target_sigma_points.col(14), sigma_points.col(14));
EXPECT_VECTOR_EQ(target_sigma_points.col(15), sigma_points.col(15));
EXPECT_VECTOR_EQ(target_sigma_points.col(16), sigma_points.col(16));
EXPECT_VECTOR_EQ(target_sigma_points.col(17), sigma_points.col(17));
EXPECT_VECTOR_EQ(target_sigma_points.col(18), sigma_points.col(18));
}
TEST(FixedMeasurementVectorTest, SigmaPointCovariance) {
MyStateVector test_state;
MyMeasurementVector test_measurement;
test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 0, 0));
test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
test_state.set_field<Altitude>(1000);
MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;
MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
(MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());
MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points =
test_measurement.calculate_sigma_point_distribution<MyStateVector>(sigma_points);
MyMeasurementVector mean_measurement = test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points);
MyMeasurementVector::SigmaPointDeltas<MyStateVector> sigma_point_deltas =
mean_measurement.calculate_sigma_point_deltas<MyStateVector>(measurement_sigma_points);
MyMeasurementVector::CovarianceMatrix calculated_covariance =
mean_measurement.calculate_sigma_point_covariance<MyStateVector>(sigma_point_deltas);
MyMeasurementVector::CovarianceMatrix expected_covariance;
expected_covariance << 5.31709, 0, 0, 0, 0, 0, 0, 0,
0, 5.31709, 0, 0, 0, 0, 0, 0,
0, 0, 29.2440, 0, 0, 0, 0, -4.237,
0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 1.44e-4, 0,
0, 0, -4.237, 0, 0, 0, 0, 32.263;
EXPECT_VECTOR_EQ(expected_covariance.col(0), calculated_covariance.col(0));
EXPECT_VECTOR_EQ(expected_covariance.col(1), calculated_covariance.col(1));
EXPECT_VECTOR_EQ(expected_covariance.col(2), calculated_covariance.col(2));
EXPECT_VECTOR_EQ(expected_covariance.col(3), calculated_covariance.col(3));
EXPECT_VECTOR_EQ(expected_covariance.col(4), calculated_covariance.col(4));
EXPECT_VECTOR_EQ(expected_covariance.col(5), calculated_covariance.col(5));
EXPECT_VECTOR_EQ(expected_covariance.col(6), calculated_covariance.col(6));
EXPECT_VECTOR_EQ(expected_covariance.col(7), calculated_covariance.col(7));
}