TEST(StateVectorTest, SigmaPointCovariance) {
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 = test_mean.calculate_sigma_point_deltas(sigma_points);
MyStateVector::CovarianceMatrix calculated_covariance = MyStateVector::calculate_sigma_point_covariance(sigma_point_deltas);
EXPECT_VECTOR_EQ(covariance.col(0), calculated_covariance.col(0));
EXPECT_VECTOR_EQ(covariance.col(1), calculated_covariance.col(1));
EXPECT_VECTOR_EQ(covariance.col(2), calculated_covariance.col(2));
EXPECT_VECTOR_EQ(covariance.col(3), calculated_covariance.col(3));
EXPECT_VECTOR_EQ(covariance.col(4), calculated_covariance.col(4));
EXPECT_VECTOR_EQ(covariance.col(5), calculated_covariance.col(5));
EXPECT_VECTOR_EQ(covariance.col(6), calculated_covariance.col(6));
EXPECT_VECTOR_EQ(covariance.col(7), calculated_covariance.col(7));
EXPECT_VECTOR_EQ(covariance.col(8), calculated_covariance.col(8));
}
TEST(StateVectorTest, Assignment) {
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);
EXPECT_VECTOR_EQ(UKF::Vector<2>(-37.8136, 144.9631), test_state.get_field<LatLon>());
EXPECT_VECTOR_EQ(UKF::Vector<3>(1, 2, 3), test_state.get_field<Velocity>());
EXPECT_QUATERNION_EQ(UKF::Quaternion(1, 0, 0, 0), test_state.get_field<Attitude>());
EXPECT_EQ(10, test_state.get_field<Altitude>());
}
TEST(FixedMeasurementVectorTest, Assignment) {
MyMeasurementVector test_measurement;
test_measurement.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));
test_measurement.set_field<DynamicPressure>(4);
test_measurement.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));
test_measurement.set_field<StaticPressure>(8);
EXPECT_EQ(8, test_measurement.size());
EXPECT_EQ(8, test_measurement.get_field<StaticPressure>());
EXPECT_EQ(4, test_measurement.get_field<DynamicPressure>());
EXPECT_VECTOR_EQ(UKF::Vector<3>(1, 2, 3), test_measurement.get_field<Gyroscope>());
EXPECT_VECTOR_EQ(UKF::Vector<3>(5, 6, 7), test_measurement.get_field<Accelerometer>());
UKF::Vector<8> expected;
expected << 5, 6, 7, 1, 2, 3, 8, 4;
EXPECT_VECTOR_EQ(expected, test_measurement);
}
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));
}
TEST(StateVectorTest, ProcessModel) {
ProcessModelTestStateVector test_state;
UKF::Vector<6> expected_state;
test_state.set_field<Position>(UKF::Vector<3>(0, 0, 0));
test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
expected_state << 0.1, 0.2, 0.3, 1, 2, 3;
EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1));
EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorRK4>(0.1));
EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorHeun>(0.1));
EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorEuler>(0.1));
expected_state << 0.1 + (0.5 * 3 * 0.1 * 0.1), 0.2 + (0.5 * 2 * 0.1 * 0.1), 0.3 + (0.5 * 1 * 0.1 * 0.1), 1.3, 2.2, 3.1;
EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1, UKF::Vector<3>(3, 2, 1)));
EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorRK4>(0.1, UKF::Vector<3>(3, 2, 1)));
EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorHeun>(0.1, UKF::Vector<3>(3, 2, 1)));
expected_state << 0.1, 0.2, 0.3, 1.3, 2.2, 3.1;
EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorEuler>(0.1, UKF::Vector<3>(3, 2, 1)));
}
TEST(FixedMeasurementVectorTest, SigmaPointMean) {
MyMeasurementVector::SigmaPointDistribution<MyStateVector> measurement_sigma_points;
MyMeasurementVector test_measurement;
measurement_sigma_points << 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,
-9.8, -9.8, -9.8, -9.8, -9.8, -9.8, -9.8, 5.188, 5.188, -9.8, -9.8, -9.8, -9.8, -9.8, -9.8, -9.8, -9.8, 5.188, 5.188, -9.8, -9.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;
MyMeasurementVector expected_mean;
expected_mean << 0.0, 0.0, -7.494, 1, 0, 0, 89.3, 10.4125;
EXPECT_VECTOR_EQ(expected_mean, test_measurement.calculate_sigma_point_mean<MyStateVector>(measurement_sigma_points));
}
TEST(FixedMeasurementVectorTest, MeasurementRootCovariance) {
MyMeasurementVector test_measurement;
MyMeasurementVector::measurement_root_covariance.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));
MyMeasurementVector::measurement_root_covariance.set_field<DynamicPressure>(4);
MyMeasurementVector::measurement_root_covariance.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));
MyMeasurementVector::measurement_root_covariance.set_field<StaticPressure>(8);
MyMeasurementVector::CovarianceMatrix expected_measurement_root_covariance = MyMeasurementVector::CovarianceMatrix::Zero();
expected_measurement_root_covariance.diagonal() << 5, 6, 7, 1, 2, 3, 8, 4;
MyMeasurementVector::CovarianceMatrix measurement_root_covariance = test_measurement.calculate_measurement_root_covariance();
EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(0), measurement_root_covariance.col(0));
EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(1), measurement_root_covariance.col(1));
EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(2), measurement_root_covariance.col(2));
EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(3), measurement_root_covariance.col(3));
EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(4), measurement_root_covariance.col(4));
EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(5), measurement_root_covariance.col(5));
EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(6), measurement_root_covariance.col(6));
EXPECT_VECTOR_EQ(expected_measurement_root_covariance.col(7), measurement_root_covariance.col(7));
}
TEST(StateVectorTest, UpdateDelta) {
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::StateVectorDelta test_delta;
test_delta << 10, -20, 3, 2, 1, 0, 0, 0.5, 1;
test_state.apply_delta(test_delta);
MyStateVector expected_state;
expected_state.set_field<LatLon>(UKF::Vector<2>(55, 115));
expected_state.set_field<Velocity>(UKF::Vector<3>(4, 4, 4));
expected_state.set_field<Attitude>(UKF::Quaternion(0.9692, 0, 0, 0.2462));
expected_state.set_field<Altitude>(11);
EXPECT_VECTOR_EQ(expected_state, test_state);
}
TEST(FixedMeasurementVectorTest, MultipleReassignment) {
MyMeasurementVector test_measurement;
test_measurement.set_field<Gyroscope>(UKF::Vector<3>(1, 2, 3));
test_measurement.set_field<DynamicPressure>(4);
test_measurement.set_field<Accelerometer>(UKF::Vector<3>(5, 6, 7));
test_measurement.set_field<StaticPressure>(8);
EXPECT_EQ(8, test_measurement.size());
UKF::Vector<8> expected;
expected << 5, 6, 7, 1, 2, 3, 8, 4;
EXPECT_VECTOR_EQ(expected, test_measurement);
test_measurement.set_field<Gyroscope>(UKF::Vector<3>(4, 5, 6));
EXPECT_EQ(8, test_measurement.size());
EXPECT_VECTOR_EQ(UKF::Vector<3>(4, 5, 6), test_measurement.get_field<Gyroscope>());
expected << 5, 6, 7, 4, 5, 6, 8, 4;
EXPECT_VECTOR_EQ(expected, test_measurement);
test_measurement.set_field<Accelerometer>(UKF::Vector<3>(7, 8, 9));
EXPECT_EQ(8, test_measurement.size());
EXPECT_VECTOR_EQ(UKF::Vector<3>(7, 8, 9), test_measurement.get_field<Accelerometer>());
expected << 7, 8, 9, 4, 5, 6, 8, 4;
EXPECT_VECTOR_EQ(expected, test_measurement);
test_measurement.set_field<DynamicPressure>(1);
EXPECT_EQ(8, test_measurement.size());
EXPECT_EQ(1, test_measurement.get_field<DynamicPressure>());
expected << 7, 8, 9, 4, 5, 6, 8, 1;
EXPECT_VECTOR_EQ(expected, test_measurement);
test_measurement.set_field<StaticPressure>(3);
EXPECT_EQ(8, test_measurement.size());
EXPECT_EQ(3, test_measurement.get_field<StaticPressure>());
expected << 7, 8, 9, 4, 5, 6, 3, 1;
EXPECT_VECTOR_EQ(expected, test_measurement);
}
TEST(StateVectorTest, SmallAlphaSigmaPoints) {
AlternateStateVector test_state;
test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 2, 3));
test_state.set_field<Acceleration>(UKF::Vector<3>(1, 2, 3));
AlternateStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
covariance.diagonal() << 1000.0, 1000.0, 1000.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;
AlternateStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
(AlternateStateVector::covariance_size() + UKF::Parameters::Lambda<AlternateStateVector>)).llt().matrixL());
AlternateStateVector test_mean = AlternateStateVector::calculate_sigma_point_mean(sigma_points);
AlternateStateVector::SigmaPointDeltas sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points);
AlternateStateVector::CovarianceMatrix calculated_covariance = AlternateStateVector::calculate_sigma_point_covariance(sigma_point_deltas);
EXPECT_VECTOR_EQ(covariance.col(0), calculated_covariance.col(0));
EXPECT_VECTOR_EQ(covariance.col(1), calculated_covariance.col(1));
EXPECT_VECTOR_EQ(covariance.col(2), calculated_covariance.col(2));
EXPECT_VECTOR_EQ(covariance.col(3), calculated_covariance.col(3));
EXPECT_VECTOR_EQ(covariance.col(4), calculated_covariance.col(4));
EXPECT_VECTOR_EQ(covariance.col(5), calculated_covariance.col(5));
EXPECT_VECTOR_EQ(covariance.col(6), calculated_covariance.col(6));
EXPECT_VECTOR_EQ(covariance.col(7), calculated_covariance.col(7));
EXPECT_VECTOR_EQ(covariance.col(8), calculated_covariance.col(8));
EXPECT_QUATERNION_EQ(test_state.get_field<Attitude>(), test_mean.get_field<Attitude>());
EXPECT_VECTOR_EQ(test_state.get_field<AngularVelocity>(), test_mean.get_field<AngularVelocity>());
EXPECT_VECTOR_EQ(test_state.get_field<Acceleration>(), test_mean.get_field<Acceleration>());
}
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, 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));
}
