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