diff --git a/include/kalman/UnscentedKalmanFilterBase.hpp b/include/kalman/UnscentedKalmanFilterBase.hpp index 25e7fb2..e8bdccb 100644 --- a/include/kalman/UnscentedKalmanFilterBase.hpp +++ b/include/kalman/UnscentedKalmanFilterBase.hpp @@ -29,13 +29,13 @@ #include "MeasurementModel.hpp" namespace Kalman { - + /** * @brief Abstract Base for Unscented Kalman Filters * * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan. * Whenever "the paper" is referenced within this file then this paper is meant. - * + * * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector) */ template @@ -45,60 +45,60 @@ namespace Kalman { // Public typedefs //! Kalman Filter base type typedef KalmanFilterBase Base; - + //! Numeric Scalar Type inherited from base using typename Base::T; - + //! State Type inherited from base using typename Base::State; - + //! Measurement Model Type template class CovarianceBase> using MeasurementModelType = MeasurementModel; - + //! System Model Type template class CovarianceBase> using SystemModelType = SystemModel; - + protected: // Protected typedefs //! The number of sigma points (depending on state dimensionality) static constexpr int SigmaPointCount = 2 * State::RowsAtCompileTime + 1; - + //! Vector containg the sigma scaling weights typedef Vector SigmaWeights; - + //! Matrix type containing the sigma state or measurement points template using SigmaPoints = Matrix; protected: // Member variables - + //! State Estimate using Base::x; - + //! Sigma weights (m) SigmaWeights sigmaWeights_m; //! Sigma weights (c) SigmaWeights sigmaWeights_c; - + //! Sigma points (state) SigmaPoints sigmaStatePoints; - + // Weight parameters T alpha; //!< Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$) T beta; //!< Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian) T kappa; //!< Secondary scaling parameter (usually 0) T gamma; //!< \f$ \gamma = \sqrt{L + \lambda} \f$ with \f$ L \f$ being the state dimensionality T lambda; //!< \f$ \lambda = \alpha^2 ( L + \kappa ) - L\f$ with \f$ L \f$ being the state dimensionality - + protected: /** * Constructor - * + * * See paper for parameter explanation - * + * * @param _alpha Scaling parameter for spread of sigma points (usually 1e-4 <= alpha <= 1) * @param _beta Parameter for prior knowledge about the distribution (beta = 2 is optimal for Gaussian) * @param _kappa Secondary scaling parameter (usually 0) @@ -107,7 +107,7 @@ namespace Kalman { { // Pre-compute all weights computeWeights(); - + // Setup state and covariance x.setZero(); } @@ -124,11 +124,11 @@ namespace Kalman { { // Pass each sigma point through non-linear state transition function computeSigmaPointTransition(s, u); - + // Compute predicted state from predicted sigma points return computePredictionFromSigmaPoints(sigmaStatePoints); } - + /** * @brief Compute predicted measurement using measurement model and predicted sigma measurements * @@ -141,11 +141,11 @@ namespace Kalman { { // Predict measurements for each sigma point computeSigmaPointMeasurements(m, sigmaMeasurementPoints); - + // Predict measurement from sigma measurement points return computePredictionFromSigmaPoints(sigmaMeasurementPoints); } - + /** * @brief Compute sigma weights */ @@ -154,33 +154,33 @@ namespace Kalman { T L = T(State::RowsAtCompileTime); lambda = alpha * alpha * ( L + kappa ) - L; gamma = std::sqrt( L + lambda ); - + // Make sure L != -lambda to avoid division by zero assert( std::abs(L + lambda) > 1e-6 ); - + // Make sure L != -kappa to avoid division by zero assert( std::abs(L + kappa) > 1e-6 ); - + T W_m_0 = lambda / ( L + lambda ); T W_c_0 = W_m_0 + (T(1) - alpha*alpha + beta); - T W_i = T(1) / ( T(2) * alpha*alpha * (L + kappa) ); - + T W_i = T(1) / ( T(2) * (L + lambda) ); + // Make sure W_i > 0 to avoid square-root of negative number assert( W_i > T(0) ); - + sigmaWeights_m[0] = W_m_0; sigmaWeights_c[0] = W_c_0; - + for(int i = 1; i < SigmaPointCount; ++i) { sigmaWeights_m[i] = W_i; sigmaWeights_c[i] = W_i; } } - + /** * @brief Predict expected sigma states from current sigma states using system model and control input - * + * * @note This covers equation (18) of Algorithm 3.1 in the Paper * * @param [in] s The System Model @@ -194,10 +194,10 @@ namespace Kalman { sigmaStatePoints.col(i) = s.f( sigmaStatePoints.col(i), u ); } } - + /** * @brief Predict the expected sigma measurements from predicted sigma states using measurement model - * + * * @note This covers equation (23) of Algorithm 3.1 in the Paper * * @param [in] m The Measurement model @@ -211,10 +211,10 @@ namespace Kalman { sigmaMeasurementPoints.col(i) = m.h( sigmaStatePoints.col(i) ); } } - + /** * @brief Compute state or measurement prediciton from sigma points using pre-computed sigma weights - * + * * @note This covers equations (19) and (24) of Algorithm 3.1 in the Paper * * @param [in] sigmaPoints The computed sigma points of the desired type (state or measurement) diff --git a/test/UnscentedKalmanFilterBase.cpp b/test/UnscentedKalmanFilterBase.cpp index 74c3c3b..1970b25 100644 --- a/test/UnscentedKalmanFilterBase.cpp +++ b/test/UnscentedKalmanFilterBase.cpp @@ -14,7 +14,7 @@ class ConcreteUKF : public UnscentedKalmanFilterBase public: typedef UnscentedKalmanFilterBase Base; using typename Base::T; - + ConcreteUKF(T alpha, T beta, T kappa) : Base(alpha, beta, kappa) {} }; @@ -22,7 +22,7 @@ typedef float T; TEST(UnscentedKalmanFilterBase, init) { auto ukf = ConcreteUKF>(1,2,1); - + // x should be zero ASSERT_FLOAT_EQ(0, ukf.x[0]); ASSERT_FLOAT_EQ(0, ukf.x[1]); @@ -31,89 +31,134 @@ TEST(UnscentedKalmanFilterBase, init) { TEST(UnscentedKalmanFilterBase, computeSigmaWeights) { T alpha = 1, beta = 2, kappa = 1; - + auto ukf = ConcreteUKF>(alpha,beta,kappa); ukf.computeWeights(); - + ASSERT_FLOAT_EQ(2, ukf.gamma); ASSERT_FLOAT_EQ(1, ukf.lambda); - + ASSERT_FLOAT_EQ(0.25, ukf.sigmaWeights_m[0]); ASSERT_FLOAT_EQ(2.25, ukf.sigmaWeights_c[0]); - + for(size_t i = 1; i < 7; i++) { ASSERT_FLOAT_EQ(0.125, ukf.sigmaWeights_c[i]); ASSERT_FLOAT_EQ(0.125, ukf.sigmaWeights_m[i]); } + + // test for sum of Sigmaweights_m is always zero + // set beta=2 and kappa=0 as default + kappa =0; + beta=2; + + // StateSize 3 + for(T alpha=0.001; alpha <=1; alpha*=3) + { + auto ukf = ConcreteUKF>(alpha,beta,kappa); + ukf.computeWeights(); + T sum = 0; + for(size_t i = 0; i < 2*3+1; i++) + { + sum+= ukf.sigmaWeights_m[i]; + } + ASSERT_FLOAT_EQ(1.0, sum); + } + + // StateSize 5 + for (T alpha = 0.001; alpha <= 1; alpha *= 3) + { + auto ukf = ConcreteUKF>(alpha, beta, kappa); + ukf.computeWeights(); + T sum = 0; + for (size_t i = 0; i < 2 * 5 + 1; i++) + { + sum += ukf.sigmaWeights_m[i]; + } + ASSERT_FLOAT_EQ(1.0, sum); + } + // StateSize 9 + for (T alpha = 0.001; alpha <= 1; alpha *= 3) + { + auto ukf = ConcreteUKF>(alpha, beta, kappa); + ukf.computeWeights(); + T sum = 0; + for (size_t i = 0; i < 2 * 9 + 1; i++) + { + sum += ukf.sigmaWeights_m[i]; + } + ASSERT_FLOAT_EQ(1.0, sum); + } + + } TEST(UnscentedKalmanFilterBase, computeSigmaPointTransition) { T alpha = 1, beta = 2, kappa = 1; - + auto ukf = ConcreteUKF>(alpha,beta,kappa); auto model = Kalman::Test::Models::QuadraticSystemModel>(); - + // Init variables ukf.sigmaStatePoints << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21; - + // Control vector Vector u; u << 1, 2, 3; - + // Compute reference result Matrix ref = (ukf.sigmaStatePoints.cwiseProduct(ukf.sigmaStatePoints).colwise() + u).eval(); - + // Compute transition ukf.computeSigmaPointTransition(model, u); - + ASSERT_MATRIX_NEAR(ref, ukf.sigmaStatePoints, 1e-10); } TEST(UnscentedKalmanFilterBase, computeSigmaPointMeasurements) { T alpha = 1, beta = 2, kappa = 1; - + auto ukf = ConcreteUKF>(alpha,beta,kappa); auto model = Kalman::Test::Models::QuadraticMeasurementModel, Vector>(); - + // Init variables ukf.sigmaStatePoints << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21; - + // Compute Reference result Matrix tmp = ukf.sigmaStatePoints.template topRows<2>(); Matrix ref = tmp.cwiseProduct(tmp).eval(); - + typename ConcreteUKF>::template SigmaPoints> points; - + ukf.computeSigmaPointMeasurements(model, points); - + // Compare ref and result ASSERT_MATRIX_NEAR(ref, points, 1e-10); } TEST(UnscentedKalmanFilterBase, computePredictionFromSigmaPoints) { T alpha = 1, beta = 2, kappa = 1; - + auto ukf = ConcreteUKF>(alpha,beta,kappa); - + // Init variables ukf.sigmaWeights_m << 7, 6, 5, 4, 3, 2, 1; - + typename ConcreteUKF>::SigmaPoints> points; points << 1, 2, 3, 4, 5, 6, 7, 10, 20, 30, 40, 50, 60, 70, 100, 200, 300, 400, 500, 600, 700, 1000, 2000, 3000, 4000, 5000, 6000, 7000; - + Vector x = ukf.computePredictionFromSigmaPoints>( points ); - + ASSERT_FLOAT_EQ(84, x[0]); ASSERT_FLOAT_EQ(840, x[1]); ASSERT_FLOAT_EQ(8400, x[2]);