Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

WIP: Problem: Sigmaweight calculation leads to numerical issue: Sum of SigmaWeights_m <> 1 #27

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

WIP: Problem: Sigmaweight calculation leads to numerical issue: Sum of SigmaWeights_m <> 1 #27

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
19ed104
change Calculation of SigmaWeights to avoid numerical issue
rigra Sep 3, 2018
529abc4
Merge branch 'master' of https://github.com/mherb/kalman into updateU…
rigra Sep 3, 2018
c9a03c0
add unit-test for sigmasum
rigra Sep 3, 2018
bf5888b
set minimum alpha to e-3
rigra Sep 3, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
68 changes: 34 additions & 34 deletions include/kalman/UnscentedKalmanFilterBase.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<class StateType>
Expand All @@ -45,60 +45,60 @@ namespace Kalman {
// Public typedefs
//! Kalman Filter base type
typedef KalmanFilterBase<StateType> 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 Measurement, template<class> class CovarianceBase>
using MeasurementModelType = MeasurementModel<State, Measurement, CovarianceBase>;

//! System Model Type
template<class Control, template<class> class CovarianceBase>
using SystemModelType = SystemModel<State, Control, CovarianceBase>;

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<T, SigmaPointCount> SigmaWeights;

//! Matrix type containing the sigma state or measurement points
template<class Type>
using SigmaPoints = Matrix<T, Type::RowsAtCompileTime, SigmaPointCount>;

protected:
// Member variables

//! State Estimate
using Base::x;

//! Sigma weights (m)
SigmaWeights sigmaWeights_m;
//! Sigma weights (c)
SigmaWeights sigmaWeights_c;

//! Sigma points (state)
SigmaPoints<State> 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)
Expand All @@ -107,7 +107,7 @@ namespace Kalman {
{
// Pre-compute all weights
computeWeights();

// Setup state and covariance
x.setZero();
}
Expand All @@ -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<State>(sigmaStatePoints);
}

/**
* @brief Compute predicted measurement using measurement model and predicted sigma measurements
*
Expand All @@ -141,11 +141,11 @@ namespace Kalman {
{
// Predict measurements for each sigma point
computeSigmaPointMeasurements<Measurement>(m, sigmaMeasurementPoints);

// Predict measurement from sigma measurement points
return computePredictionFromSigmaPoints<Measurement>(sigmaMeasurementPoints);
}

/**
* @brief Compute sigma weights
*/
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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)
Expand Down
91 changes: 68 additions & 23 deletions test/UnscentedKalmanFilterBase.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,15 +14,15 @@ class ConcreteUKF : public UnscentedKalmanFilterBase<StateType>
public:
typedef UnscentedKalmanFilterBase<StateType> Base;
using typename Base::T;

ConcreteUKF(T alpha, T beta, T kappa) : Base(alpha, beta, kappa) {}
};

typedef float T;

TEST(UnscentedKalmanFilterBase, init) {
auto ukf = ConcreteUKF<Vector<T, 3>>(1,2,1);

// x should be zero
ASSERT_FLOAT_EQ(0, ukf.x[0]);
ASSERT_FLOAT_EQ(0, ukf.x[1]);
Expand All @@ -31,89 +31,134 @@ TEST(UnscentedKalmanFilterBase, init) {

TEST(UnscentedKalmanFilterBase, computeSigmaWeights) {
T alpha = 1, beta = 2, kappa = 1;

auto ukf = ConcreteUKF<Vector<T, 3>>(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<Vector<T, 3>>(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<Vector<T, 5>>(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<Vector<T, 9>>(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<Vector<T, 3>>(alpha,beta,kappa);
auto model = Kalman::Test::Models::QuadraticSystemModel<Vector<T, 3>>();

// 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<T,3> u;
u << 1, 2, 3;

// Compute reference result
Matrix<T,3,7> 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<Vector<T, 3>>(alpha,beta,kappa);
auto model = Kalman::Test::Models::QuadraticMeasurementModel<Vector<T, 3>, Vector<T, 2>>();

// 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<T,2,7> tmp = ukf.sigmaStatePoints.template topRows<2>();
Matrix<T,2,7> ref = tmp.cwiseProduct(tmp).eval();

typename ConcreteUKF<Vector<T,3>>::template SigmaPoints<Vector<T,2>> 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<Vector<T, 3>>(alpha,beta,kappa);

// Init variables
ukf.sigmaWeights_m << 7, 6, 5, 4, 3, 2, 1;

typename ConcreteUKF<Vector<T,3>>::SigmaPoints<Vector<T,4>> 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<T,4> x = ukf.computePredictionFromSigmaPoints<Vector<T,4>>( points );

ASSERT_FLOAT_EQ(84, x[0]);
ASSERT_FLOAT_EQ(840, x[1]);
ASSERT_FLOAT_EQ(8400, x[2]);
Expand Down