Go back to Richel Bilderbeek's homepage.

Go back to Richel Bilderbeek's C++ page.

 

 

 

 

 

(C++) KalmanFilter

 

STLQt CreatorLubuntu

 

The Kalman filter is a way to do predictions from noisy measurements. For a (far) more precise definition, see [1].

 

The tool KalmanFilterer allows one to work with Kalman filters.

 

 

 

 

Examples

 

 

 

 

 

 

References

 

  1. Wikipedia page about the Kalman filter
  2. Kalman filter examples (in Matlab)
  3. Kalman filter tutorial by Greg Czerniak

Technical facts

 

 

 

 

 

 

./CppKalmanFilter/CppKalmanFilter.pri

 

INCLUDEPATH += \
    ../../Classes/CppKalmanFilter

SOURCES += \
    ../../Classes/CppKalmanFilter/fixedlagsmootherkalmanfilter.cpp \
    ../../Classes/CppKalmanFilter/fixedlagsmootherkalmanfiltercalculationelements.cpp \
    ../../Classes/CppKalmanFilter/fixedlagsmootherkalmanfilterfactory.cpp \
    ../../Classes/CppKalmanFilter/fixedlagsmootherkalmanfilterparameters.cpp \
    ../../Classes/CppKalmanFilter/gapsfilledwhitenoisesystem.cpp \
    ../../Classes/CppKalmanFilter/gapsfilledwhitenoisesystemfactory.cpp \
    ../../Classes/CppKalmanFilter/gapsfilledwhitenoisesystemparameters.cpp \
    ../../Classes/CppKalmanFilter/kalmanfilter.cpp \
    ../../Classes/CppKalmanFilter/kalmanfiltercalculationelements.cpp \
    ../../Classes/CppKalmanFilter/kalmanfiltercalculationelementsfactory.cpp \
    ../../Classes/CppKalmanFilter/kalmanfilterexample.cpp \
    ../../Classes/CppKalmanFilter/kalmanfilterexperiment.cpp \
    ../../Classes/CppKalmanFilter/kalmanfilterexperimentparameter.cpp \
    ../../Classes/CppKalmanFilter/kalmanfilterexperimentparametertype.cpp \
    ../../Classes/CppKalmanFilter/kalmanfilterfactory.cpp \
    ../../Classes/CppKalmanFilter/kalmanfilterparameter.cpp \
    ../../Classes/CppKalmanFilter/kalmanfilterparameters.cpp \
    ../../Classes/CppKalmanFilter/kalmanfilterparametertype.cpp \
    ../../Classes/CppKalmanFilter/kalmanfiltertype.cpp \
    ../../Classes/CppKalmanFilter/kalmanfiltertypes.cpp \
    ../../Classes/CppKalmanFilter/laggedwhitenoisesystem.cpp \
    ../../Classes/CppKalmanFilter/laggedwhitenoisesystemfactory.cpp \
    ../../Classes/CppKalmanFilter/laggedwhitenoisesystemparameters.cpp \
    ../../Classes/CppKalmanFilter/standardkalmanfilter.cpp \
    ../../Classes/CppKalmanFilter/standardkalmanfiltercalculationelements.cpp \
    ../../Classes/CppKalmanFilter/standardkalmanfilterfactory.cpp \
    ../../Classes/CppKalmanFilter/standardkalmanfilterparameters.cpp \
    ../../Classes/CppKalmanFilter/standardwhitenoisesystem.cpp \
    ../../Classes/CppKalmanFilter/standardwhitenoisesystemfactory.cpp \
    ../../Classes/CppKalmanFilter/standardwhitenoisesystemparameters.cpp \
    ../../Classes/CppKalmanFilter/steadystatekalmanfilter.cpp \
    ../../Classes/CppKalmanFilter/steadystatekalmanfiltercalculationelements.cpp \
    ../../Classes/CppKalmanFilter/steadystatekalmanfilterfactory.cpp \
    ../../Classes/CppKalmanFilter/steadystatekalmanfilterparameters.cpp \
    ../../Classes/CppKalmanFilter/whitenoisesystem.cpp \
    ../../Classes/CppKalmanFilter/whitenoisesystemfactory.cpp \
    ../../Classes/CppKalmanFilter/whitenoisesystemparameter.cpp \
    ../../Classes/CppKalmanFilter/whitenoisesystemparameters.cpp \
    ../../Classes/CppKalmanFilter/whitenoisesystemparametertype.cpp \
    ../../Classes/CppKalmanFilter/whitenoisesystemtype.cpp \
    ../../Classes/CppKalmanFilter/whitenoisesystemtypes.cpp

HEADERS  += \
    ../../Classes/CppKalmanFilter/fixedlagsmootherkalmanfilter.h \
    ../../Classes/CppKalmanFilter/fixedlagsmootherkalmanfiltercalculationelements.h \
    ../../Classes/CppKalmanFilter/fixedlagsmootherkalmanfilterfactory.h \
    ../../Classes/CppKalmanFilter/fixedlagsmootherkalmanfilterparameters.h \
    ../../Classes/CppKalmanFilter/gapsfilledwhitenoisesystem.h \
    ../../Classes/CppKalmanFilter/gapsfilledwhitenoisesystemfactory.h \
    ../../Classes/CppKalmanFilter/gapsfilledwhitenoisesystemparameters.h \
    ../../Classes/CppKalmanFilter/kalmanfilter.h \
    ../../Classes/CppKalmanFilter/kalmanfiltercalculationelements.h \
    ../../Classes/CppKalmanFilter/kalmanfiltercalculationelementsfactory.h \
    ../../Classes/CppKalmanFilter/kalmanfilterexample.h \
    ../../Classes/CppKalmanFilter/kalmanfilterexperiment.h \
    ../../Classes/CppKalmanFilter/kalmanfilterexperimentparameter.h \
    ../../Classes/CppKalmanFilter/kalmanfilterexperimentparametertype.h \
    ../../Classes/CppKalmanFilter/kalmanfilterfactory.h \
    ../../Classes/CppKalmanFilter/kalmanfilterparameter.h \
    ../../Classes/CppKalmanFilter/kalmanfilterparameters.h \
    ../../Classes/CppKalmanFilter/kalmanfilterparametertype.h \
    ../../Classes/CppKalmanFilter/kalmanfiltertype.h \
    ../../Classes/CppKalmanFilter/kalmanfiltertypes.h \
    ../../Classes/CppKalmanFilter/laggedwhitenoisesystem.h \
    ../../Classes/CppKalmanFilter/laggedwhitenoisesystemfactory.h \
    ../../Classes/CppKalmanFilter/laggedwhitenoisesystemparameters.h \
    ../../Classes/CppKalmanFilter/standardkalmanfilter.h \
    ../../Classes/CppKalmanFilter/standardkalmanfiltercalculationelements.h \
    ../../Classes/CppKalmanFilter/standardkalmanfilterfactory.h \
    ../../Classes/CppKalmanFilter/standardkalmanfilterparameters.h \
    ../../Classes/CppKalmanFilter/standardwhitenoisesystem.h \
    ../../Classes/CppKalmanFilter/standardwhitenoisesystemfactory.h \
    ../../Classes/CppKalmanFilter/standardwhitenoisesystemparameters.h \
    ../../Classes/CppKalmanFilter/steadystatekalmanfilter.h \
    ../../Classes/CppKalmanFilter/steadystatekalmanfiltercalculationelements.h \
    ../../Classes/CppKalmanFilter/steadystatekalmanfilterfactory.h \
    ../../Classes/CppKalmanFilter/steadystatekalmanfilterparameters.h \
    ../../Classes/CppKalmanFilter/whitenoisesystem.h \
    ../../Classes/CppKalmanFilter/whitenoisesystemfactory.h \
    ../../Classes/CppKalmanFilter/whitenoisesystemparameter.h \
    ../../Classes/CppKalmanFilter/whitenoisesystemparameters.h \
    ../../Classes/CppKalmanFilter/whitenoisesystemparametertype.h \
    ../../Classes/CppKalmanFilter/whitenoisesystemtype.h \
    ../../Classes/CppKalmanFilter/whitenoisesystemtypes.h

OTHER_FILES += \
    ../../Classes/CppKalmanFilter/Licence.txt

 

 

 

 

 

./CppKalmanFilter/fixedlagsmootherkalmanfilter.h

 

#ifndef FIXEDLAGSMOOTHERKALMANFILTER_H
#define FIXEDLAGSMOOTHERKALMANFILTER_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "kalmanfilter.h"
#include "standardkalmanfilter.h"
#include "standardkalmanfilterparameters.h"
#include "fixedlagsmootherkalmanfilterparameters.h"
#include "fixedlagsmootherkalmanfiltercalculationelements.h"
#pragma GCC diagnostic pop

namespace ribi {
namespace kalman {

struct FixedLagSmootherKalmanFilter : public KalmanFilter
{
  ///The augmented estimation error covariances, that is the estimation error covariance for each lag timestep
  ///Use boost::numeric::ublas::matrix<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///   Complex        Simplified
  /// [ [ A B ] ]
  /// [ [ C D ] ]       [ A B ]
  /// [         ]       [ C D ]
  /// [ [ E F ] ]       [ E F ]
  /// [ [ G H ] ]   ->  [ G H ]
  //const boost::numeric::ublas::matrix<double>& GetEstimationErrorCovariances() const { return m_estimation_error_covariances; }

  ///Get the Kalman filter last calculation elements
  boost::shared_ptr<KalmanFilterCalculationElements> GetLastCalculation() const
  {
    return m_last_calculation;
  }

  ///The last augmented Kalman gains, that is the gain for each lag timestep
  ///Use boost::numeric::ublas::matrix<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///   Complex        Simplified
  /// [ [ A B ] ]
  /// [ [ C D ] ]       [ A B ]
  /// [         ]       [ C D ]
  /// [ [ E F ] ]       [ E F ]
  /// [ [ G H ] ]   ->  [ G H ]
  //const boost::numeric::ublas::matrix<double>& GetLastGains() const { return m_kalman_gains; }

  ///The last predicted augmented estimation error covariances,
  ///that is the estimation error covariance for each lag timestep
  ///Use boost::numeric::ublas::matrix<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///   Complex        Simplified
  /// [ [ A B ] ]
  /// [ [ C D ] ]       [ A B ]
  /// [         ]       [ C D ]
  /// [ [ E F ] ]       [ E F ]
  /// [ [ G H ] ]   ->  [ G H ]
  //const boost::numeric::ublas::matrix<double>& GetLastPredictedCovariances() const { return m_last_predicted_covariances; }

  ///The last predicted augmented states
  ///Use boost::numeric::ublas::vector<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///  Complex       Simplified
  /// [ [ A ] ]
  /// [ [ B ] ]        [ A ]
  /// [       ]        [ B ]
  /// [ [ C ] ]        [ C ]
  /// [ [ D ] ]   ->   [ D ]
  //const boost::numeric::ublas::vector<double>& GetLastPredictedStates() const { return m_last_predicted_states; }

  ///Obtain the Kalman filter parameters
  boost::shared_ptr<const KalmanFilterParameters> GetParameters() const
  {
    return m_parameters;
  }

  ///The augmented states, that is the state for each lag timestep
  ///Use boost::numeric::ublas::vector<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///  Complex       Simplified
  /// [ [ A ] ]
  /// [ [ B ] ]        [ A ]
  /// [       ]        [ B ]
  /// [ [ C ] ]        [ C ]
  /// [ [ D ] ]   ->   [ D ]
  const boost::numeric::ublas::vector<double>& GetStateEstimates() const { return m_state_estimates; }

  ///Obtain the Kalman filter type as an enum
  KalmanFilterType GetType() const { return KalmanFilterType::fixed_lag_smoother; }

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Let the filter predict
  boost::numeric::ublas::vector<double> PredictState(
    const boost::numeric::ublas::vector<double>& input) const;

  ///Give the filter a measurement and input, and it will update its predictions
  void SupplyMeasurementAndInput(
    const boost::numeric::ublas::vector<double>& x,
    const boost::numeric::ublas::vector<double>& input);

  private:
  ///Only a FixedLagSmootherKalmanFilterFactory can create a FixedLagSmootherKalmanFilter
  explicit FixedLagSmootherKalmanFilter(
    const boost::shared_ptr<FixedLagSmootherKalmanFilterCalculationElements>& calculation,
    const boost::shared_ptr<const KalmanFilterParameters>& parameters);
  friend class FixedLagSmootherKalmanFilterFactory;

  ///Can only be deleted by boost::checked_delete
  ~FixedLagSmootherKalmanFilter() noexcept {}
  friend void boost::checked_delete<>(FixedLagSmootherKalmanFilter*);
  friend void boost::checked_delete<>(const FixedLagSmootherKalmanFilter*);

  ///The Kalman filter last calculation elements
  const boost::shared_ptr<KalmanFilterCalculationElements> m_last_calculation;

  ///The (downcasted) calculation
  const boost::shared_ptr<FixedLagSmootherKalmanFilterCalculationElements> m_last_fixed_lag_smoother_calculation;

  ///The standard (unlagged) Kalman filter used
  const boost::shared_ptr<StandardKalmanFilter> m_standard_filter;

  ///The augmented estimation error covariances, that is the estimation error covariance for each lag timestep
  ///Use boost::numeric::ublas::matrix<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///   Complex        Simplified
  /// [ [ A B ] ]
  /// [ [ C D ] ]       [ A B ]
  /// [         ]       [ C D ]
  /// [ [ E F ] ]       [ E F ]
  /// [ [ G H ] ]   ->  [ G H ]
  //boost::numeric::ublas::matrix<double> m_estimation_error_covariances;

  ///The augmented Kalman gains, that is the gain for each lag timestep
  ///Use boost::numeric::ublas::matrix<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///   Complex        Simplified
  /// [ [ A B ] ]
  /// [ [ C D ] ]       [ A B ]
  /// [         ]       [ C D ]
  /// [ [ E F ] ]       [ E F ]
  /// [ [ G H ] ]   ->  [ G H ]
  //boost::numeric::ublas::matrix<double> m_kalman_gains;

  ///The augmented Kalman gains, that is the gain for each lag timestep
  ///Use boost::numeric::ublas::matrix<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///   Complex        Simplified
  /// [ [ A B ] ]
  /// [ [ C D ] ]       [ A B ]
  /// [         ]       [ C D ]
  /// [ [ E F ] ]       [ E F ]
  /// [ [ G H ] ]   ->  [ G H ]
  //boost::numeric::ublas::matrix<double> m_last_kalman_gains;

  ///The last predicted augmented estimation error covariances,
  ///that is the estimation error covariance for each lag timestep
  ///Use boost::numeric::ublas::matrix<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///   Complex        Simplified
  /// [ [ A B ] ]
  /// [ [ C D ] ]       [ A B ]
  /// [         ]       [ C D ]
  /// [ [ E F ] ]       [ E F ]
  /// [ [ G H ] ]   ->  [ G H ]
  //boost::numeric::ublas::matrix<double> m_last_predicted_covariances;

  ///The last predicted augmented states
  ///Use boost::numeric::ublas::vector<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///  Complex       Simplified
  /// [ [ A ] ]
  /// [ [ B ] ]        [ A ]
  /// [       ]        [ B ]
  /// [ [ C ] ]        [ C ]
  /// [ [ D ] ]   ->   [ D ]
  //boost::numeric::ublas::vector<double> m_last_predicted_states;

  ///The downcasted parameters
  const boost::shared_ptr<const FixedLagSmootherKalmanFilterParameters> m_parameters;

  ///The augmented states, that is the state for each lag timestep
  ///Use boost::numeric::ublas::vector<double> instead of
  ///std::vector<boost::numeric::ublas::vector<double> > to allow for matrix operation
  ///  Complex       Simplified
  /// [ [ A ] ]
  /// [ [ B ] ]        [ A ]
  /// [       ]        [ B ]
  /// [ [ C ] ]        [ C ]
  /// [ [ D ] ]   ->   [ D ]
  boost::numeric::ublas::vector<double> m_state_estimates;

  static boost::numeric::ublas::vector<boost::numeric::ublas::vector<double>> CreateComplexInitialStates(
    const boost::shared_ptr<const FixedLagSmootherKalmanFilterParameters>& parameters);

  static boost::numeric::ublas::vector<boost::numeric::ublas::matrix<double>> CreateInitialGains(
    const int lag,
    const StandardKalmanFilter& filter);

  static boost::numeric::ublas::vector<double> CreateInitialStates(
    const boost::shared_ptr<const FixedLagSmootherKalmanFilterParameters>& parameters);

  ///         [ I ]
  ///         [ 0 ]
  ///term_a = [ 0 ], where I denotes an identity matrix with size input.size() x input.size()
  ///                and where 0 denotes a null matrix with size input.size() x input.size()
  ///The length of term_a is equal to the lag (3 in this example)
  static boost::numeric::ublas::vector<boost::numeric::ublas::matrix<double>> CreateComplexTermA(
    const int lag,
    const int state_size);

  ///         [ 0  0  0]
  ///         [ I  0  0]
  ///term_b = [ 0  I  0], where I denotes an identity matrix with size input.size() x input.size()
  ///                     and where 0 denotes a null matrix with size input.size() x input.size()
  ///The size of term_b is lag x lag (3 in this example)
  static boost::numeric::ublas::matrix<boost::numeric::ublas::matrix<double>> CreateComplexTermB(
    const int lag,
    const int state_size);

  ///                  [ 1 0 ]
  ///                  [ 0 1 ]
  ///                  [ 0 0 ]
  ///         [ I ]    [ 0 0 ]
  ///         [ 0 ]    [ 0 0 ]
  ///term_a = [ 0 ] => [ 0 0 ],
  ///  where I denotes an identity matrix with size input.size() x input.size()
  ///  and where 0 denotes a null matrix with size input.size() x input.size()
  ///The size of term_a will be:
  /// - number of rows   : lag (3) x input.size() (2) = 6
  /// - number of columns:           input.size() (2) = 2
  static boost::numeric::ublas::matrix<double> CreateTermA(
    const int lag,
    const int state_size);

  ///                       [ 0 0 0 0 0 0 ]
  ///                       [ 0 0 0 0 0 0 ]
  ///                       [ 1 0 0 0 0 0 ]
  ///         [ 0  0  0]    [ 0 1 0 0 0 0 ]
  ///         [ I  0  0]    [ 0 0 1 0 0 0 ]
  ///term_b = [ 0  I  0] => [ 0 0 0 1 0 0 ]
  ///where I denotes an identity matrix with size input.size() x input.size()
  ///                     and where 0 denotes a null matrix with size input.size() x input.size()
  ///The size of term_b is lag x lag (3 in this example)
  ///The size of term_b will be:
  /// - number of rows   : lag (3) x input.size() (2) = 6
  /// - number of columns: lag (3) x input.size() (2) = 6
  static boost::numeric::ublas::matrix<double> CreateTermB(
    const int lag,
    const int state_size);

  static boost::shared_ptr<const FixedLagSmootherKalmanFilterParameters> DownCast(
    const boost::shared_ptr<const KalmanFilterParameters>& parameters);

  ///Obtain the (unlagged) Kalman filter
  boost::shared_ptr<const StandardKalmanFilter> GetStandardKalmanFilter() const { return m_standard_filter; }

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // FIXEDLAGSMOOTHERKALMANFILTER_H

 

 

 

 

 

./CppKalmanFilter/fixedlagsmootherkalmanfilter.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "fixedlagsmootherkalmanfilter.h"

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

#include "matrix.h"
#include "testtimer.h"
#include "trace.h"
#include "standardkalmanfilterfactory.h"
#pragma GCC diagnostic pop

ribi::kalman::FixedLagSmootherKalmanFilter::FixedLagSmootherKalmanFilter(
  const boost::shared_ptr<FixedLagSmootherKalmanFilterCalculationElements>& calculation,
  const boost::shared_ptr<const KalmanFilterParameters>& parameters)
  : KalmanFilter{},
    m_last_calculation{calculation},
    m_last_fixed_lag_smoother_calculation{boost::dynamic_pointer_cast<FixedLagSmootherKalmanFilterCalculationElements>(calculation)},
    m_standard_filter{StandardKalmanFilterFactory().Create(DownCast(parameters)->GetStandardParameters())},
    m_parameters{DownCast(parameters)},
    m_state_estimates{CreateInitialStates(DownCast(parameters))}
{
  #ifndef NDEBUG
  Test();
  #endif
  assert(m_last_fixed_lag_smoother_calculation);
  assert(m_parameters);
  assert(boost::numeric_cast<int>(m_state_estimates.size()) == m_parameters->GetLag() * m_standard_filter->GetStateSize());
}


boost::numeric::ublas::vector<boost::numeric::ublas::matrix<double> > ribi::kalman::FixedLagSmootherKalmanFilter::CreateInitialGains(
  const int lag,
  const StandardKalmanFilter& filter)
{

  assert(filter.GetStateSize() == boost::numeric_cast<int>(filter.GetLastStandardCalculation()->GetKalmanGain().size1()));
  assert(filter.GetLastStandardCalculation()->GetKalmanGain().size1() == filter.GetLastStandardCalculation()->GetKalmanGain().size2());
  boost::numeric::ublas::vector<boost::numeric::ublas::matrix<double> > v(lag,filter.GetLastStandardCalculation()->GetKalmanGain());
  return v;
}

boost::numeric::ublas::vector<boost::numeric::ublas::vector<double> > ribi::kalman::FixedLagSmootherKalmanFilter::CreateComplexInitialStates(
  const boost::shared_ptr<const FixedLagSmootherKalmanFilterParameters>& parameters)
{
  assert(parameters);
  boost::numeric::ublas::vector<boost::numeric::ublas::vector<double> > v(
    parameters->GetLag(),
    parameters->GetStandardParameters()->GetInitialStateEstimate());
  return v;
}

boost::numeric::ublas::vector<boost::numeric::ublas::matrix<double> >
  ribi::kalman::FixedLagSmootherKalmanFilter::CreateComplexTermA(
  const int lag,
  const int state_size)
{
  //
  assert(lag > 0 && "Term A is not needed for a lag of zero");
  boost::numeric::ublas::vector<boost::numeric::ublas::matrix<double> > v(lag);

  v[0] = boost::numeric::ublas::identity_matrix<double>(state_size);
  for (int i=1; i!=lag; ++i)
  {
    assert(i < boost::numeric_cast<int>(v.size()));
    v[i] = boost::numeric::ublas::zero_matrix<double>(state_size);
  }
  return v;
}

boost::numeric::ublas::matrix<double>
  ribi::kalman::FixedLagSmootherKalmanFilter::CreateTermA(
  const int lag,
  const int state_size)
{
  //
  assert(lag > 0 && "Term A is not needed for a lag of zero");
  const boost::numeric::ublas::matrix<double> v
    = Matrix::SimplifyVectorOfMatrix(CreateComplexTermA(lag,state_size));
  assert(lag * state_size == boost::numeric_cast<int>(v.size1()));
  assert(  1 * state_size == boost::numeric_cast<int>(v.size2()));
  return v;
}

boost::numeric::ublas::matrix<double >
  ribi::kalman::FixedLagSmootherKalmanFilter::CreateTermB(
    const int lag,
    const int state_size)
{
  //
  boost::numeric::ublas::matrix<double> v
    = Matrix::SimplifyMatrixOfMatrix(CreateComplexTermB(lag,state_size));
  //
  assert(lag > 0 && "Term B is not needed for a lag of zero");
  assert(lag > 1 && "Term B cannot be calculated for a lag of one");
  assert((lag - 0) * state_size == boost::numeric_cast<int>(v.size1()));
  assert((lag - 1) * state_size == boost::numeric_cast<int>(v.size2()));
  return v;
}


boost::numeric::ublas::matrix<boost::numeric::ublas::matrix<double> >
  ribi::kalman::FixedLagSmootherKalmanFilter::CreateComplexTermB(
    const int lag,
    const int state_size)
{
  //
  assert(lag > 0 && "Term B is not needed for a lag of zero");
  const int n_cols = lag - 1;
  const int n_rows = lag;
  boost::numeric::ublas::matrix<boost::numeric::ublas::matrix<double> > v(n_rows,n_cols);
  for (int y=0; y!=n_rows; ++y)
  {
    for (int x=0; x!=n_cols; ++x)
    {
      if (y - 1 == x)
      {
        assert(y < boost::numeric_cast<int>(v.size1()));
        assert(x < boost::numeric_cast<int>(v.size2()));
        v(y,x) = boost::numeric::ublas::identity_matrix<double>(state_size);
      }
      else
      {
        assert(y < boost::numeric_cast<int>(v.size1()));
        assert(x < boost::numeric_cast<int>(v.size2()));
        v(y,x) = boost::numeric::ublas::zero_matrix<double>(state_size);
      }
    }
  }
  return v;
}

boost::numeric::ublas::vector<double> ribi::kalman::FixedLagSmootherKalmanFilter::CreateInitialStates(
  const boost::shared_ptr<const FixedLagSmootherKalmanFilterParameters>& parameters)
{
  assert(parameters);
  //
  //if (lag == 0) return boost::numeric::ublas::vector<double>();
  boost::numeric::ublas::vector<double> v = Matrix::SimplifyVectorOfVector(
    CreateComplexInitialStates(parameters));
  return v;
}

boost::shared_ptr<const ribi::kalman::FixedLagSmootherKalmanFilterParameters> ribi::kalman::FixedLagSmootherKalmanFilter::DownCast(
  const boost::shared_ptr<const KalmanFilterParameters>& parameters)
{
  #ifndef NDEBUG
  const auto i = parameters.use_count();
  #endif

  const boost::shared_ptr<const FixedLagSmootherKalmanFilterParameters> p
    = boost::dynamic_pointer_cast<const FixedLagSmootherKalmanFilterParameters>(parameters);
  assert(p);

  #ifndef NDEBUG
  const auto j = parameters.use_count();
  assert(i + 1 == j);
  #endif
  return p;
}


std::string ribi::kalman::FixedLagSmootherKalmanFilter::GetVersion() noexcept
{
  return "1.0";
}

std::vector<std::string> ribi::kalman::FixedLagSmootherKalmanFilter::GetVersionHistory() noexcept
{
  return {
    "2013-05-03: version 1.0: initial version"
  };
}

boost::numeric::ublas::vector<double> ribi::kalman::FixedLagSmootherKalmanFilter::PredictState(
  const boost::numeric::ublas::vector<double>& input) const
{
  //
  using boost::numeric::ublas::range;
  using boost::numeric::ublas::vector;
  using boost::numeric::ublas::vector_range;

  if (m_parameters->GetLag() == 0) return m_standard_filter->PredictState(input);
  assert(!m_state_estimates.empty());
  const std::size_t state_size = m_standard_filter->GetStateSize();
  assert(state_size <= m_state_estimates.size());
  const vector_range<const vector<double> > state(m_state_estimates,range(0,state_size));
  return vector<double>(state);
}


void ribi::kalman::FixedLagSmootherKalmanFilter::SupplyMeasurementAndInput(
  const boost::numeric::ublas::vector<double>& x,
  const boost::numeric::ublas::vector<double>& input)
{
  
  using boost::numeric::ublas::vector;
  using boost::numeric::ublas::vector_range;
  using boost::numeric::ublas::range;
  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::trans;

  //Store calculation for KalmanFilterExperiment
  m_last_fixed_lag_smoother_calculation->Clear();
  m_last_fixed_lag_smoother_calculation->SetPreviousStateEstimate(this->GetStandardKalmanFilter()->GetStateEstimate()); //1
  //m_last_fixed_lag_smoother_calculation->SetPreviousCovarianceEstimate(this->GetEstimationErrorCovariance()); //2

  const int state_size = m_standard_filter->GetStateSize();
  assert(state_size == boost::numeric_cast<int>(x.size()));
  assert(state_size == boost::numeric_cast<int>(input.size()));
  m_standard_filter->SupplyMeasurementAndInput(x,input);
  if (m_parameters->GetLag() == 0) return;
  const matrix<double> term_a = CreateTermA(m_parameters->GetLag(),state_size);
  const int lag = m_parameters->GetLag();
  assert(lag * state_size == boost::numeric_cast<int>(term_a.size1()));
  assert(  1 * state_size == boost::numeric_cast<int>(term_a.size2()));

  //Get the naive (that is, based on no time lag) prediction
  const vector<double> x_naive = m_standard_filter->PredictState(input);
  const matrix<double> term_b = CreateTermB(lag,state_size);

  //Find P(i) (yes, i can be zero and goes to lag)
  //ps has length lag
  //P(i) has size state_size x state_size
  //P(i) = P . [ [F-KH]^T ]^i (where ^T denotes a transposition, where ^i denotes an exponent to the power of i
  TRACE(m_standard_filter->GetLastStandardCalculation()->GetPreviousCovarianceEstimate());
  TRACE(m_standard_filter->GetLastStandardCalculation()->GetKalmanGain());
  vector<matrix<double> > ps_complex(lag);
  for (int i=0; i!=lag; ++i)
  {
    //
    assert(i < boost::numeric_cast<int>(ps_complex.size()));
    ps_complex[i]
      = Matrix::Prod(
          m_standard_filter->GetEstimationErrorCovariance(),
          Matrix::Power(
            trans(
              m_standard_filter->GetParameters()->GetStateTransition()
                - Matrix::Prod(
                  m_standard_filter->GetLastStandardCalculation()->GetKalmanGain(),
                  m_standard_filter->GetParameters()->GetObservation()
                )
              )
            ,
            i)
        );
  }

  //Find K(i) (yes, i can be zero and goes to lag)
  //ks has length lag
  //K(i) has size state_size x state_size
  //K(i) = P(i) . H^T . [H.P.H^T + R]^-1
  //
  vector<matrix<double> > ks_complex(lag);
  for (int i=0; i!=lag; ++i)
  {
    const boost::numeric::ublas::matrix<double> term
      = Matrix::MultiProd(
          m_standard_filter->GetParameters()->GetObservation(),
          m_standard_filter->GetEstimationErrorCovariance(),
          trans(m_standard_filter->GetParameters()->GetObservation())
        )
        + m_standard_filter->GetStandardParameters()->GetEstimatedMeasurementNoise();
    if (Matrix::CalcDeterminant(term) == 0.0)
    {
      throw std::runtime_error("Determinant of term in K(i) equals zero");
    }
    assert(i < boost::numeric_cast<int>(ks_complex.size()));
    assert(i < boost::numeric_cast<int>(ps_complex.size()));
    //
    ks_complex[i]
      = Matrix::MultiProd(
          ps_complex[i],
          trans(m_standard_filter->GetParameters()->GetObservation()),
          Matrix::Inverse(term)
        );
  }
  //
  matrix<double> ks = Matrix::SimplifyVectorOfMatrix(ks_complex);
  //
  const vector<double> innovation = m_standard_filter->GetLastStandardCalculation()->GetInnovation();
  //

  const vector<double> new_states_term_a
    = Matrix::Prod(term_a,x_naive);
  const vector<double> new_states_term_b
    = Matrix::Prod(term_b,vector_range<const vector<double> >(m_state_estimates,range(0,m_state_estimates.size()- m_standard_filter->GetStateSize())));
  //const vector<double> new_states_term_b
  //  = Matrix::Prod(term_b,vector_range<const vector<double> >(m_states,range(m_standard_filter->GetStateSize(),m_states.size())))
  const vector<double> new_states_term_c
    = Matrix::Prod(ks,innovation);

  m_state_estimates
    = new_states_term_a
    + new_states_term_b
    + new_states_term_c;

  TRACE("Store the calculation hiero");
  m_last_fixed_lag_smoother_calculation->SetStandardCalculationElement(this->m_standard_filter->GetLastStandardCalculation());
}

#ifndef NDEBUG
void ribi::kalman::FixedLagSmootherKalmanFilter::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
  try
  {
    boost::numeric_cast<std::size_t>(-1);
    assert(!"Line above must fail, so shouldn't get here");
  }
  catch (boost::numeric::bad_numeric_cast& e)
  {
    //OK!
  }

  //CreateTermA
  {
    const int lag = 13;
    const int state_size = 17;
    const boost::numeric::ublas::matrix<double> v = CreateTermA(lag,state_size);
    const int n_rows = lag * state_size;
    const int n_cols =   1 * state_size;
    assert(boost::numeric_cast<int>(v.size1()) == n_rows);
    assert(boost::numeric_cast<int>(v.size2()) == n_cols);
    for (int row = 0; row!=n_rows; ++row)
    {
      assert(row < boost::numeric_cast<int>(v.size1()));
      for (int col = 0; col!=n_cols; ++col)
      {
        assert(col < boost::numeric_cast<int>(v.size2()));
        const double expected = row == col ? 1.0 : 0.0;
        assert(Matrix::IsAboutEqual(v(row,col),expected));
      }
    }
  }
  //CreateTermB
  {
    const int lag = 3;
    const int state_size = 5;
    const boost::numeric::ublas::matrix<double> v = CreateTermB(lag,state_size);
    const int n_rows = lag * state_size;
    const int n_cols = (lag - 1) * state_size;
    assert(boost::numeric_cast<int>(v.size1()) == n_rows);
    assert(boost::numeric_cast<int>(v.size2()) == n_cols);
    for (int row = 0; row!=n_rows; ++row)
    {
      assert(row < boost::numeric_cast<int>(v.size1()));
      for (int col = 0; col!=n_cols; ++col)
      {
        assert(col < boost::numeric_cast<int>(v.size2()));
        const double expected = row - state_size == col ? 1.0 : 0.0;
        assert(Matrix::IsAboutEqual(v(row,col),expected));
      }
    }
  }
}
#endif

 

 

 

 

 

./CppKalmanFilter/fixedlagsmootherkalmanfiltercalculationelements.h

 

#ifndef FIXEDLAGSMOOTHERKALMANFILTERCALCULATIONELEMENTS_H
#define FIXEDLAGSMOOTHERKALMANFILTERCALCULATIONELEMENTS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include "kalmanfiltercalculationelements.h"
#pragma GCC diagnostic pop

namespace ribi {
namespace kalman {

struct StandardKalmanFilterCalculationElements;

struct FixedLagSmootherKalmanFilterCalculationElements : public KalmanFilterCalculationElements
{
  explicit FixedLagSmootherKalmanFilterCalculationElements(
    const boost::numeric::ublas::vector<double>& measurement = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::vector<double>& predicted_state = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::vector<double>& previous_state_estimate = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::vector<double>& updated_state = boost::numeric::ublas::vector<double>());

  ///Clear the calculation, will set IsComplete to false
  void Clear();

  ///Produce a deep copy of the derived class
  boost::shared_ptr<KalmanFilterCalculationElements> Clone() const;

  ///Obtain the Kalman filter type as an enum
  KalmanFilterType GetType() const { return KalmanFilterType::fixed_lag_smoother; }

  ///Checks if the state is complete and valid
  bool IsComplete() const;

  void SetStandardCalculationElement(
    const boost::shared_ptr<StandardKalmanFilterCalculationElements>& standard_calculation);

  private:
  boost::shared_ptr<const StandardKalmanFilterCalculationElements> m_standard_calculation;

  ///Can only be deleted by boost::checked_delete
  ~FixedLagSmootherKalmanFilterCalculationElements() noexcept {}
  friend void boost::checked_delete<>(FixedLagSmootherKalmanFilterCalculationElements*);

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // FIXEDLAGSMOOTHERKALMANFILTERCALCULATIONELEMENTS_H

 

 

 

 

 

./CppKalmanFilter/fixedlagsmootherkalmanfiltercalculationelements.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "fixedlagsmootherkalmanfiltercalculationelements.h"
#pragma GCC diagnostic pop

#include <cassert>

#include "standardkalmanfiltercalculationelements.h"

ribi::kalman::FixedLagSmootherKalmanFilterCalculationElements::FixedLagSmootherKalmanFilterCalculationElements(
  const boost::numeric::ublas::vector<double>& measurement,
  const boost::numeric::ublas::vector<double>& predicted_state,
  const boost::numeric::ublas::vector<double>& previous_state_estimate,
  const boost::numeric::ublas::vector<double>& updated_state)
  : KalmanFilterCalculationElements(
      measurement,
      predicted_state,
      previous_state_estimate,
      updated_state
    ),
    m_standard_calculation{}
{
  //... nothing to check left
}

void ribi::kalman::FixedLagSmootherKalmanFilterCalculationElements::Clear()
{
  KalmanFilterCalculationElements::Clear();

  //Others, e.g.
  //m_updated_covariance = boost::numeric::ublas::matrix<double>();
}

boost::shared_ptr<ribi::kalman::KalmanFilterCalculationElements> ribi::kalman::FixedLagSmootherKalmanFilterCalculationElements::Clone() const
{
  boost::shared_ptr<KalmanFilterCalculationElements> p;
  assert(p);
  return p;
}


bool ribi::kalman::FixedLagSmootherKalmanFilterCalculationElements::IsComplete() const
{
  const std::size_t sz = GetMeasurement().size();
  return
       sz != 0
    //Others, e.g.
    //&& sz == m_innovation.size()
    //&& sz == m_innovation_covariance.size1()
    //&& sz == m_innovation_covariance.size2()
    //&& sz == m_innovation_covariance.size1()
    && sz == GetMeasurement().size()
    && sz == GetPredictedState().size()
    && sz == GetPreviousState().size();
}

void ribi::kalman::FixedLagSmootherKalmanFilterCalculationElements::SetStandardCalculationElement(
  const boost::shared_ptr<StandardKalmanFilterCalculationElements>& standard_calculation)
{
  assert(!m_standard_calculation);
  assert(standard_calculation);
  m_standard_calculation = standard_calculation;
  assert(m_standard_calculation);
}

 

 

 

 

 

./CppKalmanFilter/fixedlagsmootherkalmanfilterfactory.h

 

#ifndef FIXEDLAGSMOOTHERKALMANFILTERFACTORY_H
#define FIXEDLAGSMOOTHERKALMANFILTERFACTORY_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include "fixedlagsmootherkalmanfilter.h"
#pragma GCC diagnostic pop

namespace ribi {
namespace kalman {

///Factory for FixedLagSmootherKalmanFilter
struct FixedLagSmootherKalmanFilterFactory
{
  FixedLagSmootherKalmanFilterFactory();

  boost::shared_ptr<FixedLagSmootherKalmanFilter> Create(
    const boost::shared_ptr<const KalmanFilterParameters>& parameters
  ) const noexcept;

  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // FIXEDLAGSMOOTHERKALMANFILTERFACTORY_H

 

 

 

 

 

./CppKalmanFilter/fixedlagsmootherkalmanfilterfactory.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "fixedlagsmootherkalmanfilterfactory.h"

#include <cassert>

#include "testtimer.h"
#pragma GCC diagnostic pop

ribi::kalman::FixedLagSmootherKalmanFilterFactory::FixedLagSmootherKalmanFilterFactory()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::shared_ptr<ribi::kalman::FixedLagSmootherKalmanFilter> ribi::kalman::FixedLagSmootherKalmanFilterFactory::Create(
  const boost::shared_ptr<const KalmanFilterParameters>& parameters) const noexcept
{
  assert(parameters);
  assert(parameters->GetType() == KalmanFilterType::fixed_lag_smoother);

  const boost::shared_ptr<FixedLagSmootherKalmanFilterCalculationElements> calculation {
    new FixedLagSmootherKalmanFilterCalculationElements
  };

  assert(calculation);

  const boost::shared_ptr<FixedLagSmootherKalmanFilter> kalman_filter {
    new FixedLagSmootherKalmanFilter(calculation,parameters)
  };
  assert(kalman_filter);
  assert(kalman_filter->GetType() == KalmanFilterType::fixed_lag_smoother);
  return kalman_filter;
}

#ifndef NDEBUG
void ribi::kalman::FixedLagSmootherKalmanFilterFactory::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/fixedlagsmootherkalmanfilterparameters.h

 

#ifndef FIXEDLAGSMOOTHERKALMANFILTERPARAMETERS_H
#define FIXEDLAGSMOOTHERKALMANFILTERPARAMETERS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "kalmanfilterparameters.h"
#include "kalmanfiltertype.h"
#include "standardkalmanfilterparameters.h"

namespace ribi {
namespace kalman {

struct FixedLagSmootherKalmanFilterParameters : public KalmanFilterParameters
{
  explicit FixedLagSmootherKalmanFilterParameters(
    const boost::shared_ptr<StandardKalmanFilterParameters>& standard_parameters,
    const int lag);

  ///Obtain the lag in timesteps
  int GetLag() const { return m_lag; }

  ///The parameters from a standard Kalman filter
  boost::shared_ptr<const StandardKalmanFilterParameters> GetStandardParameters() const
    { return m_standard_parameters; }

  ///Obtain the Kalman filter type as an enum
  KalmanFilterType GetType() const { return KalmanFilterType::fixed_lag_smoother; }

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Check if this parameter set has a certain type of KalmanFilterParameter
  static bool HasParameterType(const KalmanFilterParameterType type);

  private:
  ///Can only be deleted by boost::checked_delete
  ~FixedLagSmootherKalmanFilterParameters() noexcept {}
  friend void boost::checked_delete<>(FixedLagSmootherKalmanFilterParameters*);

  ///The lag in timesteps
  const int m_lag;

  ///The parameters from a standard Kalman filter
  const boost::shared_ptr<const StandardKalmanFilterParameters> m_standard_parameters;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // FIXEDLAGSMOOTHERKALMANFILTERPARAMETERS_H

 

 

 

 

 

./CppKalmanFilter/fixedlagsmootherkalmanfilterparameters.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "fixedlagsmootherkalmanfilterparameters.h"
#pragma GCC diagnostic pop

#include <cassert>

#include "standardkalmanfilterparameters.h"

ribi::kalman::FixedLagSmootherKalmanFilterParameters::FixedLagSmootherKalmanFilterParameters(
  const boost::shared_ptr<StandardKalmanFilterParameters>& standard_parameters,
  const int lag)
  : KalmanFilterParameters(
      standard_parameters->GetControl(),
      standard_parameters->GetInitialStateEstimate(),
      standard_parameters->GetObservation(),
      standard_parameters->GetStateTransition()),
    m_lag{lag},
    m_standard_parameters{standard_parameters}
{
  assert(lag >= 0);
  assert(m_standard_parameters);
}

bool ribi::kalman::FixedLagSmootherKalmanFilterParameters::HasParameterType(const KalmanFilterParameterType type)
{
  return StandardKalmanFilterParameters::HasParameterType(type);
}

 

 

 

 

 

./CppKalmanFilter/gapsfilledwhitenoisesystem.h

 

#ifndef GAPSFILLEDWHITENOISESYSTEM_H
#define GAPSFILLEDWHITENOISESYSTEM_H

#include <queue>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "whitenoisesystem.h"
#include "whitenoisesystemparameters.h"
#include "gapsfilledwhitenoisesystemparameters.h"

namespace ribi {
namespace kalman {

///A gaps-filled white noise system is a system that does not have a measurement every timestep.
///Instead, it has for example 1 measurement and then 4 non-measurements (which is a gap of 5)
///During the 4 non-measurements the 1 measurement is repeated
struct GapsFilledWhiteNoiseSystem : public WhiteNoiseSystem
{
  ///Obtain the gaps-filled white noise system parameters
  const boost::shared_ptr<const GapsFilledWhiteNoiseSystemParameters>&
    GetGapsFilledWhiteNoiseSystemParameters() const noexcept
      { return m_parameters; }

  ///Obtain the type as an enum
  WhiteNoiseSystemType GetType() const noexcept { return WhiteNoiseSystemType::gaps_filled; }

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Update reality, that is, let the real system (i.e. reality) go to its next state
  void GoToNextState(const boost::numeric::ublas::vector<double>& input);

  ///Measure a value from this system with normally distributed noise
  boost::numeric::ublas::vector<double> Measure() const;

  ///Peek what the real value is
  const boost::numeric::ublas::vector<double>& PeekAtRealState() const noexcept;

  private:
  ///GapsFilledWhiteNoiseSystem must be created with a GapsFilledWhiteNoiseSystemFactory
  explicit GapsFilledWhiteNoiseSystem(const boost::shared_ptr<const WhiteNoiseSystemParameters>& white_noise_system_parameters);
  friend class GapsFilledWhiteNoiseSystemFactory;

  ///Can only be deleted by boost::checked_delete
  ~GapsFilledWhiteNoiseSystem() noexcept {}
  friend void boost::checked_delete<>(GapsFilledWhiteNoiseSystem*);

  ///The last successfull measurement
  mutable boost::numeric::ublas::vector<double> m_last_measument;

  ///The gaps-filled white noise system parameters
  const boost::shared_ptr<const GapsFilledWhiteNoiseSystemParameters> m_parameters;

  ///The current timestep: zero denotes a measurement will be done
  mutable int m_timestep;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // GAPSFILLEDWHITENOISESYSTEM_H

 

 

 

 

 

./CppKalmanFilter/gapsfilledwhitenoisesystem.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "gapsfilledwhitenoisesystem.h"

#include <cassert>

#include <boost/numeric/conversion/cast.hpp>

#include "matrix.h"
#include "trace.h"
#include "testtimer.h"
#include "gapsfilledwhitenoisesystem.h"
#include "gapsfilledwhitenoisesystemfactory.h"
#include "gapsfilledwhitenoisesystemparameters.h"

#pragma GCC diagnostic pop

ribi::kalman::GapsFilledWhiteNoiseSystem::GapsFilledWhiteNoiseSystem(
  const boost::shared_ptr<const WhiteNoiseSystemParameters>& parameters)
  : WhiteNoiseSystem{parameters},
    m_last_measument{parameters->GetInitialState().size(),0.0},
    m_parameters{boost::dynamic_pointer_cast<const GapsFilledWhiteNoiseSystemParameters>(parameters)},
    m_timestep{0}
{
  #ifndef NDEBUG
  Test();
  assert(m_parameters);
  //Check measuring frequecies
  {
    const auto v = m_parameters->GetMeasurementFrequency();
    const std::size_t sz = v.size();
    for (std::size_t i=0; i!=sz; ++i)
    {
      const int x = v[i];
      assert(x >= 1 && "At least one out of one measurements is a real measurement");
    }
  }
  #endif
}

std::string ribi::kalman::GapsFilledWhiteNoiseSystem::GetVersion() noexcept
{
  return "1.0";
}

std::vector<std::string> ribi::kalman::GapsFilledWhiteNoiseSystem::GetVersionHistory() noexcept
{
  return {
    "2013-06-25: version 1.0: initial version"
  };
}

void ribi::kalman::GapsFilledWhiteNoiseSystem::GoToNextState(const boost::numeric::ublas::vector<double>& input)
{
  //Standard transition
  assert(input.size() == GetCurrentState().size());
  assert(m_parameters->GetStateTransition().size1() == GetCurrentState().size());
  assert(m_parameters->GetStateTransition().size2() == GetCurrentState().size());
  assert(m_parameters->GetControl().size1() == input.size());
  assert(m_parameters->GetControl().size2() == input.size());

  boost::numeric::ublas::vector<double> new_state
    = Matrix::Prod(m_parameters->GetStateTransition(),GetCurrentState())
    + Matrix::Prod(m_parameters->GetControl(),input);
  //Add process noise
  const auto sz = new_state.size();
  assert(new_state.size() == m_parameters->GetProcessNoise().size());
  for (std::size_t i = 0; i!=sz; ++i)
  {
    new_state(i) = GetRandomNormal(new_state(i),m_parameters->GetProcessNoise()(i));
  }
  SetNewCurrentState(new_state);
}

boost::numeric::ublas::vector<double> ribi::kalman::GapsFilledWhiteNoiseSystem::Measure() const
{
  const boost::numeric::ublas::vector<int>& fs
    = this->GetGapsFilledWhiteNoiseSystemParameters()->GetMeasurementFrequency();

  assert(fs.size() == m_last_measument.size());
  assert(fs.size() == GetCurrentState().size());
  assert(fs.size() == m_parameters->GetMeasurementNoise().size());

  const std::size_t n_states = fs.size();
  for (std::size_t state=0; state!=n_states; ++state)
  {
    assert(state < fs.size());
    const int f = fs[state];
    assert(f >= 1);

    if (m_timestep % f == 0)
    {
      assert(state < m_last_measument.size());
      assert(state < GetCurrentState().size());
      assert(state < m_parameters->GetMeasurementNoise().size());
      m_last_measument(state) = GetRandomNormal(GetCurrentState()(state),m_parameters->GetMeasurementNoise()(state));
    }
    else
    {
      //Should get here
    }
  }
  ++m_timestep;

  return m_last_measument;
}

const boost::numeric::ublas::vector<double>&
  ribi::kalman::GapsFilledWhiteNoiseSystem::PeekAtRealState() const noexcept
{
  return this->GetCurrentState();
}

#ifndef NDEBUG
void ribi::kalman::GapsFilledWhiteNoiseSystem::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
  //Check if measurements are indeed lagged:
  //The system's real value should update immediatly, but this fresh measurement
  //must only be accessible after lag timesteps
  //Context: measuring the position of an object with constant velocity
  {
    const int f = 5;
    const boost::shared_ptr<GapsFilledWhiteNoiseSystem> my_system
      = GapsFilledWhiteNoiseSystemFactory().Create(
        Matrix::CreateMatrix(1,1, { 1.0 } ), //control
        Matrix::CreateVector(     { 0.0 } ), //initial_state,
        Matrix::CreateVector(     {   f } ), //measurement frequencies
        Matrix::CreateVector(     { 0.0000001 } ), //real_measurement_noise
        Matrix::CreateVector(     { 0.0000001 } ), //real_process_noise
        Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    );
    assert(my_system);
    //Context: airhockey puck
    const boost::numeric::ublas::vector<double> input = Matrix::CreateVector( { 1.0 } );

    for (int i=0; i!= 3*f; ++i)
    {
      const boost::numeric::ublas::vector<double> measurements = my_system->Measure();
      assert(!measurements.empty());
      assert(measurements.size() == 1);
      assert(measurements.size() == my_system->PeekAtRealState().size());
      const double expected = static_cast<double>(i);
      const double measured = measurements(0);
      const double real = my_system->PeekAtRealState()(0);
      assert(Matrix::IsAboutEqual(real,expected));
      if (i % f == 0)
      {
        assert(Matrix::IsAboutEqual(measured,expected));
      }
      else
      {
        assert(!Matrix::IsAboutEqual(measured,expected));
      }
      my_system->GoToNextState(input);
    }
  }
}
#endif

 

 

 

 

 

./CppKalmanFilter/gapsfilledwhitenoisesystemfactory.h

 

#ifndef GAPSFILLEDWHITENOISESYSTEMFACTORY_H
#define GAPSFILLEDWHITENOISESYSTEMFACTORY_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "gapsfilledwhitenoisesystem.h"

namespace ribi {
namespace kalman {

///Factory for GapsFilledWhiteNoiseSystem
struct GapsFilledWhiteNoiseSystemFactory
{
  GapsFilledWhiteNoiseSystemFactory();

  ///Create a GapsFilledWhiteNoiseSystem from its parameters
  boost::shared_ptr<GapsFilledWhiteNoiseSystem> Create(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const boost::numeric::ublas::vector<int>& measurement_frequency,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition
  ) const noexcept;

  ///Create a GapsFilledWhiteNoiseSystem from the parameters
  boost::shared_ptr<GapsFilledWhiteNoiseSystem> Create(
    const boost::shared_ptr<WhiteNoiseSystemParameters>& parameters
  ) const noexcept;

  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // GAPSFILLEDWHITENOISESYSTEMFACTORY_H

 

 

 

 

 

./CppKalmanFilter/gapsfilledwhitenoisesystemfactory.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "gapsfilledwhitenoisesystemfactory.h"

#include "matrix.h"
#include "testtimer.h"
#pragma GCC diagnostic pop

ribi::kalman::GapsFilledWhiteNoiseSystemFactory::GapsFilledWhiteNoiseSystemFactory()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::shared_ptr<ribi::kalman::GapsFilledWhiteNoiseSystem> ribi::kalman::GapsFilledWhiteNoiseSystemFactory::Create(
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::vector<double>& initial_state,
  const boost::numeric::ublas::vector<int>& measurement_frequency,
  const boost::numeric::ublas::vector<double>& real_measurement_noise,
  const boost::numeric::ublas::vector<double>& real_process_noise,
  const boost::numeric::ublas::matrix<double>& state_transition
) const noexcept
{
  const boost::shared_ptr<const WhiteNoiseSystemParameters> parameters(
    new GapsFilledWhiteNoiseSystemParameters(
      control,
      initial_state,
      measurement_frequency,
      real_measurement_noise,
      real_process_noise,
      state_transition));
  assert(parameters);
  assert(parameters->GetType() == WhiteNoiseSystemType::gaps_filled);

  const boost::shared_ptr<GapsFilledWhiteNoiseSystem> system(
    new GapsFilledWhiteNoiseSystem(parameters));
  assert(system);
  assert(system->GetType() == WhiteNoiseSystemType::gaps_filled);
  return system;
}

boost::shared_ptr<ribi::kalman::GapsFilledWhiteNoiseSystem> ribi::kalman::GapsFilledWhiteNoiseSystemFactory::Create(
  const boost::shared_ptr<WhiteNoiseSystemParameters>& general_parameters) const noexcept
{
  assert(general_parameters);
  assert(general_parameters->GetType() == WhiteNoiseSystemType::gaps_filled);
  const boost::shared_ptr<GapsFilledWhiteNoiseSystemParameters> parameters
    = boost::dynamic_pointer_cast<GapsFilledWhiteNoiseSystemParameters>(general_parameters);
  assert(parameters);
  assert(parameters->GetType() == WhiteNoiseSystemType::gaps_filled);

  const boost::shared_ptr<GapsFilledWhiteNoiseSystem> my_system
    = Create(
      parameters->GetControl(),
      parameters->GetInitialState(),
      parameters->GetMeasurementFrequency(),
      parameters->GetMeasurementNoise(),
      parameters->GetProcessNoise(),
      parameters->GetStateTransition());
  assert(my_system);
  assert(my_system->GetType() == WhiteNoiseSystemType::gaps_filled);

  return my_system;
}
#ifndef NDEBUG
void ribi::kalman::GapsFilledWhiteNoiseSystemFactory::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    Matrix();
    GapsFilledWhiteNoiseSystemFactory().Create(
      Matrix::CreateMatrix(1,1, { 1.0 } ), //control
      Matrix::CreateVector(     { 0.0 } ), //initial_state,
      Matrix::CreateVector(     {   1 } ), //measurement frequencies
      Matrix::CreateVector(     { 0.0000001 } ), //real_measurement_noise
      Matrix::CreateVector(     { 0.0000001 } ), //real_process_noise
      Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    );
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/gapsfilledwhitenoisesystemparameters.h

 

#ifndef GAPSFILLEDWHITENOISESYSTEMPARAMETERS_H
#define GAPSFILLEDWHITENOISESYSTEMPARAMETERS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "whitenoisesystemparameters.h"

namespace ribi {
namespace kalman {

struct GapsFilledWhiteNoiseSystemParameters : public WhiteNoiseSystemParameters
{
  explicit GapsFilledWhiteNoiseSystemParameters(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const boost::numeric::ublas::vector<int>& measurement_frequency,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition);

  ///The number of timesteps after which a real measurement is acquired
  const boost::numeric::ublas::vector<int>& GetMeasurementFrequency() const noexcept { return m_measurement_frequency; }

  ///Obtain the type as an enum
  WhiteNoiseSystemType GetType() const noexcept { return WhiteNoiseSystemType::gaps_filled; }

  private:
  ///Can only be deleted by boost::checked_delete
  ~GapsFilledWhiteNoiseSystemParameters() noexcept {}
  friend void boost::checked_delete<>(GapsFilledWhiteNoiseSystemParameters*);

  ///The gaps (in timesteps) is the number of measurements of which only one real measurement is acquired
  const boost::numeric::ublas::vector<int> m_measurement_frequency;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // GAPSFILLEDWHITENOISESYSTEMPARAMETERS_H

 

 

 

 

 

./CppKalmanFilter/gapsfilledwhitenoisesystemparameters.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "gapsfilledwhitenoisesystemparameters.h"

#include <cassert>

#include "standardwhitenoisesystemparameters.h"
#include "trace.h"
#pragma GCC diagnostic pop

ribi::kalman::GapsFilledWhiteNoiseSystemParameters::GapsFilledWhiteNoiseSystemParameters(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const boost::numeric::ublas::vector<int>& measurement_frequency,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition)
  : WhiteNoiseSystemParameters(
      control,
      initial_state,
      real_measurement_noise,
      real_process_noise,
      state_transition
    ),
    m_measurement_frequency{measurement_frequency}
{
  #ifndef NDEBUG
  Test();
  assert(m_measurement_frequency.size() == initial_state.size());
  //Check measuring frequecies
  {
    const auto v = m_measurement_frequency;
    const std::size_t sz = v.size();
    for (std::size_t i=0; i!=sz; ++i)
    {
      const int x = v[i];
      if (x < 1)
      {
        TRACE(x);
        TRACE("BREAK");
      }
      assert(x >= 1 && "At least one out of one measurements is a real measurement");
    }
  }
  #endif
}

#ifndef NDEBUG
void ribi::kalman::GapsFilledWhiteNoiseSystemParameters::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {

  }
}
#endif

 

 

 

 

 

./CppKalmanFilter/kalmanfilter.h

 

#ifndef KALMANFILTER_H
#define KALMANFILTER_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include <vector>
#include <string>
#include <boost/shared_ptr.hpp>
#include "kalmanfiltertype.h"
#include "kalmanfilterparameters.h"
#include "kalmanfiltercalculationelements.h"
#pragma GCC diagnostic pop

namespace ribi {
namespace kalman {

///Kalman filter base class
struct KalmanFilter
{
  KalmanFilter(const KalmanFilter&) = delete;
  KalmanFilter& operator=(const KalmanFilter&) = delete;
  virtual ~KalmanFilter() noexcept {}

  ///Obtain the Kalman filter type as an enum
  virtual KalmanFilterType GetType() const = 0;

  ///Get the Kalman filter last calculation elements
  virtual boost::shared_ptr<KalmanFilterCalculationElements> GetLastCalculation() const = 0;

  ///Obtain the Kalman filter parameters
  virtual boost::shared_ptr<const KalmanFilterParameters> GetParameters() const = 0;

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  virtual void SupplyMeasurementAndInput(
    const boost::numeric::ublas::vector<double>& measurements,
    const boost::numeric::ublas::vector<double>& input
  ) = 0;

  protected:
  explicit KalmanFilter() {}

  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTER_H

 

 

 

 

 

./CppKalmanFilter/kalmanfilter.cpp

 

#include "kalmanfilter.h"

std::string ribi::kalman::KalmanFilter::GetVersion() noexcept
{
  return "1.1";
}

std::vector<std::string> ribi::kalman::KalmanFilter::GetVersionHistory() noexcept
{
  return {
    "2013-05-06: version 1.0: initial version",
    "2013-09-09: version 1.1: removed data members"
  };
}

 

 

 

 

 

./CppKalmanFilter/kalmanfiltercalculationelements.h

 

#ifndef KALMANFILTERCALCULATIONELEMENTS_H
#define KALMANFILTERCALCULATIONELEMENTS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "kalmanfiltertype.h"

namespace ribi {
namespace kalman {

///The elements of a steady-state Kalman filter calculation
struct KalmanFilterCalculationElements
{
  KalmanFilterCalculationElements(const KalmanFilterCalculationElements&) = delete;
  KalmanFilterCalculationElements& operator=(const KalmanFilterCalculationElements&) = delete;


  virtual ~KalmanFilterCalculationElements() noexcept {}

  ///Clear the calculation, will set IsComplete to false
  virtual void Clear();

  ///Obtain the measurement ('z_n')
  const boost::numeric::ublas::vector<double>& GetMeasurement() const;

  ///Get the predicted state ('x_predicted')
  ///Is calculated at step 1 of the algorithm:
  ///x_predicted = [...] x_n-1 [...]
  const boost::numeric::ublas::vector<double>& GetPredictedState() const;

  ///Obtain the previous_state_estimate ('x_prev')
  const boost::numeric::ublas::vector<double>& GetPreviousState() const;

  ///Obtain the updated state ('x_n')
  const boost::numeric::ublas::vector<double>& GetUpdatedState() const;

  ///Obtain the Kalman filter type as an enum
  virtual KalmanFilterType GetType() const = 0;

  ///Checks if the state is complete and valid
  virtual bool IsComplete() const;

  ///Set the measurement ('z_n')
  ///Fails if already set
  void SetMeasurement(const boost::numeric::ublas::vector<double>& measurement);

  ///Set the predicted state ('x_predicted')
  ///Is calculated at step 1 of the algorithm:
  ///x_predicted = [...] x_n-1 [...]
  ///Fails if already set
  void SetPredictedState(const boost::numeric::ublas::vector<double>& predicted_state);

  ///Set the previous_state_estimate ('x_prev'/'x_n-1')
  ///Is read at step 1 of the algorithm:
  ///x_predicted = [...] x_n-1 [...]
  ///Fails if already set
  void SetPreviousStateEstimate(const boost::numeric::ublas::vector<double>& previous_state_estimate);

  ///Set the updated state ('x_n')
  ///Is calculated at step 6 of the algorithm:
  ///x_n = x_predicted + [...]
  ///Fails if already set
  void SetUpdatedState(const boost::numeric::ublas::vector<double>& updated_state);

  protected:
  ///An ABC can only be constructed by derived classes
  explicit KalmanFilterCalculationElements(
    const boost::numeric::ublas::vector<double>& measurement,
    const boost::numeric::ublas::vector<double>& predicted_state,
    const boost::numeric::ublas::vector<double>& previous_state_estimate,
    const boost::numeric::ublas::vector<double>& updated_state);

  private:
  ///Produce a deep copy of the derived class
  virtual boost::shared_ptr<KalmanFilterCalculationElements> Clone() const = 0;
  friend class KalmanFilterCalculationElementsFactory;

  private:
  ///The measurement ('z_n')
  boost::numeric::ublas::vector<double> m_measurement;

  ///The predicted state ('x_predicted')
  boost::numeric::ublas::vector<double> m_predicted_state;

  ///The previous_state_estimate ('x_prev')
  boost::numeric::ublas::vector<double> m_previous_state_estimate;

  ///The updated state ('x_n')
  boost::numeric::ublas::vector<double> m_updated_state;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTERCALCULATIONELEMENTS_H

 

 

 

 

 

./CppKalmanFilter/kalmanfiltercalculationelements.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "kalmanfiltercalculationelements.h"
#pragma GCC diagnostic pop

#include <cassert>

ribi::kalman::KalmanFilterCalculationElements::KalmanFilterCalculationElements(
  const boost::numeric::ublas::vector<double>& measurement,
  const boost::numeric::ublas::vector<double>& predicted_state,
  const boost::numeric::ublas::vector<double>& previous_state_estimate,
  const boost::numeric::ublas::vector<double>& updated_state)
  : m_measurement{measurement},
    m_predicted_state{predicted_state},
    m_previous_state_estimate{previous_state_estimate},
    m_updated_state{updated_state}
{
  //Size may be zero
  #ifndef NDEBUG
  const std::size_t sz = m_measurement.size();
  assert(sz == m_measurement.size());
  assert(sz == m_predicted_state.size());
  assert(sz == m_previous_state_estimate.size());
  assert(sz == m_updated_state.size());
  #endif
}

void ribi::kalman::KalmanFilterCalculationElements::Clear()
{
  m_measurement = boost::numeric::ublas::vector<double>();
  m_predicted_state = boost::numeric::ublas::vector<double>();
  m_previous_state_estimate = boost::numeric::ublas::vector<double>();
  m_updated_state = boost::numeric::ublas::vector<double>();
  assert(m_measurement.size() == 0);
  assert(!ribi::kalman::KalmanFilterCalculationElements::IsComplete());
}

const boost::numeric::ublas::vector<double>& ribi::kalman::KalmanFilterCalculationElements::GetMeasurement() const
{
  return m_measurement;
}

const boost::numeric::ublas::vector<double>& ribi::kalman::KalmanFilterCalculationElements::GetPredictedState() const
{
  return m_predicted_state;
}

const boost::numeric::ublas::vector<double>& ribi::kalman::KalmanFilterCalculationElements::GetPreviousState() const
{
  return m_previous_state_estimate;
}

const boost::numeric::ublas::vector<double>& ribi::kalman::KalmanFilterCalculationElements::GetUpdatedState() const
{
  return m_updated_state;
}

bool ribi::kalman::KalmanFilterCalculationElements::IsComplete() const
{
  const std::size_t sz = m_measurement.size();
  return sz != 0
    && sz == m_measurement.size()
    && sz == m_predicted_state.size()
    && sz == m_previous_state_estimate.size()
    && sz == m_updated_state.size();
}


void ribi::kalman::KalmanFilterCalculationElements::SetMeasurement(const boost::numeric::ublas::vector<double>& measurement)
{
  assert(m_measurement.empty());
  m_measurement = measurement;
}

void ribi::kalman::KalmanFilterCalculationElements::SetPredictedState(const boost::numeric::ublas::vector<double>& predicted_state)
{
  assert(m_predicted_state.empty());
  m_predicted_state = predicted_state;
}

void ribi::kalman::KalmanFilterCalculationElements::SetPreviousStateEstimate(const boost::numeric::ublas::vector<double>& previous_state_estimate)
{
  assert(m_previous_state_estimate.empty());
  m_previous_state_estimate = previous_state_estimate;
}

void ribi::kalman::KalmanFilterCalculationElements::SetUpdatedState(const boost::numeric::ublas::vector<double>& updated_state)
{
  assert(m_updated_state.empty());
  m_updated_state = updated_state;
}

 

 

 

 

 

./CppKalmanFilter/kalmanfiltercalculationelementsfactory.h

 

#ifndef KALMANFILTERCALCULATIONELEMENTSFACTORY_H
#define KALMANFILTERCALCULATIONELEMENTSFACTORY_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "kalmanfiltercalculationelements.h"

namespace ribi {
namespace kalman {

struct KalmanFilterCalculationElementsFactory
{
  KalmanFilterCalculationElementsFactory();

  ///Create a KalmanFilterCalculationElements of a certain type
  boost::shared_ptr<KalmanFilterCalculationElements> Create(
    const KalmanFilterType type
  ) const noexcept;

  ///Deep copy
  boost::shared_ptr<KalmanFilterCalculationElements> DeepCopy(
    const boost::shared_ptr<KalmanFilterCalculationElements>& elements
  ) const noexcept;

  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTERCALCULATIONELEMENTSFACTORY_H

 

 

 

 

 

./CppKalmanFilter/kalmanfiltercalculationelementsfactory.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "kalmanfiltercalculationelementsfactory.h"
#pragma GCC diagnostic pop

#include <cassert>

#include "standardkalmanfiltercalculationelements.h"
#include "steadystatekalmanfiltercalculationelements.h"
#include "fixedlagsmootherkalmanfiltercalculationelements.h"
#include "testtimer.h"

ribi::kalman::KalmanFilterCalculationElementsFactory::KalmanFilterCalculationElementsFactory()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::shared_ptr<ribi::kalman::KalmanFilterCalculationElements>
  ribi::kalman::KalmanFilterCalculationElementsFactory::Create(
  const KalmanFilterType type) const noexcept
{
  boost::shared_ptr<KalmanFilterCalculationElements> p;
  switch (type)
  {
    case KalmanFilterType::fixed_lag_smoother:
      p.reset(new FixedLagSmootherKalmanFilterCalculationElements);
    break;
    case KalmanFilterType::standard:
      p.reset(new StandardKalmanFilterCalculationElements);
    break;
    case KalmanFilterType::steady_state:
      p.reset(new SteadyStateKalmanFilterCalculationElements);
    break;
    case KalmanFilterType::n_types:
      assert(!"Unimplemented KalmanFilterType");
  }
  assert(p);
  assert(p->GetType() == type);
  return p;
}

boost::shared_ptr<ribi::kalman::KalmanFilterCalculationElements> ribi::kalman::KalmanFilterCalculationElementsFactory::DeepCopy(
  const boost::shared_ptr<KalmanFilterCalculationElements>& original) const noexcept
{
  assert(original);

  const boost::shared_ptr<KalmanFilterCalculationElements> my_copy = original->Clone();

  assert(my_copy);
  assert(original->GetType() == my_copy->GetType());

  return my_copy;
}

#ifndef NDEBUG
void ribi::kalman::KalmanFilterCalculationElementsFactory::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/kalmanfilterexample.h

 

#ifndef KALMANFILTEREXAMPLE_H
#define KALMANFILTEREXAMPLE_H

#include <memory>
#include <vector>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/checked_delete.hpp>
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "standardkalmanfilterparameters.h"
#include "standardwhitenoisesystemparameters.h"


namespace ribi {
namespace kalman {

///KalmanFilterExample contains an example set of variables
struct KalmanFilterExample
{
  explicit KalmanFilterExample(
    const std::string& title,
    const std::string& context,
    const std::vector<std::string>& inputs,
    const boost::shared_ptr<const StandardKalmanFilterParameters>& kalman_filter_parameters,
    const int number_of_timesteps,
    const std::vector<std::string>& state_names,
    const boost::shared_ptr<const StandardWhiteNoiseSystemParameters>& white_noise_system_parameters);

  static std::vector<boost::shared_ptr<KalmanFilterExample> > CreateExamples() noexcept;

  static std::unique_ptr<KalmanFilterExample> CreateExample(const int i);

  const std::string& GetContext() const { return m_context; }
  const std::vector<std::string>& GetInputs() const { return m_inputs; }
  const boost::shared_ptr<const StandardKalmanFilterParameters>& GetKalmanFilterParameters() const
    { return m_kalman_filter_parameters; }
  int GetNumberOfTimesteps() const { return m_number_of_timesteps; }
  const std::vector<std::string>& GetStateNames() const { return m_state_names; }
  const std::string& GetTitle() const { return m_title; }
  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters>& GetWhiteNoiseSystemParameters() const
    { return  m_white_noise_system_parameters; }

  private:
  ///Can only be deleted by boost::checked_delete
  ~KalmanFilterExample() noexcept {}
  ///Can only be deleted by smart pointers: boost::checked_delete and std::unique_ptr
  friend void boost::checked_delete<>(KalmanFilterExample*);
  friend void boost::checked_delete<>(const KalmanFilterExample*);
  friend struct std::default_delete<KalmanFilterExample>;

  const std::string m_context;
  const std::vector<std::string> m_inputs;
  const boost::shared_ptr<const StandardKalmanFilterParameters> m_kalman_filter_parameters;
  const int m_number_of_timesteps;
  const std::vector<std::string> m_state_names;
  const std::string m_title;
  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> m_white_noise_system_parameters;

  static std::unique_ptr<KalmanFilterExample> CreateExample0();
  static std::unique_ptr<KalmanFilterExample> CreateExample1();
  static std::unique_ptr<KalmanFilterExample> CreateExample2();
  static std::unique_ptr<KalmanFilterExample> CreateExample3();
  static std::unique_ptr<KalmanFilterExample> CreateExample4();
  static std::unique_ptr<KalmanFilterExample> CreateExample5();
  static std::unique_ptr<KalmanFilterExample> CreateExample6();
  static std::unique_ptr<KalmanFilterExample> CreateExample7();
  static std::unique_ptr<KalmanFilterExample> CreateExample8();

  static std::string DisplayAsUblasVector(const std::vector<std::string>& v);

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTEREXAMPLE_H

 

 

 

 

 

./CppKalmanFilter/kalmanfilterexample.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "kalmanfilterexample.h"

#include <cassert>

#include <boost/math/constants/constants.hpp>

#include <memory>


#include <boost/lexical_cast.hpp>
#include <boost/numeric/ublas/io.hpp>
#include "matrix.h"
#include "trace.h"

#pragma GCC diagnostic pop

ribi::kalman::KalmanFilterExample::KalmanFilterExample(
  const std::string& title,
  const std::string& context,
  const std::vector<std::string>& inputs,
  const boost::shared_ptr<const StandardKalmanFilterParameters>& kalman_filter_parameters,
  const int number_of_timesteps,
  const std::vector<std::string>& state_names,
  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters>& white_noise_system_parameters)
  : m_context{context},
    m_inputs{inputs},
    m_kalman_filter_parameters{kalman_filter_parameters},
    m_number_of_timesteps{number_of_timesteps},
    m_state_names{state_names},
    m_title{title},
    m_white_noise_system_parameters{white_noise_system_parameters}
{
  assert(!m_state_names.empty());
  assert(m_state_names.size() == m_inputs.size());
  assert(m_state_names.size() == m_kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(m_state_names.size() == m_white_noise_system_parameters->GetInitialState().size());
  assert(number_of_timesteps >= 0);

}

std::vector<boost::shared_ptr<ribi::kalman::KalmanFilterExample> > ribi::kalman::KalmanFilterExample::CreateExamples() noexcept
{
  std::vector<boost::shared_ptr<KalmanFilterExample> > v;
  for (int i=0; ; ++i)
  {
    std::unique_ptr<KalmanFilterExample> p(CreateExample(i));
    if (!p) return v;
    const boost::shared_ptr<KalmanFilterExample> q(p.release());
    assert(q);
    assert(q.use_count() == 1);
    assert(!p);
    v.push_back(q);
  }
  assert(!"Cannot get here: must return in for loop above");
}

std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample(const int i)
{
  std::unique_ptr<KalmanFilterExample> p;
  switch (i)
  {
    case 0: p = CreateExample0(); break;
    case 1: p = CreateExample1(); break;
    case 2: p = CreateExample2(); break;
    case 3: p = CreateExample3(); break;
    case 4: p = CreateExample4(); break;
    case 5: p = CreateExample5(); break;
    case 6: p = CreateExample6(); break;
    case 7: p = CreateExample7(); break;
    case 8: p = CreateExample8(); break;
    default: break;
  }
  //When p is nullptr, this indicates that there are no more examples

  return p;
}



std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample0()
{
  const std::string title = "Constant voltage";

  const int number_of_timesteps = 1000;

  const boost::numeric::ublas::matrix<double> control
    = Matrix::CreateMatrix(1,1, { 0.0 } );
  const boost::numeric::ublas::matrix<double> estimated_measurement_noise
    = Matrix::CreateMatrix(1,1, { 0.1 } );
  const boost::numeric::ublas::matrix<double> estimated_process_noise_covariance
    = Matrix::CreateMatrix(1,1, { 0.0001 } );
  const boost::numeric::ublas::matrix<double> initial_covariance_estimate
    = Matrix::CreateMatrix(1,1, { 1.0 } );
  const boost::numeric::ublas::vector<double> initial_state_estimate
    = Matrix::CreateVector( { 3.0 } );
  const boost::numeric::ublas::matrix<double> observation
    = Matrix::CreateMatrix(1,1, { 1.0 } );
  const boost::numeric::ublas::matrix<double> state_transition
    = Matrix::CreateMatrix(1,1, {1.0} );
  const boost::numeric::ublas::vector<double> initial_state
    = Matrix::CreateVector( { 1.25 } );
  const boost::numeric::ublas::vector<double> real_measurement_noise
    = Matrix::CreateVector( { 0.1 } );
  const boost::numeric::ublas::vector<double> real_process_noise
    = Matrix::CreateVector( { 0.00001 } );


  const std::vector<std::string> inputs = { "0.0" };

  const boost::shared_ptr<const StandardKalmanFilterParameters> kalman_filter_parameters(
    new StandardKalmanFilterParameters(
    control,
    estimated_measurement_noise,
    estimated_process_noise_covariance,
    initial_covariance_estimate,
    initial_state_estimate,
    observation,
    state_transition)
  );

  const std::vector<std::string> state_names = { "V" };

  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> white_noise_system_parameters(
    new StandardWhiteNoiseSystemParameters(
    control,
    initial_state,
    real_measurement_noise,
    real_process_noise,
    state_transition)
  );

  assert(state_names.size() == inputs.size());
  assert(state_names.size() == kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(state_names.size() == white_noise_system_parameters->GetInitialState().size());

  std::stringstream context_stream;
  context_stream
    << "<h1>" << title << "</h1>" << '\n'
    << "<p>This is an example from <a href=\"http://greg.czerniak.info/guides/kalman1\">Greg Czerniak's tutorial</a></p>" << '\n'
    << "<p>The context is the noisy measurement of a constant voltage.<p>" << '\n'
    << "<ul>" << '\n'
    << "  <li>State names: " << DisplayAsUblasVector(state_names) << ": the voltage</li>" << '\n'
    << "  <li>Initial state, real: " << initial_state << ": just some voltage</li>" << '\n'
    << "  <li>Initial state, estimate: " << initial_state_estimate << ": set this value off on purpose, to see the Kalman filter converge</li>" << '\n'
    << "  <li>Input: " << DisplayAsUblasVector(inputs) << ": the state (that is, the voltage) will not be changed</li>" << '\n'
    << "  <li>Control: " << control << ": any input (albeit none) will have no effect</li>" << '\n'
    << "  <li>Observation: " << observation << ": the voltage is measured directly (that is: as a voltage)</li>" << '\n'
    << "  <li>State transition: " << state_transition << ": the voltage remains constant</li>" << '\n'
    << "  <li>Measurement noise, real: " << real_measurement_noise << ": just some low value</li>" << '\n'
    << "  <li>Measurement noise, estimate: " << estimated_measurement_noise << ": just some low value</li>" << '\n'
    << "  <li>Process noise, real: " << real_process_noise << ": just some low value</li>" << '\n'
    << "  <li>Process noise, estimated covariance: " << estimated_process_noise_covariance << ": just some low value</li>" << '\n'
    << "  <li>Initial covariance estimate: " << initial_covariance_estimate << ": just some low value</li>" << '\n'
    << "</ul>";
  const std::string context = context_stream.str();


  std::unique_ptr<KalmanFilterExample> example(
    new KalmanFilterExample(
      title,
      context,
      inputs,
      kalman_filter_parameters,
      number_of_timesteps,
      state_names,
      white_noise_system_parameters
    )
  );
  assert(example);
  
  return example;
}

std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample1()
{
  const std::string title = "Accelerating car (two states)";
  const int n = 2;
  const double acceleration = 1.0;
  const double measurement_noise = 10.0; //Called 'measnoise'
  const double accelnoise = 0.2; //Called 'accelnoise'
  const double dt = 0.1; //Timestep

  const int number_of_timesteps = 1000;

  const boost::numeric::ublas::matrix<double> control
    = Matrix::CreateMatrix(n,n, { 0.0,0.0,0.0,dt } );

  const boost::numeric::ublas::matrix<double> initial_covariance_estimate
    = Matrix::CreateMatrix(n,n, { 1.0,0.0,0.0,1.0 } );

  const boost::numeric::ublas::vector<double> initial_state_estimate
    = Matrix::CreateVector( { 10.0,1.0 } );

  const boost::numeric::ublas::vector<double> initial_state
    = Matrix::CreateVector( { 0.0,0.0 } );

  const boost::numeric::ublas::matrix<double> estimated_measurement_noise
    = Matrix::CreateMatrix(n,n, { 10.0 * measurement_noise,0.0,0.0,1000000.0 } );

  const boost::numeric::ublas::matrix<double> observation
    = Matrix::CreateMatrix(n,n, { 1.0,0.0,0.0,0.0 } );

  const boost::numeric::ublas::matrix<double> estimated_process_noise_covariance
    = Matrix::CreateMatrix(n,n, {
      10.0 * accelnoise,
      10.0 * accelnoise,
      10.0 * accelnoise,
      10.0 * accelnoise
    } );

  const boost::numeric::ublas::vector<double> real_measurement_noise
    = Matrix::CreateVector( { measurement_noise, 1000000.0 } );

  const boost::numeric::ublas::vector<double> real_process_noise
    = Matrix::CreateVector( { 0.5  * accelnoise * dt * dt, accelnoise * dt} );

  const boost::numeric::ublas::matrix<double> state_transition
    = Matrix::CreateMatrix(n,n, { 1.0, 0.0, dt, 1.0 } );

  const std::vector<std::string> inputs = { "0.0", boost::lexical_cast<std::string>(acceleration) };

  const boost::shared_ptr<const StandardKalmanFilterParameters> kalman_filter_parameters(
    new StandardKalmanFilterParameters(
    control,
    estimated_measurement_noise,
    estimated_process_noise_covariance,
    initial_covariance_estimate,
    initial_state_estimate,
    observation,
    state_transition)
  );

  const std::vector<std::string> state_names = { "x", "v" };

  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> white_noise_system_parameters(
    new StandardWhiteNoiseSystemParameters(
    control,
    initial_state,
    real_measurement_noise,
    real_process_noise,
    state_transition)
  );

  assert(control.size1() > 0);
  assert(control.size2() > 0);
  assert(state_names.size() == inputs.size());
  assert(state_names.size() == kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(state_names.size() == white_noise_system_parameters->GetInitialState().size());
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetControl(),white_noise_system_parameters->GetControl()));
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetStateTransition(),white_noise_system_parameters->GetStateTransition()));

  std::stringstream context_stream;
  context_stream
    << "<h1>" << title << "</h1>" << '\n'
    << "<p>This is an example from D. Simon's article 'Kalman Filtering', published in 'Embedded Systems Programming' (June 2001).</p>" << '\n'
    << "<p>The context is an accelerating car with a GPS and a defect speedometer<p>" << '\n'
    << "<ul>" << '\n'
    << "  <li>State names: " << DisplayAsUblasVector(state_names) << ": position (e.g. meter) and velocity (e.g. meter per second)</li>" << '\n'
    << "  <li>Initial state, real: " << initial_state << ": car starts from standstill</li>" << '\n'
    << "  <li>Initial state, estimate: " << initial_state_estimate << ": set this value off on purpose, to see the Kalman filter converge</li>" << '\n'
    << "  <li>Input: " << DisplayAsUblasVector(inputs) << ": a gas pedal is pushed to a certain acceleration value</li>" << '\n'
    << "  <li>Control: " << control << ": a gas pedal has an influence on the velocity</li>" << '\n'
    << "  <li>Observation: " << observation << ": only observe position, which can be observed directly with a GPS</li>" << '\n'
    << "  <li>State transition: " << state_transition << ": the position is increased by a velocity, the velocity stays constant without input</li>" << '\n'
    << "  <li>Measurement noise, real: " << real_measurement_noise << ": GPS works, speedometer does not</li>" << '\n'
    << "  <li>Measurement noise, estimate: " << estimated_measurement_noise << ": just some pessimistic value</li>" << '\n'
    << "  <li>Process noise, real: " << real_process_noise << ": noise caused by acceleration</li>" << '\n'
    << "  <li>Process noise, estimated covariance: " << estimated_process_noise_covariance << ": a more pessimistic value</li>" << '\n'
    << "  <li>Initial covariance estimate: " << initial_covariance_estimate << ": just a guess</li>" << '\n'
    << "</ul>";
  const std::string context = context_stream.str();

  std::unique_ptr<KalmanFilterExample> example(
    new KalmanFilterExample(
      title,
      context,
      inputs,
      kalman_filter_parameters,
      number_of_timesteps,
      state_names,
      white_noise_system_parameters
    )
  );
  assert(example);
  return example;
}

std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample2()
{
  const std::string title = "Cannonball";
  const int number_of_timesteps = 1000;

  const int n = 4; //Size of all vectors and matrices
  const double dt = 0.1; //Timestep
  const double g = 9.81; //Gravity

  const double pi = boost::math::constants::pi<double>();
  const double angle = pi / 4.0; //Radians. 45 degrees = pi / 4.0 radians


  //Gravity influences y and Vy
  const boost::numeric::ublas::matrix<double> control
    = Matrix::CreateMatrix(n,n,
      {
        0.0, 0.0, 0.0, 0.0,
        0.0, 0.0, 0.0, 0.0,
        0.0, 0.0, 1.0, 0.0,
        0.0, 0.0, 0.0, 1.0
      }
    );

  //Just a guess
  const boost::numeric::ublas::matrix<double> initial_covariance_estimate
    = Matrix::CreateMatrix(n,n,
      {
        1.0, 0.0, 0.0, 0.0,
        0.0, 1.0, 0.0, 0.0,
        0.0, 0.0, 1.0, 0.0,
        0.0, 0.0, 0.0, 1.0
      }
    );

  //Initial state estimates are a bit off on purpose
  const boost::numeric::ublas::vector<double> initial_state_estimate
    = Matrix::CreateVector( { 0.0, 100.0 * std::cos(angle), 500.0, 100.0 * sin(angle) } );

  //Shot from the cannon with velocity 100.0 at an angle
  const boost::numeric::ublas::vector<double> initial_state
    = Matrix::CreateVector( { 0.0, 100.0 * std::cos(angle), 0.0, 100.0 * sin(angle) } );

  //Gravity influences position and velocity in the vertical direction
  const std::vector<std::string> inputs
    = {
      boost::lexical_cast<std::string>( 0.0 ),
      boost::lexical_cast<std::string>( 0.0 ),
      boost::lexical_cast<std::string>( -0.5*g*dt*dt ),
      boost::lexical_cast<std::string>( -    g*dt    )
    };

  //ust an estimation
  const boost::numeric::ublas::matrix<double> estimated_measurement_noise
    = Matrix::CreateMatrix(n,n,
      {
        0.2, 0.0, 0.0, 0.0,
        0.0, 0.2, 0.0, 0.0,
        0.0, 0.0, 0.2, 0.0,
        0.0, 0.0, 0.0, 0.2
      }
    );

  //Can observe all positions and speeds
  const boost::numeric::ublas::matrix<double> observation
    = Matrix::CreateMatrix(n,n,
      {
        1.0, 0.0, 0.0, 0.0,
        0.0, 1.0, 0.0, 0.0,
        0.0, 0.0, 1.0, 0.0,
        0.0, 0.0, 0.0, 1.0
      }
    );

  //Due to this being a simulation, one can safely assume no process noise
  const boost::numeric::ublas::matrix<double> estimated_process_noise_covariance
    = Matrix::CreateMatrix(n,n,
      {
        0.0, 0.0, 0.0, 0.0,
        0.0, 0.0, 0.0, 0.0,
        0.0, 0.0, 0.0, 0.0,
        0.0, 0.0, 0.0, 0.0
      }
    );

  //Noise in GPS and speedometer in cannonball
  const boost::numeric::ublas::vector<double> real_measurement_noise
    = Matrix::CreateVector( { 30.0, 30.0, 30.0, 30.0 } );

  //Due to this being a simulation, put in no process noise
  const boost::numeric::ublas::vector<double> real_process_noise
    = Matrix::CreateVector( { 0.0, 0.0, 0.0, 0.0 } );


  //Velocities influence positions
  const boost::numeric::ublas::matrix<double> state_transition
    = Matrix::CreateMatrix(n,n,
      { //Beware: appears as transposition of real matrix
        1.0, 0.0, 0.0, 0.0,
         dt, 1.0, 0.0, 0.0,
        0.0, 0.0, 1.0, 0.0,
        0.0, 0.0, dt , 1.0
      }
    );

  const boost::shared_ptr<const StandardKalmanFilterParameters> kalman_filter_parameters(
    new StandardKalmanFilterParameters(
    control,
    estimated_measurement_noise,
    estimated_process_noise_covariance,
    initial_covariance_estimate,
    initial_state_estimate,
    observation,
    state_transition)
  );

  const std::vector<std::string> state_names = { "x", "Vx", "y", "Vy" };

  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> white_noise_system_parameters(
    new StandardWhiteNoiseSystemParameters(
    control,
    initial_state,
    real_measurement_noise,
    real_process_noise,
    state_transition)
  );

  assert(state_names.size() == inputs.size());
  assert(state_names.size() == kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(state_names.size() == white_noise_system_parameters->GetInitialState().size());

  std::stringstream context_stream;
  context_stream
    << "<h1>" << title << "</h1>" << '\n'
    << "<p>This is an example from <a href=\"http://greg.czerniak.info/guides/kalman1\">Greg Czerniak's tutorial</a></p>" << '\n'
    << "<p>The context is a cannonball lauched from a cannon.<p>" << '\n'
    << "<ul>" << '\n'
    << "  <li>State names: " << DisplayAsUblasVector(state_names) << ": horizontal position (m), horizontal velocity (m/s), vertical position (m), vertical velocity (m)</li>" << '\n'
    << "  <li>Initial state, real: " << initial_state << ": the cannon shoots the ball from an angle of 45 degrees at an initial speed of 100 m/s</li>" << '\n'
    << "  <li>Initial state, estimate: " << initial_state_estimate << ": set this value off on purpose, to see the Kalman filter converge</li>" << '\n'
    << "  <li>Input: " << DisplayAsUblasVector(inputs) << ": gravity will change the vertical velocity by 9.81 m/(s^2)</li>" << '\n'
    << "  <li>Control: " << control << ": gravity influences the vertical velocity only</li>" << '\n'
    << "  <li>Observation: " << observation << ": all states are observed directly with a GPS and an internal speedometer</li>" << '\n'
    << "  <li>State transition: " << state_transition << ": positions are increased by their velocities. Velocities remain constant without input</li>" << '\n'
    << "  <li>Measurement noise, real: " << real_measurement_noise << ": just some low value</li>" << '\n'
    << "  <li>Measurement noise, estimate: " << estimated_measurement_noise << ": just some low value</li>" << '\n'
    << "  <li>Process noise, real: " << real_process_noise << ": just some low value</li>" << '\n'
    << "  <li>Process noise, estimated covariance: " << estimated_process_noise_covariance << ": just some low value</li>" << '\n'
    << "  <li>Initial covariance estimate: " << initial_covariance_estimate << ": just some low value</li>" << '\n'
    << "</ul>";
  const std::string context = context_stream.str();

  std::unique_ptr<KalmanFilterExample> example(
    new KalmanFilterExample(
      title,
      context,
      inputs,
      kalman_filter_parameters,
      number_of_timesteps,
      state_names,
      white_noise_system_parameters
    )
  );
  assert(example);
  return example;
}

std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample3()
{
  const std::string title = "Spring";
  const std::string context
    = "<h1>" + title + "</h1>";

  //Use spring system: a mass is lying on a frictionless surface and is connected to two horizontal springs
  const int n = 2; //Size of vectors and matrices
  const double dt = 0.1; //Timestep
  const double k = 1.0; //Spring constant
  const double mass = 1.0; //Mass

  const int number_of_timesteps = 1000;

  //No input used, so control matrix can be zeroes only
  const boost::numeric::ublas::matrix<double> control
    = Matrix::CreateMatrix(n,n,
      {
        0.0, 0.0,
        0.0, 0.0
      }
    );

  //Just a guess
  const boost::numeric::ublas::matrix<double> initial_covariance_estimate
    = Matrix::CreateMatrix(n,n,
      {
        1.0, 0.0,
        0.0, 1.0
      }
    );

  //Initial state estimates are a bit off on purpose
  const boost::numeric::ublas::vector<double> initial_state_estimate
    = Matrix::CreateVector( { -10.0, -1.0 } );

  //From a perfect standstill at a certain position
  const boost::numeric::ublas::vector<double> initial_state
    = Matrix::CreateVector( { 10.0, 0.0 } );

  //There is no input supplied
  const std::vector<std::string> inputs
    = {
      boost::lexical_cast<std::string>( 0.0 ),
      boost::lexical_cast<std::string>( 0.0 )
    };

  const boost::numeric::ublas::matrix<double> estimated_measurement_noise
    = Matrix::CreateMatrix(n,n,
      {
        1.0, 0.0,
        0.0, 1.0
      }
    );

  //Observe the position of the spring only
  const boost::numeric::ublas::matrix<double> observation
    = Matrix::CreateMatrix(n,n,
      {
        1.0, 0.0,
        0.0, 0.0
      }
    );

  const boost::numeric::ublas::matrix<double> estimated_process_noise_covariance
    = Matrix::CreateMatrix(n,n,
      {
        1.0, 0.0,
        0.0, 1.0
      }
    );

  //Some noise
  const boost::numeric::ublas::vector<double> real_measurement_noise
    = Matrix::CreateVector( { 1.0, 1.0 } );

  //Simulation, so process noise is zero
  const boost::numeric::ublas::vector<double> real_process_noise
    = Matrix::CreateVector( { 0.0, 0.0 } );

  const std::vector<std::string> state_names = { "x", "v" };

  const boost::numeric::ublas::matrix<double> state_transition
    = boost::numeric::ublas::trans(Matrix::CreateMatrix(n,n,
      { //As on paper
        1.0,   -dt*k/mass,
        dt , 0.99
      }
    ));


  const boost::shared_ptr<const StandardKalmanFilterParameters> kalman_filter_parameters(
    new StandardKalmanFilterParameters(
    control,
    estimated_measurement_noise,
    estimated_process_noise_covariance,
    initial_covariance_estimate,
    initial_state_estimate,
    observation,
    state_transition)
  );

  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> white_noise_system_parameters(
    new StandardWhiteNoiseSystemParameters(
    control,
    initial_state,
    real_measurement_noise,
    real_process_noise,
    state_transition)
  );

  assert(state_names.size() == inputs.size());
  assert(state_names.size() == kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(state_names.size() == white_noise_system_parameters->GetInitialState().size());

  std::unique_ptr<KalmanFilterExample> example(
    new KalmanFilterExample(
      title,
      context,
      inputs,
      kalman_filter_parameters,
      number_of_timesteps,
      state_names,
      white_noise_system_parameters
    )
  );
  assert(example);
  return example;
}

std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample4()
{
  const std::string title = "Airhockey puck";
  const std::string context
    = "<h1>" + title + "</h1>";

  //Context: airhockey puck with a constant speed
  const int number_of_timesteps = 1000;

  const boost::numeric::ublas::matrix<double> control
    = Matrix::CreateMatrix(1,1, { 1.0 } );
  const boost::numeric::ublas::matrix<double> estimated_measurement_noise
    = Matrix::CreateMatrix(1,1, { 0.0000001 } );
  const boost::numeric::ublas::matrix<double> estimated_process_noise_covariance
    = Matrix::CreateMatrix(1,1, { 0.0 } );
  const boost::numeric::ublas::matrix<double> initial_covariance_estimate
    = Matrix::CreateMatrix(1,1, { 0.0 } );
  const boost::numeric::ublas::vector<double> initial_state_estimate
    = Matrix::CreateVector( { 0.0 } );
  const boost::numeric::ublas::matrix<double> observation
    = Matrix::CreateMatrix(1,1, { 1.0 } );
  const boost::numeric::ublas::matrix<double> state_transition
    = Matrix::CreateMatrix(1,1, {1.0} );
  const boost::numeric::ublas::vector<double> initial_state
    = Matrix::CreateVector( { 0.0 } );
  const boost::numeric::ublas::vector<double> real_measurement_noise
    = Matrix::CreateVector( { 0.0 } );
  const boost::numeric::ublas::vector<double> real_process_noise
    = Matrix::CreateVector( { 0.0 } );


  const std::vector<std::string> inputs
    = {
      boost::lexical_cast<std::string>( 1.0 )
    };

  const boost::shared_ptr<const StandardKalmanFilterParameters> kalman_filter_parameters(
    new StandardKalmanFilterParameters(
    control,
    estimated_measurement_noise,
    estimated_process_noise_covariance,
    initial_covariance_estimate,
    initial_state_estimate,
    observation,
    state_transition)
  );

  const std::vector<std::string> state_names = { "x" };

  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> white_noise_system_parameters(
    new StandardWhiteNoiseSystemParameters(
    control,
    initial_state,
    real_measurement_noise,
    real_process_noise,
    state_transition)
  );

  assert(state_names.size() == inputs.size());
  assert(state_names.size() == kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(state_names.size() == white_noise_system_parameters->GetInitialState().size());

  std::unique_ptr<KalmanFilterExample> example(
    new KalmanFilterExample(
      title,
      context,
      inputs,
      kalman_filter_parameters,
      number_of_timesteps,
      state_names,
      white_noise_system_parameters
    )
  );
  assert(example);
  return example;
}



std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample5()
{
  const std::string title = "Accelerating car (3 states)";
  const std::string context
    = "<h1>" + title + "</h1>";

  //Another accelerating car
  const int n = 3;
  const double force =  1000.0; //Newton
  const double mass  = 10000.0; //kilogram
  const double dt = 0.1; //Timestep
  const double acc = dt * force / mass;

  const int number_of_timesteps = 1000;

  //A gas pedal only influences acceleration
  const boost::numeric::ublas::matrix<double> control
    = Matrix::CreateMatrix(n,n,
    {
      0.0,0.0,0.0,
      0.0,0.0,0.0,
      0.0,0.0,acc
    } );

  //Just a guess
  const boost::numeric::ublas::matrix<double> initial_covariance_estimate
    = Matrix::CreateMatrix(n,n,
      {
        1.0,0.0,0.0,
        0.0,1.0,0.0,
        0.0,0.0,1.0
      } );

  //Initial state estimates are a bit off on purpose
  const boost::numeric::ublas::vector<double> initial_state_estimate
    = Matrix::CreateVector( { 10.0,1.0,-1.0 } );

  //From exact standstill
  const boost::numeric::ublas::vector<double> initial_state
    = Matrix::CreateVector( { 0.0,0.0,0.0 } );

  //Only (pessimistic) normal noise in GPS, speedometer and accelerometer
  const boost::numeric::ublas::matrix<double> estimated_measurement_noise
    = Matrix::CreateMatrix(n,n,
    {
      100.0,  0.0,  0.0,
        0.0,100.0,  0.0,
        0.0,  0.0,100.0
    } );

  //Observe all: GPS, speedometer, accelerometer
  const boost::numeric::ublas::matrix<double> observation
    = Matrix::CreateMatrix(n,n,
    {
      1.0,0.0,0.0,
      0.0,1.0,0.0,
      0.0,0.0,1.0
    } );

  const boost::numeric::ublas::matrix<double> estimated_process_noise_covariance
    = Matrix::CreateMatrix(n,n, {
      0.1,0.0,0.0,
      0.0,0.1,0.0,
      0.0,0.0,0.1,
    } ); //Pessimistic estimate

  //Only normal noise in GPS, speedometer has enormous noise as if defect (yet cannot be 0.0)
  const boost::numeric::ublas::vector<double> real_measurement_noise
    = Matrix::CreateVector( { 10.0, 10.0, 10.0 } );

  const boost::numeric::ublas::vector<double> real_process_noise
    = Matrix::CreateVector( { 0.01, 0.01, 0.01 } );

  //x(t+1) = x(t) + dt*v(t) + 0.5*(dt^2)*a(t)
  //v(t+1) =           v(t) +     (dt  )*a(t)
  //a(t+1) =                             a(t)
  const boost::numeric::ublas::matrix<double> state_transition
    = boost::numeric::ublas::trans(Matrix::CreateMatrix(n,n,
      { //Shown as on paper
        1.0,  dt, 0.5*dt*dt,
        0.0, 1.0,        dt,
        0.0, 0.0,       1.0
      } ));

  //A gas pedal only influences acceleration
  const std::vector<std::string> inputs = { "0.0", "0.0", "1.0" };

  const boost::shared_ptr<const StandardKalmanFilterParameters> kalman_filter_parameters(
    new StandardKalmanFilterParameters(
    control,
    estimated_measurement_noise,
    estimated_process_noise_covariance,
    initial_covariance_estimate,
    initial_state_estimate,
    observation,
    state_transition)
  );

  const std::vector<std::string> state_names = { "x", "v", "a" };

  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> white_noise_system_parameters(
    new StandardWhiteNoiseSystemParameters(
    control,
    initial_state,
    real_measurement_noise,
    real_process_noise,
    state_transition)
  );

  assert(control.size1() > 0);
  assert(control.size2() > 0);
  assert(state_names.size() == inputs.size());
  assert(state_names.size() == kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(state_names.size() == white_noise_system_parameters->GetInitialState().size());
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetControl(),white_noise_system_parameters->GetControl()));
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetStateTransition(),white_noise_system_parameters->GetStateTransition()));

  std::unique_ptr<KalmanFilterExample> example(
    new KalmanFilterExample(
      title,
      context,
      inputs,
      kalman_filter_parameters,
      number_of_timesteps,
      state_names,
      white_noise_system_parameters
    )
  );
  assert(example);
  return example;
}

std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample6()
{
  const std::string title = "Exponential decay";
  //One state
  const int n = 1;
  //As small as possible
  const double epsilon = 0.00000000001;
  //Reach a 10% value after 1000 timesteps with the closed-form solution
  const double gamma = -std::log(0.1)/1000.0;
  //Reach a 10% value after 1000 timesteps with the recurrence equation

  const double e = boost::math::constants::e<double>();
  const double tau = std::pow(e,std::log(0.1) / 1000.0);

  const int number_of_timesteps = 1000;

  //Just a variable name
  const std::vector<std::string> state_names = { "x" };

  //Input does not change state
  const boost::numeric::ublas::matrix<double> control
    = Matrix::CreateMatrix(n,n, { 0.0 } );

  //As small as possible
  const boost::numeric::ublas::matrix<double> initial_covariance_estimate
    = Matrix::CreateMatrix(n,n, { epsilon } );

  //From 100%
  const boost::numeric::ublas::vector<double> initial_state
    = Matrix::CreateVector( { 1.0 } );

  //Initial state estimate is correct on purpose
  const boost::numeric::ublas::vector<double> initial_state_estimate = initial_state;

  //As small as possible
  const boost::numeric::ublas::matrix<double> estimated_measurement_noise
    = Matrix::CreateMatrix(n,n, { epsilon } );

  //Observe directly
  const boost::numeric::ublas::matrix<double> observation
    = Matrix::CreateMatrix(n,n, { 1.0 } );

  //As small as possible
  const boost::numeric::ublas::matrix<double> estimated_process_noise_covariance
    = Matrix::CreateMatrix(n,n, { epsilon } );

  //As small as possible
  const boost::numeric::ublas::vector<double> real_measurement_noise
    = Matrix::CreateVector( { epsilon } );

  //As small as possible
  const boost::numeric::ublas::vector<double> real_process_noise
    = Matrix::CreateVector( { epsilon } );

  //Reach a 10% value after 1000 timesteps with the recurrence equation
  const boost::numeric::ublas::matrix<double> state_transition
    = boost::numeric::ublas::trans(Matrix::CreateMatrix(n,n, { tau } ));

  //Reach a 10% value after 1000 timesteps with the closed-form solution
  const std::string input = "exp(-" + boost::lexical_cast<std::string>(gamma) + "*(t+1))";
  const std::vector<std::string> inputs = { input };

  const boost::shared_ptr<const StandardKalmanFilterParameters> kalman_filter_parameters(
    new StandardKalmanFilterParameters(
    control,
    estimated_measurement_noise,
    estimated_process_noise_covariance,
    initial_covariance_estimate,
    initial_state_estimate,
    observation,
    state_transition)
  );

  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> white_noise_system_parameters(
    new StandardWhiteNoiseSystemParameters(
    control,
    initial_state,
    real_measurement_noise,
    real_process_noise,
    state_transition)
  );

  assert(control.size1() > 0);
  assert(control.size2() > 0);
  assert(state_names.size() == inputs.size());
  assert(state_names.size() == kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(state_names.size() == white_noise_system_parameters->GetInitialState().size());
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetControl(),white_noise_system_parameters->GetControl()));
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetStateTransition(),white_noise_system_parameters->GetStateTransition()));

  std::stringstream context_stream;
  context_stream
    << "<h1>" << title << "</h1>" << '\n'
    << "<p>&nbsp;</p>" << '\n'
    << "<p>" << '\n'
    << "  This is more of a mathematical example. It shows that" << '\n'
    << "  a recurrence equation and its closed-form solution for exponential" << '\n'
    << "  decay are equivalent" << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  The closed-form solution of exponential decay is:" << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  f(t) = e^(-gamma*t)" << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  Where t is the time step and gamma a constant, which is equal to -ln(0.1)/1000 = 0.0023025850929940454." << '\n'
    << "  This value of gamma lets the exponential decay reach 10% after 1000 timesteps." << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  The recurrence equation of exponential decay is:" << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  f(t+1) = tau * f(t)" << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  Where t is the time step and tau a constant, which is equal to e^(ln(0.1)/1000) = 0.99770006382255327." << '\n'
    << "  This value of tau lets the exponential decay reach 10% after 1000 timesteps." << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  The closed-form solution is plotted as the input, the recurrence equation as the state." << '\n'
    << "  Because the control matrix contains a zero only, the input has no influence on the state." << '\n'
    << "  For the cleanest look, all noise was set to a low value." << '\n'
    << "</p>" << '\n'
    << "<p>&nbsp;</p>" << '\n'
    << "<ul>" << '\n'
    << "  <li>State names: " << DisplayAsUblasVector(state_names) << ": just a variable without meaning</li>" << '\n'
    << "  <li>Initial state, real: " << initial_state << ": decay starting from 1.0</li>" << '\n'
    << "  <li>Initial state, estimate: " << initial_state_estimate << ": set this value to the real value, there is no need to do estimation</li>" << '\n'
    << "  <li>Input: " << DisplayAsUblasVector(inputs) << ": the closed-form solution of exponential decay. t must be increased by one timestep, because a Kalman Filter estimates after an update</li>" << '\n'
    << "  <li>Control: " << control << ": input must have no effect in changing the state to do a valid comparison</li>" << '\n'
    << "  <li>Observation: " << observation << ": the value is observed directly</li>" << '\n'
    << "  <li>State transition: " << state_transition << ": the recurrence equation of exponential decay</li>" << '\n'
    << "  <li>Measurement noise, real: " << real_measurement_noise << ": some very low value</li>" << '\n'
    << "  <li>Measurement noise, estimate: " << estimated_measurement_noise << ": some very low value</li>" << '\n'
    << "  <li>Process noise, real: " << real_process_noise << ": some very low value</li>" << '\n'
    << "  <li>Process noise, estimated covariance: " << estimated_process_noise_covariance << ": some very low value</li>" << '\n'
    << "  <li>Initial covariance estimate: " << initial_covariance_estimate << ": some very low value</li>" << '\n'
    << "</ul>"
    ;
  const std::string context = context_stream.str();

  std::unique_ptr<KalmanFilterExample> example(
    new KalmanFilterExample(
      title,
      context,
      inputs,
      kalman_filter_parameters,
      number_of_timesteps,
      state_names,
      white_noise_system_parameters
    )
  );
  assert(example);
  return example;
}

std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample7()
{
  const std::string title = "Harmonic oscillation (two states)";
  //Two states
  const int n = 2;
  //As small as possible
  const double e = 0.00000000001;
  //Period of 100 timesteps

  const double pi = boost::math::constants::pi<double>();
  const double angular_frequency = 2.0 * pi / 100.0;

  //Correct for floating point rounding errors that will increase the amplitude.
  //This value is found by experimenting
  const double correction = 0.998026148;

  const int number_of_timesteps = 1000;

  //Name of the functions
  const std::vector<std::string> state_names = { "sin", "cos" };

  //Input does not change state
  const boost::numeric::ublas::matrix<double> control
    = Matrix::CreateMatrix(n,n, { 0.0, 0.0, 0.0, 0.0 } );

  //As small as possible
  const boost::numeric::ublas::matrix<double> initial_covariance_estimate
    = Matrix::CreateMatrix(n,n, { e,0.0,0.0,e } );

  //Correct
  const boost::numeric::ublas::vector<double> initial_state
    = Matrix::CreateVector( { 0.0, 1.0 } );

  //Initial state estimate is correct on purpose
  const boost::numeric::ublas::vector<double> initial_state_estimate = initial_state;

  //As small as possible
  const boost::numeric::ublas::matrix<double> estimated_measurement_noise
    = Matrix::CreateMatrix(n,n, { e,0.0,0.0,e } );

  //Observe directly
  const boost::numeric::ublas::matrix<double> observation
    = Matrix::CreateMatrix(n,n, { 1.0,0.0,0.0,1.0 } );

  //As small as possible
  const boost::numeric::ublas::matrix<double> estimated_process_noise_covariance
    = Matrix::CreateMatrix(n,n, { e,0.0,0.0,e } );

  //As small as possible
  const boost::numeric::ublas::vector<double> real_measurement_noise
    = Matrix::CreateVector( { e,e } );

  //As small as possible
  const boost::numeric::ublas::vector<double> real_process_noise
    = Matrix::CreateVector( { e,e } );

  //Reach a 10% value after 1000 timesteps with the recurrence equation
  const boost::numeric::ublas::matrix<double> state_transition
    = boost::numeric::ublas::trans(Matrix::CreateMatrix(n,n,
      {
                correction, angular_frequency,
        -angular_frequency,        correction
      } ));

  //Reach a 10% value after 1000 timesteps with the closed-form solution
  const std::string input1 = "sin(" + boost::lexical_cast<std::string>(angular_frequency) + "*(t+1))";
  const std::string input2 = "cos(" + boost::lexical_cast<std::string>(angular_frequency) + "*(t+1))";
  const std::vector<std::string> inputs = { input1, input2 };

  const boost::shared_ptr<const StandardKalmanFilterParameters> kalman_filter_parameters(
    new StandardKalmanFilterParameters(
    control,
    estimated_measurement_noise,
    estimated_process_noise_covariance,
    initial_covariance_estimate,
    initial_state_estimate,
    observation,
    state_transition)
  );

  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> white_noise_system_parameters(
    new StandardWhiteNoiseSystemParameters(
    control,
    initial_state,
    real_measurement_noise,
    real_process_noise,
    state_transition)
  );

  assert(control.size1() > 0);
  assert(control.size2() > 0);
  assert(state_names.size() == inputs.size());
  assert(state_names.size() == kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(state_names.size() == white_noise_system_parameters->GetInitialState().size());
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetControl(),white_noise_system_parameters->GetControl()));
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetStateTransition(),white_noise_system_parameters->GetStateTransition()));

  std::stringstream context_stream;
  context_stream
    << "<h1>" << title << "</h1>" << '\n'
    << "<p>&nbsp;</p>" << '\n'
    << "<p>" << '\n'
    << "  This is more of a mathematical example. It shows that" << '\n'
    << "  a recurrence equation and its closed-form solution for a harmonic" << '\n'
    << "  oscillation are equivalent" << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  The closed-form solution of a harmonic oscillation is:" << '\n'
    << "</p>" << '\n'
    << "<code>" << '\n'
    << "x(t) = sin(angular_frequency*t)<br/>" << '\n'
    << "v(t) = cos(angular_frequency*t)<br/>" << '\n'
    << "</code>" << '\n'
    << "<p>" << '\n'
    << "  The period is set to 100 timesteps," << '\n'
    << "  so the angular frequency equals 2*pi radian per period = 2 * pi / 100 = 0.0628." << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  The recurrence equation of a harmonic oscillation is" << '\n'
    << "</p>" << '\n'
    << "<code>" << '\n'
    << "x(t+1) = c * x(t) + angular_frequency * v(t)<br/>" << '\n'
    << "v(t+1) = c * v(t) - angular_frequency * x(t)<br/>" << '\n'
    << "</code>" << '\n'
    << "<p>" << '\n'
    << "  Where c is a correction close to 1.0, to prevent the amplitude from increasing beyond 1" << '\n'
    << "  (probably due to rounding errors)" << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  The closed-form solution is plotted as the input, the recurrence equation as the state." << '\n'
    << "  Because the control matrix contains a zero only, the input has no influence on the state." << '\n'
    << "  For the cleanest look, all noise was set to a low value." << '\n'
    << "</p>" << '\n'
    << "<p>&nbsp;</p>" << '\n'
    << "<ul>" << '\n'
    << "  <li>State names: " << DisplayAsUblasVector(state_names) << ": position (e.g. meters) and velocity (e.g. meters per second)</li>" << '\n'
    << "  <li>Initial state, real: " << initial_state << ": a sine starts at 0.0, a cosine starts at 1.0</li>" << '\n'
    << "  <li>Initial state, estimate: " << initial_state_estimate << ": set this value to the real value, there is no need to do estimation</li>" << '\n'
    << "  <li>Input: " << DisplayAsUblasVector(inputs) << ": the closed-form solution of a harmonic oscillation. t must be increased by one timestep, because a Kalman Filter estimates after an update</li>" << '\n'
    << "  <li>Control: " << control << ": input must have no effect in changing the state to do a valid comparison</li>" << '\n'
    << "  <li>Observation: " << observation << ": the value is observed directly</li>" << '\n'
    << "  <li>State transition: " << state_transition << ": the recurrence equation of a harmonic oscillation</li>" << '\n'
    << "  <li>Measurement noise, real: " << real_measurement_noise << ": some very low value</li>" << '\n'
    << "  <li>Measurement noise, estimate: " << estimated_measurement_noise << ": some very low value</li>" << '\n'
    << "  <li>Process noise, real: " << real_process_noise << ": some very low value</li>" << '\n'
    << "  <li>Process noise, estimated covariance: " << estimated_process_noise_covariance << ": some very low value</li>" << '\n'
    << "  <li>Initial covariance estimate: " << initial_covariance_estimate << ": some very low value</li>" << '\n'
    << "</ul>"
    ;
  const std::string context = context_stream.str();

  std::unique_ptr<KalmanFilterExample> example(
    new KalmanFilterExample(
      title,
      context,
      inputs,
      kalman_filter_parameters,
      number_of_timesteps,
      state_names,
      white_noise_system_parameters
    )
  );
  assert(example);
  return example;
}

std::unique_ptr<ribi::kalman::KalmanFilterExample> ribi::kalman::KalmanFilterExample::CreateExample8()
{
  const std::string title = "Harmonic oscillation (three states)";
  //Two states
  const int n = 3;
  //As small as possible
  const double e = 0.00000000001;
  //Period of 100 timesteps

  const double pi = boost::math::constants::pi<double>();
  const double angular_frequency = 2.0 * pi / 100.0;
  //Correct for floating point rounding errors that will increase the amplitude.
  //This value is found by experimenting
  const double correction = 0.998026148;

  const int number_of_timesteps = 1000;

  //Name of the functions
  const std::vector<std::string> state_names = { "x", "v", "a" };

  //Input does not change state
  const boost::numeric::ublas::matrix<double> control
    = Matrix::CreateMatrix(n,n,std::vector<double>(n*n,0.0));

  //As small as possible
  const boost::numeric::ublas::matrix<double> initial_covariance_estimate
    = Matrix::CreateMatrix(n,n, { e,0.0,0.0,0.0,e,0.0,0.0,0.0,e } );

  //Correct
  const boost::numeric::ublas::vector<double> initial_state
    = Matrix::CreateVector( { 0.0, angular_frequency, -angular_frequency*angular_frequency } );

  //Initial state estimate is correct on purpose
  const boost::numeric::ublas::vector<double> initial_state_estimate = initial_state;

  //As small as possible
  const boost::numeric::ublas::matrix<double> estimated_measurement_noise
    = Matrix::CreateMatrix(n,n, { e,0.0,0.0,0.0,e,0.0,0.0,0.0,e } );

  //Observe directly
  const boost::numeric::ublas::matrix<double> observation
    = Matrix::CreateMatrix(n,n, { 1.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,1.0 } );

  //As small as possible
  const boost::numeric::ublas::matrix<double> estimated_process_noise_covariance
    = Matrix::CreateMatrix(n,n, { e,0.0,0.0,0.0,e,0.0,0.0,0.0,e } );

  //As small as possible
  const boost::numeric::ublas::vector<double> real_measurement_noise
    = Matrix::CreateVector( { e,e,e } );

  //As small as possible
  const boost::numeric::ublas::vector<double> real_process_noise
    = Matrix::CreateVector( { e,e,e } );

  //Reach a 10% value after 1000 timesteps with the recurrence equation
  const boost::numeric::ublas::matrix<double> state_transition
    = boost::numeric::ublas::trans(Matrix::CreateMatrix(n,n,
      {
                                  correction, angular_frequency,angular_frequency*angular_frequency,
                          -angular_frequency,        correction,                  angular_frequency,
        -angular_frequency*angular_frequency,               0.0,                                0.0
      } ));

  //Reach a 10% value after 1000 timesteps with the closed-form solution
  const std::string input1
    = "sin(" + boost::lexical_cast<std::string>(angular_frequency) + "*(t+1))";
  const std::string input2
    = boost::lexical_cast<std::string>(angular_frequency)
    + "*cos(" + boost::lexical_cast<std::string>(angular_frequency)
    + "*(t+1))";
  const std::string input3
    = "-"
    + boost::lexical_cast<std::string>(angular_frequency)
    + "*"
    + boost::lexical_cast<std::string>(angular_frequency)
    + "*sin(" + boost::lexical_cast<std::string>(angular_frequency) + "*(t+1))";
  const std::vector<std::string> inputs = { input1, input2, input3 };

  const boost::shared_ptr<const StandardKalmanFilterParameters> kalman_filter_parameters(
    new StandardKalmanFilterParameters(
    control,
    estimated_measurement_noise,
    estimated_process_noise_covariance,
    initial_covariance_estimate,
    initial_state_estimate,
    observation,
    state_transition)
  );

  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> white_noise_system_parameters(
    new StandardWhiteNoiseSystemParameters(
    control,
    initial_state,
    real_measurement_noise,
    real_process_noise,
    state_transition)
  );

  assert(control.size1() > 0);
  assert(control.size2() > 0);
  assert(state_names.size() == inputs.size());
  assert(state_names.size() == kalman_filter_parameters->GetInitialStateEstimate().size());
  assert(state_names.size() == white_noise_system_parameters->GetInitialState().size());
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetControl(),white_noise_system_parameters->GetControl()));
  assert(Matrix::MatricesAreEqual(kalman_filter_parameters->GetStateTransition(),white_noise_system_parameters->GetStateTransition()));

  std::stringstream context_stream;
  context_stream
    << "<h1>" << title << "</h1>" << '\n'
    << "<p>&nbsp;</p>" << '\n'
    << "<p>" << '\n'
    << "  This is more of a mathematical example. It shows that" << '\n'
    << "  a recurrence equation and its closed-form solution for a harmonic" << '\n'
    << "  oscillation (three states) are equivalent" << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  The closed-form solution of a harmonic oscillation is:" << '\n'
    << "</p>" << '\n'
    << "<code>" << '\n'
    << "x(t) = sin(angular_frequency*t)<br/>" << '\n'
    << "v(t) = cos(angular_frequency*t)<br/>" << '\n'
    << "</code>" << '\n'
    << "<p>" << '\n'
    << "  The period is set to 100 timesteps," << '\n'
    << "  so the angular frequency equals 2*pi radian per period = 2 * pi / 100 = 0.0628." << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  The recurrence equation of a harmonic oscillation is" << '\n'
    << "</p>" << '\n'
    << "<code>" << '\n'
    << "x(t+1) = c * x(t) + angular_frequency * v(t)<br/>" << '\n'
    << "v(t+1) = c * v(t) - angular_frequency * x(t)<br/>" << '\n'
    << "</code>" << '\n'
    << "<p>" << '\n'
    << "  Where c is a correction close to 1.0, to prevent the amplitude from increasing beyond 1" << '\n'
    << "  (probably due to rounding errors)" << '\n'
    << "</p>" << '\n'
    << "<p>" << '\n'
    << "  The closed-form solution is plotted as the input, the recurrence equation as the state." << '\n'
    << "  Because the control matrix contains a zero only, the input has no influence on the state." << '\n'
    << "  For the cleanest look, all noise was set to a low value." << '\n'
    << "</p>" << '\n'
    << "<p>&nbsp;</p>" << '\n'
    << "<ul>" << '\n'
    << "  <li>State names: " << DisplayAsUblasVector(state_names) << ": position (e.g. meters) and velocity (e.g. meters per second)</li>" << '\n'
    << "  <li>Initial state, real: " << initial_state << ": a sine starts at 0.0, a cosine starts at 1.0</li>" << '\n'
    << "  <li>Initial state, estimate: " << initial_state_estimate << ": set this value to the real value, there is no need to do estimation</li>" << '\n'
    << "  <li>Input: " << DisplayAsUblasVector(inputs) << ": the closed-form solution of a harmonic oscillation. t must be increased by one timestep, because a Kalman Filter estimates after an update</li>" << '\n'
    << "  <li>Control: " << control << ": input must have no effect in changing the state to do a valid comparison</li>" << '\n'
    << "  <li>Observation: " << observation << ": the value is observed directly</li>" << '\n'
    << "  <li>State transition: " << state_transition << ": the recurrence equation of a harmonic oscillation</li>" << '\n'
    << "  <li>Measurement noise, real: " << real_measurement_noise << ": some very low value</li>" << '\n'
    << "  <li>Measurement noise, estimate: " << estimated_measurement_noise << ": some very low value</li>" << '\n'
    << "  <li>Process noise, real: " << real_process_noise << ": some very low value</li>" << '\n'
    << "  <li>Process noise, estimated covariance: " << estimated_process_noise_covariance << ": some very low value</li>" << '\n'
    << "  <li>Initial covariance estimate: " << initial_covariance_estimate << ": some very low value</li>" << '\n'
    << "</ul>"
    ;
  const std::string context = context_stream.str();

  std::unique_ptr<KalmanFilterExample> example(
    new KalmanFilterExample(
      title,
      context,
      inputs,
      kalman_filter_parameters,
      number_of_timesteps,
      state_names,
      white_noise_system_parameters
    )
  );
  assert(example);
  return example;
}

std::string ribi::kalman::KalmanFilterExample::DisplayAsUblasVector(const std::vector<std::string>& v)
{
  std::stringstream s;
  s << "[" << v.size() << "](";
  for(const std::string& str: v) { s << str << ","; }
  //Replace trailing comma with a closing bracket
  std::string str = s.str();
  str[str.size() - 1] = ')';
  return str;
}

 

 

 

 

 

./CppKalmanFilter/kalmanfilterexperiment.h

 

#ifndef KALMANFILTEREXPERIMENT_H
#define KALMANFILTEREXPERIMENT_H

#include <vector>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/checked_delete.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include "steadystatekalmanfilterparameters.h"
#include "steadystatekalmanfiltercalculationelements.h"
//#include "standardwhitenoisesystemparameters.h"
#include "kalmanfilter.h"
#include "whitenoisesystem.h"

#pragma GCC diagnostic pop

namespace ribi {
namespace kalman {

///A KalmanFilterExperiment
struct KalmanFilterExperiment
{
  KalmanFilterExperiment(const KalmanFilterExperiment&) = delete;
  KalmanFilterExperiment& operator=(const KalmanFilterExperiment&) = delete;

  explicit KalmanFilterExperiment(
    const int time,
    const std::vector<std::string>& input,
    const boost::shared_ptr<KalmanFilter> kalman_filter,
    const std::vector<std::string>& state_names,
    const boost::shared_ptr<WhiteNoiseSystem>& white_noise_system,
    const std::string& context = ""
  );

  ///Obtain the calculation elements of each timestep
  const std::vector<boost::shared_ptr<KalmanFilterCalculationElements> >& GetCalculationElements() const
    { return m_calculation_elements; }

  ///Obtain the HTML description of the context of this experiment
  const std::string& GetContext() const { return m_context; }

  ///Obtain the predicted states
  ///The states are stored as a chronological collection (the std::vector) of states (the ublas::vector)
  std::vector<boost::numeric::ublas::vector<double>> GetPredictedStates() const;

  ///Obtain the inputs of each timestep
  ///The states are stored as a chronological collection (the std::vector) of inputs (the ublas::vector)
  const std::vector<boost::numeric::ublas::vector<double> >& GetInputs() const
    { return m_inputs; }

  ///Obtain the Kalman filter
  const boost::shared_ptr<KalmanFilter>& GetKalmanFilter() const { return m_kalman_filter; }

  ///Obtain the measured states, that is, measured with noise
  ///The states are stored as a chronological collection (the std::vector) of states (the ublas::vector)
  std::vector<boost::numeric::ublas::vector<double>> GetMeasuredStates() const;

  ///Obtain the real states, that is, the values that would be measured without noise
  ///The states are stored as a chronological collection (the std::vector) of states (the ublas::vector)
  const std::vector<boost::numeric::ublas::vector<double> >& GetRealStates() const
    { return m_real_states; }

  ///Obtain the state names
  const std::vector<std::string>& GetStateNames() const
    { return m_state_names; }

  ///The white noise system
  const boost::shared_ptr<WhiteNoiseSystem>& GetWhiteNoiseSystem() const
    { return m_white_noise_system; }

  ///Check if the state of the class is valid
  bool IsValid() const;

  protected:

  ///Append a real state
  void AppendRealState(const boost::numeric::ublas::vector<double>& real_state);

  private:
  ///Can only be deleted by boost::checked_delete
  ~KalmanFilterExperiment() noexcept {}
  friend void boost::checked_delete<>(KalmanFilterExperiment*);

  static std::vector<boost::numeric::ublas::vector<double>> ParseInput(
    const std::vector<std::string>& input,
    const int n_timesteps);

  ///The calculation elements of each timestep
  std::vector<boost::shared_ptr<KalmanFilterCalculationElements>> m_calculation_elements;

  ///The HTML description of the context of this experiment
  const std::string m_context;

  ///The inputs of each timestep
  const std::vector<boost::numeric::ublas::vector<double>> m_inputs;

  ///The Kalman filter
  const boost::shared_ptr<KalmanFilter> m_kalman_filter;

  ///The real state, that is, the values that would be measured without noise
  std::vector<boost::numeric::ublas::vector<double> > m_real_states;

  ///State names
  const std::vector<std::string> m_state_names;

  ///The white noise system
  const boost::shared_ptr<WhiteNoiseSystem> m_white_noise_system;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTEREXPERIMENT_H

 

 

 

 

 

./CppKalmanFilter/kalmanfilterexperiment.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "kalmanfilterexperiment.h"

#include <cassert>
#include <boost/numeric/conversion/cast.hpp>

#include "kalmanfiltercalculationelementsfactory.h"
#include "modelfunctionparser.h"
#include "matrix.h"
#include "trace.h"
#pragma GCC diagnostic pop

ribi::kalman::KalmanFilterExperiment::KalmanFilterExperiment(
  const int time,
  const std::vector<std::string>& input_functions,
  const boost::shared_ptr<KalmanFilter> m_kalman_filter,
  const std::vector<std::string>& state_names,
  const boost::shared_ptr<WhiteNoiseSystem>& m_white_noise_system,
  const std::string& context
  )
  : m_calculation_elements{},
    m_context{context},
    m_inputs{ribi::kalman::KalmanFilterExperiment::ParseInput(input_functions,time)},
    m_kalman_filter{m_kalman_filter},
    m_real_states{},
    m_state_names{state_names},
    m_white_noise_system{m_white_noise_system}
{
  assert(m_kalman_filter);
  assert(this->GetKalmanFilter());
  assert(this->GetKalmanFilter()->GetParameters());
  assert(m_white_noise_system);
  assert(this->GetWhiteNoiseSystem());
  assert(this->GetWhiteNoiseSystem()->GetParameters());
  #ifndef NDEBUG
  {
    const std::size_t sz = state_names.size();
    assert(sz == state_names.size());
    assert(sz == this->GetKalmanFilter()->GetParameters()->GetControl().size1());
    assert(sz == this->GetKalmanFilter()->GetParameters()->GetControl().size2());
    assert(sz == this->GetKalmanFilter()->GetParameters()->GetInitialStateEstimate().size());
    assert(sz == this->GetKalmanFilter()->GetParameters()->GetObservation().size1());
    assert(sz == this->GetKalmanFilter()->GetParameters()->GetObservation().size2());
    assert(sz == this->GetKalmanFilter()->GetParameters()->GetStateTransition().size1());
    assert(sz == this->GetKalmanFilter()->GetParameters()->GetStateTransition().size2());
    assert(sz == GetWhiteNoiseSystem()->GetParameters()->GetControl().size1());
    assert(sz == GetWhiteNoiseSystem()->GetParameters()->GetControl().size2());
    assert(sz == GetWhiteNoiseSystem()->GetParameters()->GetInitialState().size());
    assert(sz == GetWhiteNoiseSystem()->GetParameters()->GetMeasurementNoise().size());
    assert(sz == GetWhiteNoiseSystem()->GetParameters()->GetProcessNoise().size());
    assert(sz == GetWhiteNoiseSystem()->GetParameters()->GetStateTransition().size1());
    assert(sz == GetWhiteNoiseSystem()->PeekAtRealState().size());
    assert(sz == input_functions.size());
  }
  #endif

  

  assert(Matrix::MatricesAreAboutEqual(
    m_kalman_filter->GetParameters()->GetControl(),
    m_white_noise_system->GetParameters()->GetControl()));
  assert(Matrix::MatricesAreAboutEqual(
    m_kalman_filter->GetParameters()->GetStateTransition(),
    m_white_noise_system->GetParameters()->GetStateTransition()));

  
  for (int i=0;i!=time;++i)
  {
    //Update reality, that is, let the real system (i.e. reality) go to its next state
    assert(i < boost::numeric_cast<int>(GetInputs().size()));
    const boost::numeric::ublas::vector<double>& input = GetInputs()[i];

    //assert(m_white_noise_system->GetCurrentState().size() == input.size());

    
    m_white_noise_system->GoToNextState(input);
    

    //Perform a noisy measurement
    const boost::numeric::ublas::vector<double> z_measured = m_white_noise_system->Measure();
    

    //Pass this measurement to the filter
    try
    {
      
      m_kalman_filter->SupplyMeasurementAndInput(z_measured,input);
      
    }
    catch (std::runtime_error& e)
    {
      //Happens when innovation covariance becomes degenerate
      //(that is, its determinant is zero)
      assert(this->IsValid() && "The experiment must end in a valid state");
      return;
    }
    catch (...)
    {
      assert(!"Should never get here");
    }

    this->AppendRealState(m_white_noise_system->PeekAtRealState());

    //Store
    const boost::shared_ptr<KalmanFilterCalculationElements> last_calculation
      = KalmanFilterCalculationElementsFactory().DeepCopy(m_kalman_filter->GetLastCalculation());
    m_calculation_elements.push_back(last_calculation);
  }
  assert(time == boost::numeric_cast<int>(m_calculation_elements.size()));
  assert(this->IsValid() && "The experiment must end in a valid state");
}

void ribi::kalman::KalmanFilterExperiment::AppendRealState(const boost::numeric::ublas::vector<double>& real_state)
{
  this->m_real_states.push_back(real_state);
}

std::vector<boost::numeric::ublas::vector<double> > ribi::kalman::KalmanFilterExperiment::GetMeasuredStates() const
{
  std::vector<boost::numeric::ublas::vector<double> > v;
  v.reserve(m_calculation_elements.size());
  const auto j = m_calculation_elements.end();
  for (auto i = m_calculation_elements.begin(); i!=j; ++i)
  {
    v.push_back( (*i)->GetMeasurement() );
  }
  return v;
}

std::vector<boost::numeric::ublas::vector<double> > ribi::kalman::KalmanFilterExperiment::GetPredictedStates() const
{
  std::vector<boost::numeric::ublas::vector<double> > v;
  v.reserve(m_calculation_elements.size());
  const auto j = m_calculation_elements.end();
  for (auto i = m_calculation_elements.begin(); i!=j; ++i)
  {
    const boost::shared_ptr<KalmanFilterCalculationElements> element = *i;
    v.push_back( element->GetPredictedState() );
  }
  return v;
}

bool ribi::kalman::KalmanFilterExperiment::IsValid() const
{
  //Do not count the inputs: these are created at startup of the experiment
  //and always have the maximum size (of n_timesteps)
  //const int n_inputs = boost::numeric_cast<int>(GetInputs().size());
  const int n_real_states = boost::numeric_cast<int>(GetRealStates().size());
  const int n_predicted_states = boost::numeric_cast<int>(GetPredictedStates().size());
  const int n_measured_states = boost::numeric_cast<int>(GetMeasuredStates().size());
  //Does the experiment produce an equal amount of real, estimated and measured states?
  return n_real_states == n_predicted_states
      && n_real_states == n_measured_states;
}

std::vector<boost::numeric::ublas::vector<double> > ribi::kalman::KalmanFilterExperiment::ParseInput(
  const std::vector<std::string>& input,
  const int n_timesteps)
{
  const int n_rows = n_timesteps;
  const int n_cols = input.size();
  std::vector<boost::numeric::ublas::vector<double> > m(n_rows,
    boost::numeric::ublas::vector<double>(n_cols));

  for (int row=0; row!=n_rows; ++row)
  {
    for (int col=0; col!=n_cols; ++col)
    {
      assert(col < boost::numeric_cast<int>(input.size()));
      const std::string& s = input[col];
      try
      {
        const std::string zero{std::to_string(0.0)}; //For locale
        const ModelFunctionParser f(s.empty() ? zero : s, "t");
        const double y = f.Evaluate( boost::numeric_cast<double>(row) );
        assert(row < boost::numeric_cast<int>(m.size()));
        assert(col < boost::numeric_cast<int>(m[row].size()));
        m[row](col) = y;
      }
      catch (std::runtime_error& e)
      {
        TRACE(e.what());
        TRACE("Unparsable function (will be parsed against 't'):");
        TRACE(s);
        assert(!"Parsing the function should have succeeded, as the GUI takes this out");
        throw;
      }
    }
  }
  assert(n_timesteps == boost::numeric_cast<int>(m.size()));
  assert(m.empty() || input.size() == m[0].size());
  return m;

}

 

 

 

 

 

./CppKalmanFilter/kalmanfilterexperimentparameter.h

 

#ifndef KALMANFILTEREXPERIMENTPARAMETER_H
#define KALMANFILTEREXPERIMENTPARAMETER_H

#include <map>
#include <string>

#include "kalmanfilterparametertype.h"
#include "kalmanfilterexperimentparametertype.h"
#include "whitenoisesystemparametertype.h"

namespace ribi {
namespace kalman {

///A parameter used in a KalmanFilterExperiment
///Note that a KalmanFilterExperiment uses a KalmanFilter,
///which has its own parameter type called KalmanFilterParameter, which is
///a subset of these
struct KalmanFilterExperimentParameter
{
  KalmanFilterExperimentParameter();

  ///Check if a convert from KalmanFilterExperimentParameter to a KalmanFilterParameter will succeed
  bool CanConvertToKalmanFilterParameter(const KalmanFilterExperimentParameterType parameter) const noexcept;

  ///Check if a convert from KalmanFilterExperimentParameter to a WhiteNoiseSystemParameter will succeed
  bool CanConvertToWhiteNoiseSystemParameter(const KalmanFilterExperimentParameterType parameter) const noexcept;

  ///Convert a KalmanFilterParameter to a KalmanFilterExperimentParameter,
  ///which will always succeed
  KalmanFilterExperimentParameterType ConvertToKalmanFilterExperimentParameter(const KalmanFilterParameterType parameter) const noexcept;

  ///Convert a KalmanFilterParameter to a KalmanFilterExperimentParameter,
  ///which will always succeed
  KalmanFilterExperimentParameterType ConvertToKalmanFilterExperimentParameter(const WhiteNoiseSystemParameterType parameter) const noexcept;

  ///Convert a KalmanFilterExperimentParameter to a KalmanFilterParameter,
  ///assumes CanConvertToKalmanFilterParameter succeeds
  KalmanFilterParameterType ConvertToKalmanFilterParameter(const KalmanFilterExperimentParameterType parameter) const noexcept;

  ///Convert a KalmanFilterExperimentParameter to a WhiteNoiseSystemParameterType,
  ///assumes CanConvertToWhiteNoiseSystemParameter succeeds
  WhiteNoiseSystemParameterType ConvertToWhiteNoiseSystemParameter(const KalmanFilterExperimentParameterType parameter) const noexcept;

  ///Get all KalmanFilterExperimentParameterType values
  std::vector<KalmanFilterExperimentParameterType> GetAll() const noexcept;

  ///Is this parameter a matrix/vector of type double?
  bool IsDouble(const KalmanFilterExperimentParameterType type) const noexcept;

  ///Is this parameter a matrix/vector of type std::string for a function?
  bool IsFunction(const KalmanFilterExperimentParameterType type) const noexcept;

  ///Is this parameter a matrix/vector of type integer?
  bool IsInt(const KalmanFilterExperimentParameterType type) const noexcept;

  ///Is this parameter a matrix/vector of type std::string?
  bool IsString(const KalmanFilterExperimentParameterType type) const noexcept;

  ///Is this parameter a matrix?
  bool IsMatrix(const KalmanFilterExperimentParameterType type) const noexcept;

  ///Is this parameter a vector?
  bool IsVector(const KalmanFilterExperimentParameterType type) const noexcept;

  ///Obtain the full name of a type, e.g. 'Matrix to capture the physics of the system'
  std::string ToDescription(const KalmanFilterExperimentParameterType type) const noexcept;

  ///Obtain the full name of a type, e.g. 'State transition'
  std::string ToName(const KalmanFilterExperimentParameterType type) const noexcept;

  ///Obtain the symbol of a type, e.g. 'A'
  std::string ToSymbol(const KalmanFilterExperimentParameterType type) const noexcept;

  private:

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif

  static const std::vector<std::pair<KalmanFilterParameterType,KalmanFilterExperimentParameterType> > m_map_kalman_filter;
  static const std::vector<std::pair<WhiteNoiseSystemParameterType,KalmanFilterExperimentParameterType> > m_map_white_noise_system;
  static const std::vector<std::pair<KalmanFilterParameterType,KalmanFilterExperimentParameterType> > CreateMapKalmanFilter();
  static const std::vector<std::pair<WhiteNoiseSystemParameterType,KalmanFilterExperimentParameterType> > CreateMapWhiteNoiseSystem();

};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTEREXPERIMENTPARAMETER_H

 

 

 

 

 

./CppKalmanFilter/kalmanfilterexperimentparameter.cpp

 

#include "kalmanfilterexperimentparameter.h"

#include <cassert>
#include <stdexcept>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include <boost/numeric/conversion/cast.hpp>
#pragma GCC diagnostic pop

#include "kalmanfilterparameter.h"
#include "whitenoisesystemparameter.h"
#include "testtimer.h"
#include "trace.h"

const std::vector<std::pair<ribi::kalman::KalmanFilterParameterType,ribi::kalman::KalmanFilterExperimentParameterType> >
  ribi::kalman::KalmanFilterExperimentParameter::m_map_kalman_filter = CreateMapKalmanFilter();

const std::vector<std::pair<ribi::kalman::WhiteNoiseSystemParameterType,ribi::kalman::KalmanFilterExperimentParameterType> >
  ribi::kalman::KalmanFilterExperimentParameter::m_map_white_noise_system = CreateMapWhiteNoiseSystem();

ribi::kalman::KalmanFilterExperimentParameter::KalmanFilterExperimentParameter()
{
  #ifndef NDEBUG
  Test();
  #endif
}

bool ribi::kalman::KalmanFilterExperimentParameter::CanConvertToKalmanFilterParameter(
  const KalmanFilterExperimentParameterType parameter) const noexcept
{
  const auto j = m_map_kalman_filter.end();
  for (auto i = m_map_kalman_filter.begin(); i!=j ;++i)
  {
    if ((*i).second == parameter) return true;
  }
  return false;
}


bool ribi::kalman::KalmanFilterExperimentParameter::CanConvertToWhiteNoiseSystemParameter(
  const KalmanFilterExperimentParameterType parameter) const noexcept
{
  const auto j = m_map_white_noise_system.end();
  for (auto i = m_map_white_noise_system.begin(); i!=j ;++i)
  {
    if ((*i).second == parameter) return true;
  }
  return false;
}

ribi::kalman::KalmanFilterExperimentParameterType ribi::kalman::KalmanFilterExperimentParameter::ConvertToKalmanFilterExperimentParameter(
  const KalmanFilterParameterType parameter) const noexcept
{
  const auto j = m_map_kalman_filter.end();
  for (auto i = m_map_kalman_filter.begin(); i!=j ;++i)
  {
    if ((*i).first == parameter) return (*i).second;
  }
  assert(!"Should never get here");
  throw std::logic_error(__func__);
  //throw std::logic_error(
  //  "ribi::kalman::KalmanFilterExperimentParameter::ConvertToKalmanFilterExperimentParameter(const KalmanFilterParameterType parameter)");
}

ribi::kalman::KalmanFilterExperimentParameterType ribi::kalman::KalmanFilterExperimentParameter::ConvertToKalmanFilterExperimentParameter(
  const WhiteNoiseSystemParameterType parameter) const noexcept
{
  const auto j = m_map_white_noise_system.end();
  for (auto i = m_map_white_noise_system.begin(); i!=j ;++i)
  {
    if ((*i).first == parameter) return (*i).second;
  }
  assert(!"Should never get here");
  throw std::logic_error(
    "ribi::kalman::KalmanFilterExperimentParameter::ConvertToKalmanFilterExperimentParameter(const WhiteNoiseSystemParameterType parameter)");
}

ribi::kalman::KalmanFilterParameterType ribi::kalman::KalmanFilterExperimentParameter::ConvertToKalmanFilterParameter(
  const KalmanFilterExperimentParameterType parameter) const noexcept
{
  assert(CanConvertToKalmanFilterParameter(parameter));
  const auto j = m_map_kalman_filter.end();
  for (auto i = m_map_kalman_filter.begin(); i!=j ;++i)
  {
    if ((*i).second == parameter) return (*i).first;
  }
  assert(!"Should never get here");
  throw std::logic_error(
    "ribi::kalman::KalmanFilterExperimentParameter::ConvertToKalmanFilterParameter");
}

ribi::kalman::WhiteNoiseSystemParameterType ribi::kalman::KalmanFilterExperimentParameter::ConvertToWhiteNoiseSystemParameter(
  const KalmanFilterExperimentParameterType parameter) const noexcept
{
  assert(CanConvertToWhiteNoiseSystemParameter(parameter));
  const auto j = m_map_white_noise_system.end();
  for (auto i = m_map_white_noise_system.begin(); i!=j ;++i)
  {
    if ((*i).second == parameter) return (*i).first;
  }
  assert(!"Should never get here");
  throw std::logic_error(
    "ribi::kalman::KalmanFilterExperimentParameter::ConvertToWhiteNoiseSystemParameter");
}

const std::vector<std::pair<ribi::kalman::KalmanFilterParameterType,ribi::kalman::KalmanFilterExperimentParameterType> >
  ribi::kalman::KalmanFilterExperimentParameter::CreateMapKalmanFilter()
{
  std::vector<std::pair<KalmanFilterParameterType,KalmanFilterExperimentParameterType> > v;
  v.push_back(std::make_pair(
    KalmanFilterParameterType::control,
    KalmanFilterExperimentParameterType::control
  ));
  v.push_back(std::make_pair(
    KalmanFilterParameterType::estimated_measurement_noise,
    KalmanFilterExperimentParameterType::estimated_measurement_noise
  ));
  v.push_back(std::make_pair(
    KalmanFilterParameterType::estimated_optimal_kalman_gain,
    KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain
  ));
  v.push_back(std::make_pair(
    KalmanFilterParameterType::estimated_process_noise_covariance,
    KalmanFilterExperimentParameterType::estimated_process_noise_covariance
  ));
  v.push_back(std::make_pair(
    KalmanFilterParameterType::initial_covariance_estimate,
    KalmanFilterExperimentParameterType::initial_covariance_estimate
  ));
  v.push_back(std::make_pair(
    KalmanFilterParameterType::initial_state_estimate,
    KalmanFilterExperimentParameterType::initial_state_estimate
  ));
  v.push_back(std::make_pair(
    KalmanFilterParameterType::observation,
    KalmanFilterExperimentParameterType::observation
  ));
  v.push_back(std::make_pair(
    KalmanFilterParameterType::state_transition,
    KalmanFilterExperimentParameterType::state_transition
  ));
  assert(boost::numeric_cast<int>(v.size()) == static_cast<int>(KalmanFilterParameterType::n_parameters));
  return v;
}

const std::vector<std::pair<ribi::kalman::WhiteNoiseSystemParameterType,ribi::kalman::KalmanFilterExperimentParameterType> >
  ribi::kalman::KalmanFilterExperimentParameter::CreateMapWhiteNoiseSystem()
{
  std::vector<std::pair<WhiteNoiseSystemParameterType,KalmanFilterExperimentParameterType> > v;
  v.push_back(std::make_pair(
    WhiteNoiseSystemParameterType::control,
    KalmanFilterExperimentParameterType::control
  ));
  v.push_back(std::make_pair(
    WhiteNoiseSystemParameterType::initial_state_real,
    KalmanFilterExperimentParameterType::initial_state_real
  ));
  v.push_back(std::make_pair(
    WhiteNoiseSystemParameterType::measurement_frequency,
    KalmanFilterExperimentParameterType::measurement_frequency
  ));
  v.push_back(std::make_pair(
    WhiteNoiseSystemParameterType::real_measurement_noise,
    KalmanFilterExperimentParameterType::real_measurement_noise
  ));
  v.push_back(std::make_pair(
    WhiteNoiseSystemParameterType::real_process_noise,
    KalmanFilterExperimentParameterType::real_process_noise
  ));
  v.push_back(std::make_pair(
    WhiteNoiseSystemParameterType::state_transition,
    KalmanFilterExperimentParameterType::state_transition
  ));
  assert(boost::numeric_cast<int>(v.size()) == static_cast<int>(WhiteNoiseSystemParameterType::n_parameters));
  return v;
}

std::vector<ribi::kalman::KalmanFilterExperimentParameterType>
  ribi::kalman::KalmanFilterExperimentParameter::GetAll() const noexcept
{
  #ifndef NDEBUG
  Test();
  #endif
  const std::vector<KalmanFilterExperimentParameterType> v
    =
    {
      KalmanFilterExperimentParameterType::control,                            //E K W
      KalmanFilterExperimentParameterType::estimated_measurement_noise,        //E K
      KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain,      //E K
      KalmanFilterExperimentParameterType::estimated_process_noise_covariance, //E K
      KalmanFilterExperimentParameterType::initial_covariance_estimate,        //E K
      KalmanFilterExperimentParameterType::initial_state_estimate,             //E K
      KalmanFilterExperimentParameterType::measurement_frequency,              //E   W
      KalmanFilterExperimentParameterType::initial_state_real,                 //E   W
      KalmanFilterExperimentParameterType::real_measurement_noise,             //E   W
      KalmanFilterExperimentParameterType::real_process_noise,                 //E   W
      KalmanFilterExperimentParameterType::input,                              //E
      KalmanFilterExperimentParameterType::observation,                        //E K
      KalmanFilterExperimentParameterType::state_names,                        //E
      KalmanFilterExperimentParameterType::state_transition                    //E K W
    };

  assert(boost::numeric_cast<int>(v.size()) == static_cast<int>(KalmanFilterExperimentParameterType::n_parameters)
    && "All parameters must be in");
  return v;
}
bool ribi::kalman::KalmanFilterExperimentParameter::IsDouble(
  const KalmanFilterExperimentParameterType type
) const noexcept
{
  #ifndef NDEBUG
  Test();
  #endif
  switch (type)
  {
    case KalmanFilterExperimentParameterType::control:                            return true;
    case KalmanFilterExperimentParameterType::estimated_measurement_noise:        return true;
    case KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain:      return true;
    case KalmanFilterExperimentParameterType::estimated_process_noise_covariance: return true;
    case KalmanFilterExperimentParameterType::initial_covariance_estimate:        return true;
    case KalmanFilterExperimentParameterType::initial_state_estimate:             return true;
    case KalmanFilterExperimentParameterType::initial_state_real:                 return true;
    case KalmanFilterExperimentParameterType::measurement_frequency:              return false;
    case KalmanFilterExperimentParameterType::real_measurement_noise:             return true;
    case KalmanFilterExperimentParameterType::real_process_noise:                 return true;
    case KalmanFilterExperimentParameterType::input:                              return false;
    case KalmanFilterExperimentParameterType::observation:                        return true;
    case KalmanFilterExperimentParameterType::state_names:                        return false;
    case KalmanFilterExperimentParameterType::state_transition:                   return true;
    case KalmanFilterExperimentParameterType::n_parameters:
      assert(!"n_parameters is not implemented to be used");
      throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsDouble: use of n_parameters");
  }
  assert(!"Unimplemented type");
  throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsDouble: use of unimplemented type");
}

bool ribi::kalman::KalmanFilterExperimentParameter::IsFunction(
  const KalmanFilterExperimentParameterType type
) const noexcept
{
  switch (type)
  {
    case KalmanFilterExperimentParameterType::control:                            return false;
    case KalmanFilterExperimentParameterType::estimated_measurement_noise:        return false;
    case KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain:      return false;
    case KalmanFilterExperimentParameterType::estimated_process_noise_covariance: return false;
    case KalmanFilterExperimentParameterType::initial_covariance_estimate:        return false;
    case KalmanFilterExperimentParameterType::initial_state_estimate:             return false;
    case KalmanFilterExperimentParameterType::initial_state_real:                 return false;
    case KalmanFilterExperimentParameterType::measurement_frequency:              return false;
    case KalmanFilterExperimentParameterType::real_measurement_noise:             return false;
    case KalmanFilterExperimentParameterType::real_process_noise:                 return false;
    case KalmanFilterExperimentParameterType::input:                              return true;
    case KalmanFilterExperimentParameterType::observation:                        return false;
    case KalmanFilterExperimentParameterType::state_names:                        return false;
    case KalmanFilterExperimentParameterType::state_transition:                   return false;
    case KalmanFilterExperimentParameterType::n_parameters:
      assert(!"n_parameters is not implemented to be used");
      throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsFunction: use of n_parameters");
  }
  assert(!"Unimplemented type");
  throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsFunction: use of unimplemented type");
}

bool ribi::kalman::KalmanFilterExperimentParameter::IsInt(
  const KalmanFilterExperimentParameterType type
) const noexcept
{
  switch (type)
  {
    case KalmanFilterExperimentParameterType::control:                            return false;
    case KalmanFilterExperimentParameterType::estimated_measurement_noise:        return false;
    case KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain:      return false;
    case KalmanFilterExperimentParameterType::estimated_process_noise_covariance: return false;
    case KalmanFilterExperimentParameterType::initial_covariance_estimate:        return false;
    case KalmanFilterExperimentParameterType::initial_state_estimate:             return false;
    case KalmanFilterExperimentParameterType::initial_state_real:                 return false;
    case KalmanFilterExperimentParameterType::measurement_frequency:              return true;
    case KalmanFilterExperimentParameterType::real_measurement_noise:             return false;
    case KalmanFilterExperimentParameterType::real_process_noise:                 return false;
    case KalmanFilterExperimentParameterType::input:                              return false;
    case KalmanFilterExperimentParameterType::observation:                        return false;
    case KalmanFilterExperimentParameterType::state_names:                        return false;
    case KalmanFilterExperimentParameterType::state_transition:                   return false;
    case KalmanFilterExperimentParameterType::n_parameters:
      assert(!"n_parameters is not implemented to be used");
      throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsInt: use of n_parameters");
  }
  assert(!"Unimplemented type");
  throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsInt: use of unimplemented type");
}

bool ribi::kalman::KalmanFilterExperimentParameter::IsString(
  const KalmanFilterExperimentParameterType type
) const noexcept
{
  switch (type)
  {
    case KalmanFilterExperimentParameterType::control:                            return false;
    case KalmanFilterExperimentParameterType::estimated_measurement_noise:        return false;
    case KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain:      return false;
    case KalmanFilterExperimentParameterType::estimated_process_noise_covariance: return false;
    case KalmanFilterExperimentParameterType::initial_covariance_estimate:        return false;
    case KalmanFilterExperimentParameterType::initial_state_estimate:             return false;
    case KalmanFilterExperimentParameterType::initial_state_real:                 return false;
    case KalmanFilterExperimentParameterType::measurement_frequency:              return false;
    case KalmanFilterExperimentParameterType::real_measurement_noise:             return false;
    case KalmanFilterExperimentParameterType::real_process_noise:                 return false;
    case KalmanFilterExperimentParameterType::input:                              return true;
    case KalmanFilterExperimentParameterType::observation:                        return false;
    case KalmanFilterExperimentParameterType::state_names:                        return true;
    case KalmanFilterExperimentParameterType::state_transition:                   return false;
    case KalmanFilterExperimentParameterType::n_parameters:
      assert(!"n_parameters is not implemented to be used");
      throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsString: use of n_parameters");
  }
  assert(!"Unimplemented type");
  throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsString: use of unimplemented type");
}

bool ribi::kalman::KalmanFilterExperimentParameter::IsMatrix(
  const KalmanFilterExperimentParameterType type
) const noexcept
{
  return !ribi::kalman::KalmanFilterExperimentParameter::IsVector(type);
}

bool ribi::kalman::KalmanFilterExperimentParameter::IsVector(
  const KalmanFilterExperimentParameterType type
) const noexcept
{
  switch (type)
  {
    case KalmanFilterExperimentParameterType::control:                            return false;
    case KalmanFilterExperimentParameterType::estimated_measurement_noise:        return false;
    case KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain:      return false;
    case KalmanFilterExperimentParameterType::estimated_process_noise_covariance: return false;
    case KalmanFilterExperimentParameterType::initial_covariance_estimate:        return false;
    case KalmanFilterExperimentParameterType::initial_state_estimate:             return true;
    case KalmanFilterExperimentParameterType::initial_state_real:                 return true;
    case KalmanFilterExperimentParameterType::measurement_frequency:              return true;
    case KalmanFilterExperimentParameterType::real_measurement_noise:             return true;
    case KalmanFilterExperimentParameterType::real_process_noise:                 return true;
    case KalmanFilterExperimentParameterType::input:                              return true;
    case KalmanFilterExperimentParameterType::observation:                        return false;
    case KalmanFilterExperimentParameterType::state_names:                        return true;
    case KalmanFilterExperimentParameterType::state_transition:                   return false;
    case KalmanFilterExperimentParameterType::n_parameters:
      assert(!"n_parameters is not implemented to be used");
      throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsVector: use of n_parameters");
  }
  assert(!"Unimplemented type");
  throw std::logic_error("ribi::kalman::KalmanFilterExperimentParameter::IsVector: use of unimplemented type");
}

#ifndef NDEBUG
void ribi::kalman::KalmanFilterExperimentParameter::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
  KalmanFilterExperimentParameter p;
  assert( p.IsMatrix(KalmanFilterExperimentParameterType::control));
  assert( p.IsMatrix(KalmanFilterExperimentParameterType::estimated_measurement_noise));
  assert( p.IsMatrix(KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain));
  assert( p.IsMatrix(KalmanFilterExperimentParameterType::estimated_process_noise_covariance));
  assert( p.IsMatrix(KalmanFilterExperimentParameterType::initial_covariance_estimate));
  assert(!p.IsMatrix(KalmanFilterExperimentParameterType::initial_state_estimate));
  assert(!p.IsMatrix(KalmanFilterExperimentParameterType::initial_state_real));
  assert(!p.IsMatrix(KalmanFilterExperimentParameterType::measurement_frequency));
  assert(!p.IsMatrix(KalmanFilterExperimentParameterType::real_measurement_noise));
  assert(!p.IsMatrix(KalmanFilterExperimentParameterType::real_process_noise));
  assert(!p.IsMatrix(KalmanFilterExperimentParameterType::input));
  assert( p.IsMatrix(KalmanFilterExperimentParameterType::observation));
  assert(!p.IsMatrix(KalmanFilterExperimentParameterType::state_names));
  assert( p.IsMatrix(KalmanFilterExperimentParameterType::state_transition));

  assert(!p.IsVector(KalmanFilterExperimentParameterType::control));
  assert(!p.IsVector(KalmanFilterExperimentParameterType::estimated_measurement_noise));
  assert(!p.IsVector(KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain));
  assert(!p.IsVector(KalmanFilterExperimentParameterType::estimated_process_noise_covariance));
  assert(!p.IsVector(KalmanFilterExperimentParameterType::initial_covariance_estimate));
  assert( p.IsVector(KalmanFilterExperimentParameterType::initial_state_estimate));
  assert( p.IsVector(KalmanFilterExperimentParameterType::initial_state_real));
  assert( p.IsVector(KalmanFilterExperimentParameterType::measurement_frequency));
  assert( p.IsVector(KalmanFilterExperimentParameterType::real_measurement_noise));
  assert( p.IsVector(KalmanFilterExperimentParameterType::real_process_noise));
  assert( p.IsVector(KalmanFilterExperimentParameterType::input));
  assert(!p.IsVector(KalmanFilterExperimentParameterType::observation));
  assert( p.IsVector(KalmanFilterExperimentParameterType::state_names));
  assert(!p.IsVector(KalmanFilterExperimentParameterType::state_transition));

  assert(!p.IsString(KalmanFilterExperimentParameterType::control));
  assert(!p.IsString(KalmanFilterExperimentParameterType::estimated_measurement_noise));
  assert(!p.IsString(KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain));
  assert(!p.IsString(KalmanFilterExperimentParameterType::estimated_process_noise_covariance));
  assert(!p.IsString(KalmanFilterExperimentParameterType::initial_covariance_estimate));
  assert(!p.IsString(KalmanFilterExperimentParameterType::initial_state_estimate));
  assert(!p.IsString(KalmanFilterExperimentParameterType::initial_state_real));
  assert(!p.IsString(KalmanFilterExperimentParameterType::measurement_frequency));
  assert(!p.IsString(KalmanFilterExperimentParameterType::real_measurement_noise));
  assert(!p.IsString(KalmanFilterExperimentParameterType::real_process_noise));
  assert( p.IsString(KalmanFilterExperimentParameterType::input));
  assert(!p.IsString(KalmanFilterExperimentParameterType::observation));
  assert( p.IsString(KalmanFilterExperimentParameterType::state_names));
  assert(!p.IsString(KalmanFilterExperimentParameterType::state_transition));

  assert(!p.IsFunction(KalmanFilterExperimentParameterType::control));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::estimated_measurement_noise));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::estimated_process_noise_covariance));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::initial_covariance_estimate));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::initial_state_estimate));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::initial_state_real));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::measurement_frequency));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::real_measurement_noise));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::real_process_noise));
  assert( p.IsFunction(KalmanFilterExperimentParameterType::input));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::observation));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::state_names));
  assert(!p.IsFunction(KalmanFilterExperimentParameterType::state_transition));

  assert( p.IsDouble(KalmanFilterExperimentParameterType::control));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::estimated_measurement_noise));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::estimated_process_noise_covariance));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::initial_covariance_estimate));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::initial_state_estimate));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::initial_state_real));
  assert(!p.IsDouble(KalmanFilterExperimentParameterType::measurement_frequency));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::real_measurement_noise));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::real_process_noise));
  assert(!p.IsDouble(KalmanFilterExperimentParameterType::input));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::observation));
  assert(!p.IsDouble(KalmanFilterExperimentParameterType::state_names));
  assert( p.IsDouble(KalmanFilterExperimentParameterType::state_transition));

  assert(!p.IsInt(KalmanFilterExperimentParameterType::control));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::estimated_measurement_noise));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::estimated_optimal_kalman_gain));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::estimated_process_noise_covariance));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::initial_covariance_estimate));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::initial_state_estimate));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::initial_state_real));
  assert( p.IsInt(KalmanFilterExperimentParameterType::measurement_frequency));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::real_measurement_noise));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::real_process_noise));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::input));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::observation));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::state_names));
  assert(!p.IsInt(KalmanFilterExperimentParameterType::state_transition));

}
#endif

std::string ribi::kalman::KalmanFilterExperimentParameter::ToDescription(
  const KalmanFilterExperimentParameterType type
) const noexcept
{
  #ifndef NDEBUG
  Test();
  #endif
  //Check for the subset
  if (CanConvertToKalmanFilterParameter(type))
  {
    const KalmanFilterParameterType sub_type = ConvertToKalmanFilterParameter(type);
    return KalmanFilterParameter().ToDescription(sub_type);
  }
  if (CanConvertToWhiteNoiseSystemParameter(type))
  {
    const WhiteNoiseSystemParameterType sub_type = ConvertToWhiteNoiseSystemParameter(type);
    return WhiteNoiseSystemParameter().ToDescription(sub_type);
  }
  //Check the unique types
  switch (type)
  {
    case KalmanFilterExperimentParameterType::input:
      return "Vector of inputs";
    case KalmanFilterExperimentParameterType::state_names:
      return "Vector of the state element names";
    default: assert(!"Unimplemented type of KalmanFilterExperimentParameterType");
      throw std::logic_error(__func__);
  }
}

std::string ribi::kalman::KalmanFilterExperimentParameter::ToName(const KalmanFilterExperimentParameterType type) const noexcept
{
  #ifndef NDEBUG
  Test();
  #endif
  //Check for the subset
  if (CanConvertToKalmanFilterParameter(type))
  {
    const KalmanFilterParameterType sub_type = ConvertToKalmanFilterParameter(type);
    return KalmanFilterParameter().ToName(sub_type);
  }
  if (CanConvertToWhiteNoiseSystemParameter(type))
  {
    const WhiteNoiseSystemParameterType sub_type = ConvertToWhiteNoiseSystemParameter(type);
    return WhiteNoiseSystemParameter().ToName(sub_type);
  }
  //Check the unique types
  switch (type)
  {
    case KalmanFilterExperimentParameterType::input:
      return "Input";
    case KalmanFilterExperimentParameterType::state_names:
      return "State names";
    default: assert(!"Unimplemented type of KalmanFilterExperimentParameterType");
      throw std::logic_error(__func__);
  }
}

  std::string ribi::kalman::KalmanFilterExperimentParameter::ToSymbol(const KalmanFilterExperimentParameterType type) const noexcept
{
  #ifndef NDEBUG
  Test();
  #endif
  //Check for the subset
  if (CanConvertToKalmanFilterParameter(type))
  {
    const KalmanFilterParameterType sub_type = ConvertToKalmanFilterParameter(type);
    return KalmanFilterParameter().ToSymbol(sub_type);
  }
  if (CanConvertToWhiteNoiseSystemParameter(type))
  {
    const WhiteNoiseSystemParameterType sub_type = ConvertToWhiteNoiseSystemParameter(type);
    return WhiteNoiseSystemParameter().ToSymbol(sub_type);
  }
  //Check the unique types
  switch (type)
  {
    case KalmanFilterExperimentParameterType::input:
      return "y";
    case KalmanFilterExperimentParameterType::state_names:
      return ""; //State names has no symbol
    default: assert(!"Unimplemented type of KalmanFilterExperimentParameterType");
      throw std::logic_error(__func__);
  }
}

 

 

 

 

 

./CppKalmanFilter/kalmanfilterexperimentparametertype.h

 

#ifndef KALMANFILTEREXPERIMENTPARAMETERTYPE_H
#define KALMANFILTEREXPERIMENTPARAMETERTYPE_H

namespace ribi {
namespace kalman {

//#include <vector>

///These parameters overlap
///E: Experiment
///K: Kalman filter
///W: White noise system parameter
enum class KalmanFilterExperimentParameterType
{
  control,                            //E K W
  estimated_measurement_noise,        //E K
  estimated_optimal_kalman_gain,      //E K
  estimated_process_noise_covariance, //E K
  initial_covariance_estimate,        //E K
  initial_state_estimate,             //E K
  initial_state_real,                 //E   W
  measurement_frequency,              //E   W
  real_measurement_noise,             //E   W
  real_process_noise,                 //E   W
  input,                              //E
  observation,                        //E K
  state_names,                        //E
  state_transition,                   //E K W
  n_parameters                        //E
};

bool operator<(const KalmanFilterExperimentParameterType lhs, const KalmanFilterExperimentParameterType rhs);
bool operator==(const KalmanFilterExperimentParameterType lhs, const KalmanFilterExperimentParameterType rhs);
bool operator!=(const KalmanFilterExperimentParameterType lhs, const KalmanFilterExperimentParameterType rhs);

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTEREXPERIMENTPARAMETERTYPE_H

 

 

 

 

 

./CppKalmanFilter/kalmanfilterexperimentparametertype.cpp

 



#include "kalmanfilterexperimentparametertype.h"

//#include <cassert>
//#include <boost/numeric/conversion/cast.hpp>

bool ribi::kalman::operator<(const KalmanFilterExperimentParameterType lhs, const KalmanFilterExperimentParameterType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) < static_cast<int>(rhs);
}

bool ribi::kalman::operator==(const KalmanFilterExperimentParameterType lhs, const KalmanFilterExperimentParameterType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) == static_cast<int>(rhs);
}

bool ribi::kalman::operator!=(const KalmanFilterExperimentParameterType lhs, const KalmanFilterExperimentParameterType rhs)
{
  return !(lhs == rhs);
}

 

 

 

 

 

./CppKalmanFilter/kalmanfilterfactory.h

 

#ifndef KALMANFILTERFACTORY_H
#define KALMANFILTERFACTORY_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "kalmanfilter.h"

namespace ribi {
namespace kalman {

///Factory for KalmanFilter
struct KalmanFilterFactory
{
  KalmanFilterFactory();

  boost::shared_ptr<KalmanFilter> Create(
    const boost::shared_ptr<const KalmanFilterParameters>& parameters
  ) const noexcept;

  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTERFACTORY_H

 

 

 

 

 

./CppKalmanFilter/kalmanfilterfactory.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "kalmanfilterfactory.h"
#pragma GCC diagnostic pop

#include <cassert>

#include "standardkalmanfilterfactory.h"
#include "steadystatekalmanfilterfactory.h"
#include "fixedlagsmootherkalmanfilterfactory.h"
#include "testtimer.h"

ribi::kalman::KalmanFilterFactory::KalmanFilterFactory()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::shared_ptr<ribi::kalman::KalmanFilter> ribi::kalman::KalmanFilterFactory::Create(
  const boost::shared_ptr<const KalmanFilterParameters>& parameters
) const noexcept
{
  assert(parameters);

  boost::shared_ptr<KalmanFilter> kalman_filter;

  switch(parameters->GetType())
  {
    case KalmanFilterType::standard:
      kalman_filter = StandardKalmanFilterFactory().Create(parameters);
    break;
    case KalmanFilterType::steady_state:
      kalman_filter = SteadyStateKalmanFilterFactory().Create(parameters);
    break;
    case KalmanFilterType::fixed_lag_smoother:
      kalman_filter = FixedLagSmootherKalmanFilterFactory().Create(parameters);
    break;
    case KalmanFilterType::n_types:
      assert(!"Unimplemented Kalman filter type");
      throw std::logic_error(__func__);
  }

  assert(kalman_filter->GetType() == parameters->GetType());
  return kalman_filter;
}

#ifndef NDEBUG
void ribi::kalman::KalmanFilterFactory::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/kalmanfilterparameter.h

 

#ifndef KALMANFILTERPARAMETER_H
#define KALMANFILTERPARAMETER_H

#include <string>
#include "kalmanfilterparametertype.h"

namespace ribi {
namespace kalman {

///A single KalmanFilterParameter
///for example, the state transition matrix
/// - is a matrix
/// - is not a vector
/// - has a description like 'Matrix that [...]'
/// - has the name 'State transition matrix'
/// - has the symbol 'A'
struct KalmanFilterParameter
{
  KalmanFilterParameter();

  ///Obtain all KalmanFilterParameterType values, except n_parameters
  std::vector<KalmanFilterParameterType> GetAll() const noexcept;

  ///Is the type a matrix?
  bool IsMatrix(const KalmanFilterParameterType type) const noexcept;

  ///Is the type a vector?
  bool IsVector(const KalmanFilterParameterType type) const noexcept;

  ///Obtain the full name of a type, e.g. 'Matrix to capture the physics of the system'
  std::string ToDescription(const KalmanFilterParameterType type) const noexcept;

  ///Obtain the full name of a type, e.g. 'State transition'
  std::string ToName(const KalmanFilterParameterType type) const noexcept;

  ///Obtain the symbol of a type, e.g. 'A'
  std::string ToSymbol(const KalmanFilterParameterType type) const noexcept;

  private:

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTERPARAMETER_H

 

 

 

 

 

./CppKalmanFilter/kalmanfilterparameter.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "kalmanfilterparameter.h"

#include <cassert>
#include <stdexcept>
#include <boost/numeric/conversion/cast.hpp>

#include "testtimer.h"
#pragma GCC diagnostic pop

ribi::kalman::KalmanFilterParameter::KalmanFilterParameter()
{
  #ifndef NDEBUG
  Test();
  #endif
}

std::vector<ribi::kalman::KalmanFilterParameterType> ribi::kalman::KalmanFilterParameter::GetAll() const noexcept
{
  const std::vector<KalmanFilterParameterType> v {
    KalmanFilterParameterType::control,
    KalmanFilterParameterType::estimated_measurement_noise,
    KalmanFilterParameterType::estimated_optimal_kalman_gain,
    KalmanFilterParameterType::estimated_process_noise_covariance,
    KalmanFilterParameterType::initial_covariance_estimate,
    KalmanFilterParameterType::initial_state_estimate,
    KalmanFilterParameterType::observation,
    KalmanFilterParameterType::state_transition
  };

  assert(boost::numeric_cast<int>(v.size()) == static_cast<int>(KalmanFilterParameterType::n_parameters)
    && "All parameters must be in");
  return v;
}

bool ribi::kalman::KalmanFilterParameter::IsMatrix(const KalmanFilterParameterType type) const noexcept
{
  return !ribi::kalman::KalmanFilterParameter::IsVector(type);
}

bool ribi::kalman::KalmanFilterParameter::IsVector(const KalmanFilterParameterType type) const noexcept
{
  return type == KalmanFilterParameterType::initial_state_estimate;
}

std::string ribi::kalman::KalmanFilterParameter::ToDescription(const KalmanFilterParameterType type) const noexcept
{
  switch (type)
  {
    case KalmanFilterParameterType::control:
      return "Matrix for converting input to state change";
    case KalmanFilterParameterType::estimated_measurement_noise:
      return "Matrix that has an estimated measurement noise covariance";
    case KalmanFilterParameterType::estimated_optimal_kalman_gain:
      return "Matrix with the estimated optimal Kalman gain";
    case KalmanFilterParameterType::estimated_process_noise_covariance:
      return "Matrix with the estimated process noise covariance";
    case KalmanFilterParameterType::initial_covariance_estimate:
      return "Matrix with the initial covariance estimate";
    case KalmanFilterParameterType::initial_state_estimate:
      return "Vector with the initial state estimate";
    case KalmanFilterParameterType::observation:
      return "Matrix that with effect of a measurement on a state change";
    case KalmanFilterParameterType::state_transition:
      return "Matrix that contains the internal physics of the system; the effect of current state on the next state";
    case KalmanFilterParameterType::n_parameters:
      assert(!"Unimplemented type of KalmanFilterParameterType");
      throw std::logic_error(__func__);
  }
  assert(!"Unimplemented type of KalmanFilterParameterType");
  throw std::logic_error(__func__);
}

std::string ribi::kalman::KalmanFilterParameter::ToName(const KalmanFilterParameterType type) const noexcept
{
  switch (type)
  {
    case KalmanFilterParameterType::control:
      return "Control";
    case KalmanFilterParameterType::estimated_measurement_noise:
      return "Estimated measurement error covariance";
    case KalmanFilterParameterType::estimated_optimal_kalman_gain:
      return "Estimated optimal Kalman gain";
    case KalmanFilterParameterType::estimated_process_noise_covariance:
      return "Estimated process noise covariance";
    case KalmanFilterParameterType::initial_covariance_estimate:
      return "Initial covariance estimate";
    case KalmanFilterParameterType::initial_state_estimate:
      return "Initial state estimate";
    case KalmanFilterParameterType::observation:
      return "Observation";
    case KalmanFilterParameterType::state_transition:
      return "State transition";
    case KalmanFilterParameterType::n_parameters:
      assert(!"Unimplemented type of KalmanFilterParameterType");
      throw std::logic_error(__func__);
  }
  assert(!"Unimplemented type of KalmanFilterParameterType");
  throw std::logic_error(__func__);
}

std::string ribi::kalman::KalmanFilterParameter::ToSymbol(const KalmanFilterParameterType type) const noexcept
{
  switch (type)
  {
    case KalmanFilterParameterType::control:
      return "B";
    case KalmanFilterParameterType::estimated_measurement_noise:
      return "R";
    case KalmanFilterParameterType::estimated_optimal_kalman_gain:
      return "K";
    case KalmanFilterParameterType::estimated_process_noise_covariance:
      return "Q";
    case KalmanFilterParameterType::initial_covariance_estimate:
      return "P";
    case KalmanFilterParameterType::initial_state_estimate:
      return "x";
    case KalmanFilterParameterType::observation:
      return "H";
    case KalmanFilterParameterType::state_transition:
      return "A";
    case KalmanFilterParameterType::n_parameters:
      assert(!"Unimplemented type of KalmanFilterParameterType");
      throw std::logic_error(__func__);
  }
  assert(!"Unimplemented type of KalmanFilterParameterType");
  throw std::logic_error(__func__);
}

#ifndef NDEBUG
void ribi::kalman::KalmanFilterParameter::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/kalmanfilterparameters.h

 

#ifndef KALMANFILTERPARAMETERS_H
#define KALMANFILTERPARAMETERS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#pragma GCC diagnostic pop

#include "kalmanfiltertype.h"
#include "kalmanfilterparametertype.h"

namespace ribi {
namespace kalman {

///ABC for the parameters every Kalman filter needs at least
struct KalmanFilterParameters
{
  KalmanFilterParameters(const KalmanFilterParameters&) = delete;
  KalmanFilterParameters& operator=(const KalmanFilterParameters&) = delete;
  virtual ~KalmanFilterParameters() noexcept {}

  ///Obtain the Kalman filter type as an enum
  virtual KalmanFilterType GetType() const = 0;

  ///Obtain the control matrix ('B'): the effect of inputs on the current states
  const boost::numeric::ublas::matrix<double>& GetControl() const
    { return m_control; }

  ///x: The initial state estimate
  const boost::numeric::ublas::vector<double>& GetInitialStateEstimate() const
    { return m_initial_state_estimate; }

  ///Obtain how the states are observed ('H')
  const boost::numeric::ublas::matrix<double>& GetObservation() const
    { return m_observation; }

  ///Obtain the state transition matrix ('F'), containing the physics of the system
  const boost::numeric::ublas::matrix<double>& GetStateTransition() const { return m_state_transition; }

  ///Check if this parameter set has a certain type of KalmanFilterParameter
  static bool HasParameterType(const KalmanFilterParameterType type);

  protected:
  ///An ABC can only be constructed by derived classes
  explicit KalmanFilterParameters(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state_estimate,
    const boost::numeric::ublas::matrix<double>& observation,
    const boost::numeric::ublas::matrix<double>& state_transition
  );

  private:
  ///B: control matrix: the effect of inputs on the current states
  const boost::numeric::ublas::matrix<double> m_control;

  ///x: The initial state estimate
  const boost::numeric::ublas::vector<double> m_initial_state_estimate;

  ///H: How the states are observed
  const boost::numeric::ublas::matrix<double> m_observation;

  ///F: state transition matrix, containing the physics of the system
  const boost::numeric::ublas::matrix<double> m_state_transition;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTERPARAMETERS_H

 

 

 

 

 

./CppKalmanFilter/kalmanfilterparameters.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "kalmanfilterparameters.h"
#pragma GCC diagnostic pop

#include <cassert>
#include "trace.h"

ribi::kalman::KalmanFilterParameters::KalmanFilterParameters(
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::vector<double>& initial_state_estimate,
  const boost::numeric::ublas::matrix<double>& observation,
  const boost::numeric::ublas::matrix<double>& state_transition)
  : m_control{control},
    m_initial_state_estimate{initial_state_estimate},
    m_observation{observation},
    m_state_transition{state_transition}
{
  #ifndef NDEBUG
  //Check for correct dimensionality
  const std::size_t sz = GetInitialStateEstimate().size();
  //assert(sz >= 0); //Inevitable for std::size_t
  assert(GetControl().size1() == sz);
  assert(GetControl().size2() == sz);
  assert(GetObservation().size1() == sz);
  assert(GetObservation().size2() == sz);
  assert(GetStateTransition().size1() == sz);
  assert(GetStateTransition().size2() == sz);
  assert(GetInitialStateEstimate().size() == sz);
  #endif
}

bool ribi::kalman::KalmanFilterParameters::HasParameterType(const KalmanFilterParameterType type)
{
  return
       type == KalmanFilterParameterType::control
    || type == KalmanFilterParameterType::initial_state_estimate
    || type == KalmanFilterParameterType::observation
    || type == KalmanFilterParameterType::state_transition;
}

 

 

 

 

 

./CppKalmanFilter/kalmanfilterparametertype.h

 

#ifndef KALMANFILTERPARAMETERTYPE_H
#define KALMANFILTERPARAMETERTYPE_H

#include <vector>

namespace ribi {
namespace kalman {

///The matrix/vector types/names of Kalman filter parameters
enum class KalmanFilterParameterType
{
  control,
  estimated_measurement_noise,
  estimated_optimal_kalman_gain,
  estimated_process_noise_covariance,
  initial_covariance_estimate,
  initial_state_estimate,
  observation,
  state_transition,
  n_parameters //Must be last element, used in debugging
};

bool operator<(const KalmanFilterParameterType lhs, const KalmanFilterParameterType rhs);
bool operator==(const KalmanFilterParameterType lhs, const KalmanFilterParameterType rhs);
bool operator!=(const KalmanFilterParameterType lhs, const KalmanFilterParameterType rhs);

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTERPARAMETERTYPE_H

 

 

 

 

 

./CppKalmanFilter/kalmanfilterparametertype.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "kalmanfilterparametertype.h"

#include <cassert>
#include <boost/numeric/conversion/cast.hpp>
#pragma GCC diagnostic pop

bool ribi::kalman::operator<(const KalmanFilterParameterType lhs, const KalmanFilterParameterType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) < static_cast<int>(rhs);
}

bool ribi::kalman::operator==(const KalmanFilterParameterType lhs, const KalmanFilterParameterType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) == static_cast<int>(rhs);
}

 

 

 

 

 

./CppKalmanFilter/kalmanfiltertype.h

 

#ifndef KALMANFILTERTYPE_H
#define KALMANFILTERTYPE_H

namespace ribi {
namespace kalman {

enum class KalmanFilterType
{
  //ensemble,
  //extended,
  fixed_lag_smoother,
  //hybrid
  //kalman_bucy
  //minimum_variance_smoother
  //modified_Bryson_Frazier_smoother
  //rauch_tung_striebel_smoother
  standard, //Also called: 'discrete'
  steady_state,
  //unscented,
  n_types //Must be last value, used in debugging
};


bool operator==(const KalmanFilterType lhs, const KalmanFilterType rhs);
bool operator!=(const KalmanFilterType lhs, const KalmanFilterType rhs);
bool operator<(const KalmanFilterType lhs, const KalmanFilterType rhs);

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTERTYPE_H

 

 

 

 

 

./CppKalmanFilter/kalmanfiltertype.cpp

 

#include "kalmanfiltertype.h"

#include <cassert>
#include <stdexcept>

bool ribi::kalman::operator==(const KalmanFilterType lhs, const KalmanFilterType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) == static_cast<int>(rhs);
}

bool ribi::kalman::operator!=(const KalmanFilterType lhs, const KalmanFilterType rhs)
{
  return !(lhs == rhs);
}

bool ribi::kalman::operator<(const KalmanFilterType lhs, const KalmanFilterType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) < static_cast<int>(rhs);
}

 

 

 

 

 

./CppKalmanFilter/kalmanfiltertypes.h

 

#ifndef KALMANFILTERTYPES_H
#define KALMANFILTERTYPES_H

#include <string>
#include <vector>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/bimap.hpp>
#pragma GCC diagnostic pop

#include "kalmanfiltertype.h"


namespace ribi {
namespace kalman {

///Class to work on one or more KalmanFilterType instances
struct KalmanFilterTypes
{
  std::vector<KalmanFilterType> GetAllTypes() const noexcept;
  std::string ToStr(const KalmanFilterType type) const noexcept;
  KalmanFilterType ToType(const std::string& s) const;

  private:
  static boost::bimap<KalmanFilterType,std::string> m_map;
  static boost::bimap<KalmanFilterType,std::string> CreateMap() noexcept;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // KALMANFILTERTYPES_H

 

 

 

 

 

./CppKalmanFilter/kalmanfiltertypes.cpp

 

#include "kalmanfiltertypes.h"

boost::bimap<ribi::kalman::KalmanFilterType,std::string> ribi::kalman::KalmanFilterTypes::m_map;

boost::bimap<ribi::kalman::KalmanFilterType,std::string> ribi::kalman::KalmanFilterTypes::CreateMap() noexcept
{
  #ifndef NDEBUG
  Test();
  #endif

  boost::bimap<KalmanFilterType,std::string> m;
  m.insert(boost::bimap<KalmanFilterType,std::string>::value_type(
    KalmanFilterType::fixed_lag_smoother,"fixed lag smoother"));
  m.insert(boost::bimap<KalmanFilterType,std::string>::value_type(
    KalmanFilterType::standard,"discrete"));
  m.insert(boost::bimap<KalmanFilterType,std::string>::value_type(
    KalmanFilterType::steady_state,"steady state"));
  return m;
}

std::vector<ribi::kalman::KalmanFilterType>
  ribi::kalman::KalmanFilterTypes::GetAllTypes() const noexcept
{
  const std::vector<KalmanFilterType> v
  =
  {
    KalmanFilterType::fixed_lag_smoother,
    KalmanFilterType::standard,
    KalmanFilterType::steady_state
  };
  assert(static_cast<int>(v.size()) == static_cast<int>(KalmanFilterType::n_types));
  return v;
}

#ifndef NDEBUG
void ribi::kalman::KalmanFilterTypes::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  const std::vector<KalmanFilterType> v = KalmanFilterTypes().GetAllTypes();
  const std::size_t sz = v.size();
  for (std::size_t i=0; i!=sz; ++i)
  {
    assert(i < v.size());
    const KalmanFilterType t = v[i];
    const std::string s = KalmanFilterTypes().ToStr(t);
    assert(!s.empty());
    const KalmanFilterType u = KalmanFilterTypes().ToType(s);
    assert(u == t);
  }
}
#endif

std::string ribi::kalman::KalmanFilterTypes::ToStr(const KalmanFilterType type) const noexcept
{
  if (m_map.left.empty()) m_map = CreateMap();
  assert(!m_map.left.empty());
  assert(m_map.left.count(type));
  const std::string s = m_map.left.find(type)->second;
  return s;
}

ribi::kalman::KalmanFilterType ribi::kalman::KalmanFilterTypes::ToType(const std::string& s) const
{
  if (m_map.right.empty()) m_map = CreateMap();
  assert(!m_map.right.empty());
  assert(m_map.right.count(s) == 1);
  const KalmanFilterType t = m_map.right.find(s)->second;
  return t;
}

 

 

 

 

 

./CppKalmanFilter/laggedwhitenoisesystem.h

 

#ifndef LAGGEDWHITENOISESYSTEM_H
#define LAGGEDWHITENOISESYSTEM_H

#include <queue>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "whitenoisesystem.h"
#include "whitenoisesystemparameters.h"
#include "laggedwhitenoisesystemparameters.h"
#include "standardwhitenoisesystem.h"

namespace ribi {
namespace kalman {

///A lagged white noise system is a system that can be measured only after a certain number
///of periods.
struct LaggedWhiteNoiseSystem : public WhiteNoiseSystem
{
  ///Obtain the lagged white noise system parameters
  const boost::shared_ptr<const LaggedWhiteNoiseSystemParameters>& GetLaggedWhiteNoiseSystemParameters() const
    { return m_parameters; }

  ///Obtain the type as an enum
  WhiteNoiseSystemType GetType() const noexcept { return WhiteNoiseSystemType::lagged; }

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Update reality, that is, let the real system (i.e. reality) go to its next state
  void GoToNextState(const boost::numeric::ublas::vector<double>& input);

  ///Measure a value from this system with normally distributed noise
  boost::numeric::ublas::vector<double> Measure() const noexcept;

  ///Peek what the real value is
  const boost::numeric::ublas::vector<double>& PeekAtRealState() const noexcept;

  private:
  ///LaggedWhiteNoiseSystem must be created with a LaggedWhiteNoiseSystemFactory
  explicit LaggedWhiteNoiseSystem(const boost::shared_ptr<const WhiteNoiseSystemParameters>& white_noise_system_parameters);
  friend class LaggedWhiteNoiseSystemFactory;

  ///Can only be deleted by boost::checked_delete
  ~LaggedWhiteNoiseSystem() noexcept {}
  friend void boost::checked_delete<>(LaggedWhiteNoiseSystem*);

  ///The front one is the one that can be read,
  ///the back one is the freshest measurement
  mutable std::queue<boost::numeric::ublas::vector<double> > m_measuments;

  ///The lagged white noise system parameters
  const boost::shared_ptr<const LaggedWhiteNoiseSystemParameters> m_parameters;

  ///The standard white noise system used as an engine
  boost::shared_ptr<StandardWhiteNoiseSystem> m_system;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // LAGGEDWHITENOISESYSTEM_H

 

 

 

 

 

./CppKalmanFilter/laggedwhitenoisesystem.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "laggedwhitenoisesystem.h"

#include <cassert>

#include <boost/numeric/conversion/cast.hpp>
#pragma GCC diagnostic pop

#include "matrix.h"
#include "trace.h"
#include "laggedwhitenoisesystem.h"
#include "laggedwhitenoisesystemfactory.h"
#include "laggedwhitenoisesystemparameters.h"
#include "standardwhitenoisesystemparameters.h"
#include "standardwhitenoisesystemfactory.h"
#include "testtimer.h"

ribi::kalman::LaggedWhiteNoiseSystem::LaggedWhiteNoiseSystem(
  const boost::shared_ptr<const WhiteNoiseSystemParameters>& parameters)
  : WhiteNoiseSystem{parameters},
    m_measuments{},
    m_parameters{boost::dynamic_pointer_cast<const LaggedWhiteNoiseSystemParameters>(parameters)},
    m_system{
      StandardWhiteNoiseSystemFactory().Create(
        parameters->GetControl(),
        parameters->GetInitialState(),
        parameters->GetMeasurementNoise(),
        parameters->GetProcessNoise(),
        parameters->GetStateTransition()
      )
    }
{
  #ifndef NDEBUG
  Test();
  #endif
  assert(m_parameters);
  assert(m_parameters->GetLag() >= 0);
  assert(boost::numeric_cast<int>(m_measuments.size()) <= m_parameters->GetLag());
  assert(m_system);

  const int lag = m_parameters->GetLag();
  while (lag != boost::numeric_cast<int>(m_measuments.size()))
  {
    m_measuments.push(m_system->Measure());
  }
  assert(lag == boost::numeric_cast<int>(m_measuments.size()));
}

std::string ribi::kalman::LaggedWhiteNoiseSystem::GetVersion() noexcept
{
  return "1.0";
}

std::vector<std::string> ribi::kalman::LaggedWhiteNoiseSystem::GetVersionHistory() noexcept
{
  return {
    "2013-05-03: version 1.0: initial version"
  };
}

void ribi::kalman::LaggedWhiteNoiseSystem::GoToNextState(const boost::numeric::ublas::vector<double>& input)
{
  m_system->GoToNextState(input);
}

boost::numeric::ublas::vector<double> ribi::kalman::LaggedWhiteNoiseSystem::Measure() const noexcept
{
  assert(m_parameters->GetLag() == boost::numeric_cast<int>(m_measuments.size()));
  m_measuments.push(m_system->Measure());

  //Result is copied now, to also work for m_lag == 0
  const boost::numeric::ublas::vector<double> result = m_measuments.front();

  m_measuments.pop();
  assert(m_parameters->GetLag() == boost::numeric_cast<int>(m_measuments.size()));
  return result;
}

const boost::numeric::ublas::vector<double>& ribi::kalman::LaggedWhiteNoiseSystem::PeekAtRealState() const noexcept
{
  return m_system->PeekAtRealState();
}

#ifndef NDEBUG
void ribi::kalman::LaggedWhiteNoiseSystem::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
  //Check if measurements are indeed lagged:
  //The system's real value should update immediatly, but this fresh measurement
  //must only be accessible after lag timesteps
  //Context: measuring the position of an object with constant velocity
  {
    const int lag = 5;
    const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
      = LaggedWhiteNoiseSystemFactory().Create(
        Matrix::CreateMatrix(1,1, { 1.0 } ), //control
        Matrix::CreateVector(     { 0.0 } ), //initial_state,
        lag,
        Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
        Matrix::CreateVector(     { 0.0 } ), //real_process_noise
        Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    );
    const boost::numeric::ublas::vector<double> input = Matrix::CreateVector( { 1.0 } );

    for (int i=0; i!=lag; ++i)
    {
      assert(Matrix::IsAboutEqual( my_system->Measure()(0), 0.0));
      assert(Matrix::IsAboutEqual( my_system->PeekAtRealState()(0), boost::numeric_cast<double>(i) ) );
      my_system->GoToNextState(input);
    }
    for (int i=0; i!=10; ++i) //10 = just some value
    {
      const double expected = boost::numeric_cast<double>(i);
      assert(Matrix::IsAboutEqual( my_system->Measure()(0), expected));
      assert(Matrix::IsAboutEqual( my_system->PeekAtRealState()(0), boost::numeric_cast<double>(lag + i) ) );
      my_system->GoToNextState(input);
    }
  }
}
#endif

 

 

 

 

 

./CppKalmanFilter/laggedwhitenoisesystemfactory.h

 

#ifndef LAGGEDWHITENOISESYSTEMFACTORY_H
#define LAGGEDWHITENOISESYSTEMFACTORY_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "laggedwhitenoisesystem.h"

namespace ribi {
namespace kalman {

///Factory for LaggedWhiteNoiseSystem
struct LaggedWhiteNoiseSystemFactory
{
  LaggedWhiteNoiseSystemFactory();

  ///Create a LaggedWhiteNoiseSystem from the loose parameters
  boost::shared_ptr<LaggedWhiteNoiseSystem> Create(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const int lag,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition
  ) const noexcept;

  ///Create a LaggedWhiteNoiseSystem from the parameters
  boost::shared_ptr<LaggedWhiteNoiseSystem> Create(
    const boost::shared_ptr<WhiteNoiseSystemParameters>& parameters
  ) const noexcept;


  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // LAGGEDWHITENOISESYSTEMFACTORY_H

 

 

 

 

 

./CppKalmanFilter/laggedwhitenoisesystemfactory.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "laggedwhitenoisesystemfactory.h"
#include "testtimer.h"
#include "matrix.h"
#pragma GCC diagnostic pop

ribi::kalman::LaggedWhiteNoiseSystemFactory::LaggedWhiteNoiseSystemFactory()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::shared_ptr<ribi::kalman::LaggedWhiteNoiseSystem>
  ribi::kalman::LaggedWhiteNoiseSystemFactory::Create(
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::vector<double>& initial_state,
  const int lag,
  const boost::numeric::ublas::vector<double>& real_measurement_noise,
  const boost::numeric::ublas::vector<double>& real_process_noise,
  const boost::numeric::ublas::matrix<double>& state_transition
) const noexcept
{
  const boost::shared_ptr<const WhiteNoiseSystemParameters> parameters{
    new LaggedWhiteNoiseSystemParameters(
      control,
      initial_state,
      lag,
      real_measurement_noise,
      real_process_noise,
      state_transition)
  };
  assert(parameters);

  const boost::shared_ptr<LaggedWhiteNoiseSystem> system{
    new LaggedWhiteNoiseSystem(parameters)
  };
  assert(system);
  return system;
}

boost::shared_ptr<ribi::kalman::LaggedWhiteNoiseSystem> ribi::kalman::LaggedWhiteNoiseSystemFactory::Create(
  const boost::shared_ptr<WhiteNoiseSystemParameters>& parameters
) const noexcept
{
  assert(parameters);
  assert(parameters->GetType() == WhiteNoiseSystemType::lagged);
  const boost::shared_ptr<LaggedWhiteNoiseSystemParameters> lagged_parameters
    = boost::dynamic_pointer_cast<LaggedWhiteNoiseSystemParameters>(parameters);
  assert(lagged_parameters);

  const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    = Create(
      lagged_parameters->GetControl(),
      lagged_parameters->GetInitialState(),
      lagged_parameters->GetLag(),
      lagged_parameters->GetMeasurementNoise(),
      lagged_parameters->GetProcessNoise(),
      lagged_parameters->GetStateTransition());
  assert(my_system);
  return my_system;
}


#ifndef NDEBUG
void ribi::kalman::LaggedWhiteNoiseSystemFactory::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    Matrix();
    const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
      = LaggedWhiteNoiseSystemFactory().Create(
        Matrix::CreateMatrix(1,1, { 1.0 } ), //control
        Matrix::CreateVector(     { 0.0 } ), //initial_state,
        0,
        Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
        Matrix::CreateVector(     { 0.0 } ), //real_process_noise
        Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    );
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/laggedwhitenoisesystemparameters.h

 

#ifndef LAGGEDWHITENOISESYSTEMPARAMETERS_H
#define LAGGEDWHITENOISESYSTEMPARAMETERS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "whitenoisesystemparameters.h"
#include "standardwhitenoisesystemparameters.h"

namespace ribi {
namespace kalman {

struct LaggedWhiteNoiseSystemParameters : public WhiteNoiseSystemParameters
{
  explicit LaggedWhiteNoiseSystemParameters(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const int lag,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition);

  ///The lag (in timesteps) the lagged white noise system has
  int GetLag() const noexcept { return m_lag; }

  ///The white noise system parameters of unlagged behavior
  //const boost::shared_ptr<const StandardWhiteNoiseSystemParameters>& GetStandardWhiteNoiseSystemParameters() const
  //  { return m_standard_parameters; }

  ///Obtain the type as an enum
  WhiteNoiseSystemType GetType() const noexcept { return WhiteNoiseSystemType::lagged; }

  private:
  ///Can only be deleted by boost::checked_delete
  ~LaggedWhiteNoiseSystemParameters() noexcept {}
  friend void boost::checked_delete<>(LaggedWhiteNoiseSystemParameters*);

  ///The lag (in timesteps) the lagged white noise system has
  const int m_lag;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // LAGGEDWHITENOISESYSTEMPARAMETERS_H

 

 

 

 

 

./CppKalmanFilter/laggedwhitenoisesystemparameters.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "laggedwhitenoisesystemparameters.h"
#pragma GCC diagnostic pop

#include <cassert>
#include "standardwhitenoisesystemparameters.h"
#include "testtimer.h"

ribi::kalman::LaggedWhiteNoiseSystemParameters::LaggedWhiteNoiseSystemParameters(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const int lag,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition)
  : WhiteNoiseSystemParameters(
      control,
      initial_state,
      real_measurement_noise,
      real_process_noise,
      state_transition
    ),
    m_lag{lag}
{
  assert(lag >= 0);
}

#ifndef NDEBUG
void ribi::kalman::LaggedWhiteNoiseSystemParameters::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/standardkalmanfilter.h

 

#ifndef STANDARDKALMANFILTER_H
#define STANDARDKALMANFILTER_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#pragma GCC diagnostic pop

#include "kalmanfilter.h"
#include "standardkalmanfilterparameters.h"
#include "standardkalmanfiltercalculationelements.h"


namespace ribi {
namespace kalman {

///A discrete time Kalman filter
struct StandardKalmanFilter : public KalmanFilter
{
  ///Obtain the estimation error covariance ('P'), which is updated during SupplyMeasurementAndInput
  const boost::numeric::ublas::matrix<double>& GetEstimationErrorCovariance() const noexcept
    { return m_covariance_estimate; }

  ///Get the Kalman filter last calculation elements
  boost::shared_ptr<KalmanFilterCalculationElements> GetLastCalculation() const noexcept
  { return m_last_standard_calculation; }

  ///Obtain the (downcasted) calculation
  const boost::shared_ptr<StandardKalmanFilterCalculationElements> GetLastStandardCalculation() const noexcept
    { return m_last_standard_calculation; }

  ///Obtain the Kalman filter parameters
  boost::shared_ptr<const KalmanFilterParameters> GetParameters() const noexcept
  { return m_standard_parameters; }

  ///The downcasted parameters
  boost::shared_ptr<const StandardKalmanFilterParameters> GetStandardParameters() const noexcept
    { return m_standard_parameters; }

  ///Obtain the number of values a state consists of
  int GetStateSize() const noexcept;

  ///Obtain the current prediction of the state ('x')
  const boost::numeric::ublas::vector<double>& GetStateEstimate() const noexcept { return m_state_estimate; }

  ///Obtain the Kalman filter type as an enum
  KalmanFilterType GetType() const noexcept { return KalmanFilterType::standard; }

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Give the filter a measurement and input, and it will update its predictions
  ///Also logs the calculation
  void SupplyMeasurementAndInput(
    const boost::numeric::ublas::vector<double>& measurements,
    const boost::numeric::ublas::vector<double>& input);

  ///Let the filter estimate the next state
  boost::numeric::ublas::vector<double> PredictState(
    const boost::numeric::ublas::vector<double>& input) const;

  ///Let the filter estimate the next estimation error covariance
  boost::numeric::ublas::matrix<double> PredictCovariance() const;

  private:
  ///Initialize the filter with a first measurent
  explicit StandardKalmanFilter(
    const boost::shared_ptr<StandardKalmanFilterCalculationElements>& calculation,
    const boost::shared_ptr<const KalmanFilterParameters>& parameters
  );
  friend class StandardKalmanFilterFactory;

  ///Can only be deleted by boost::checked_delete
  ~StandardKalmanFilter() noexcept {}
  friend void boost::checked_delete<>(StandardKalmanFilter*);

  ///P: The estimation error covariance
  boost::numeric::ublas::matrix<double> m_covariance_estimate;

  ///The downcasted parameters
  const boost::shared_ptr<StandardKalmanFilterCalculationElements> m_last_standard_calculation;

  ///The Kalman filter parameters
  //const boost::shared_ptr<const KalmanFilterParameters> m_parameters;

  ///The downcasted parameters
  const boost::shared_ptr<const StandardKalmanFilterParameters> m_standard_parameters;

  ///x: The (current prediction of the) state
  boost::numeric::ublas::vector<double> m_state_estimate;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STANDARDKALMANFILTER_H

 

 

 

 

 

./CppKalmanFilter/standardkalmanfilter.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "standardkalmanfilter.h"

#include <boost/numeric/conversion/cast.hpp>

#include "matrix.h"
#include "trace.h"
#pragma GCC diagnostic pop

ribi::kalman::StandardKalmanFilter::StandardKalmanFilter(
  const boost::shared_ptr<StandardKalmanFilterCalculationElements>& calculation,
  const boost::shared_ptr<const KalmanFilterParameters>& parameters
  )
  : KalmanFilter{},
    m_covariance_estimate{boost::dynamic_pointer_cast<const StandardKalmanFilterParameters>(parameters)->GetInitialCovarianceEstimate()},
    m_last_standard_calculation{boost::dynamic_pointer_cast<StandardKalmanFilterCalculationElements>(calculation)},
    m_standard_parameters{boost::dynamic_pointer_cast<const StandardKalmanFilterParameters>(parameters)},
    m_state_estimate{boost::dynamic_pointer_cast<const StandardKalmanFilterParameters>(parameters)->GetInitialStateEstimate()}
{
  assert(m_last_standard_calculation);
  assert(m_standard_parameters);
  assert(this->GetType() == m_last_standard_calculation->GetType());
  assert(this->GetType() == parameters->GetType());
  assert(this->GetType() == m_standard_parameters->GetType());
  assert(this->GetType() == GetParameters()->GetType()
    && "Initialize each Kalman filter type with the right type of parameters");

  //m_covariance_estimate = m_standard_parameters->GetInitialCovarianceEstimate();
  //m_state_estimate = m_standard_parameters->GetInitialStateEstimate();

  //Do not initialize the calculation yet
  //m_last_standard_calculation->m_predicted_state = m_standard_parameters->GetInitialStateEstimate();
  //m_last_standard_calculation->m_predicted_covariance = m_standard_parameters->GetInitialCovarianceEstimate();

  #ifndef NDEBUG
  //Check for correct dimensionality
  const auto sz = m_state_estimate.size();
  assert(m_standard_parameters->GetControl().size1() == sz);
  assert(m_standard_parameters->GetControl().size2() == sz);
  assert(m_standard_parameters->GetEstimatedMeasurementNoise().size1() == sz);
  assert(m_standard_parameters->GetEstimatedMeasurementNoise().size2() == sz);
  assert(m_standard_parameters->GetObservation().size1() == sz);
  assert(m_standard_parameters->GetObservation().size2() == sz);
  assert(m_covariance_estimate.size1() == sz);
  assert(m_covariance_estimate.size2() == sz);
  assert(m_standard_parameters->GetEstimatedProcessNoiseCovariance().size1() == sz);
  assert(m_standard_parameters->GetEstimatedProcessNoiseCovariance().size2() == sz);
  assert(m_standard_parameters->GetStateTransition().size1() == sz);
  assert(m_standard_parameters->GetStateTransition().size2() == sz);
  assert(m_state_estimate.size() == sz);
  #endif
}

int ribi::kalman::StandardKalmanFilter::GetStateSize() const noexcept
{
  const int sz = boost::numeric_cast<int>(m_state_estimate.size());
  return sz;
}

std::string ribi::kalman::StandardKalmanFilter::GetVersion() noexcept
{
  return "1.0";
}

std::vector<std::string> ribi::kalman::StandardKalmanFilter::GetVersionHistory() noexcept
{
  return {
    "2013-04-28: version 1.0: initial version"
  };
}

boost::numeric::ublas::vector<double> ribi::kalman::StandardKalmanFilter::PredictState(
  const boost::numeric::ublas::vector<double>& input) const
{
  const boost::numeric::ublas::vector<double> state_prediction
    = Matrix::Prod(m_standard_parameters->GetStateTransition(),m_state_estimate)
    + Matrix::Prod(m_standard_parameters->GetControl(),input);
  return state_prediction;
}

boost::numeric::ublas::matrix<double> ribi::kalman::StandardKalmanFilter::PredictCovariance() const
{
  const boost::numeric::ublas::matrix<double> covariance_prediction
    = Matrix::MultiProd(
      m_standard_parameters->GetStateTransition(),
      m_covariance_estimate,
      trans(m_standard_parameters->GetStateTransition())
    )
    + m_standard_parameters->GetEstimatedProcessNoiseCovariance();
  return covariance_prediction;
}


void ribi::kalman::StandardKalmanFilter::SupplyMeasurementAndInput(
  const boost::numeric::ublas::vector<double>& measurements,
  const boost::numeric::ublas::vector<double>& input)
{
  
  using boost::numeric::ublas::identity_matrix;
  using boost::numeric::ublas::matrix;
  using boost::numeric::ublas::trans;
  using boost::numeric::ublas::vector;
  
  ///Debug statements to keep code below clean
  assert(measurements.size() == input.size());
  
  assert(m_standard_parameters->GetStateTransition().size2() == m_covariance_estimate.size1());
  
  assert(Matrix::Prod(m_standard_parameters->GetStateTransition(),m_covariance_estimate).size2()
    ==  trans(m_standard_parameters->GetStateTransition()).size1() );
  
  assert(matrix<double>(Matrix::Prod(
      matrix<double>(Matrix::Prod(m_standard_parameters->GetStateTransition(),m_covariance_estimate)),
      trans(m_standard_parameters->GetStateTransition()))).size1()
    == m_standard_parameters->GetEstimatedProcessNoiseCovariance().size1());
  
  assert(matrix<double>(Matrix::Prod(
      matrix<double>(Matrix::Prod(m_standard_parameters->GetStateTransition(),m_covariance_estimate)),
      trans(m_standard_parameters->GetStateTransition()))).size2()
    == m_standard_parameters->GetEstimatedProcessNoiseCovariance().size2());
  



  //Store calculation for KalmanFilterExperiment
  m_last_standard_calculation->Clear();
  //assert(!m_last_standard_calculation->IsComplete()); //Can be empty and thus complete
  m_last_standard_calculation->SetPreviousStateEstimate(this->GetStateEstimate()); //1
  assert(!m_last_standard_calculation->IsComplete() || input.empty()); //Can be empty or incomplete
  m_last_standard_calculation->SetPreviousCovarianceEstimate(this->GetEstimationErrorCovariance()); //2

  
  // 1/7) State prediction
  const vector<double> state_prediction = PredictState(input);
  // 2/7) Covariance prediction
  const matrix<double> covariance_prediction = PredictCovariance();
  // 3/7) Innovation (y with a squiggle above it)
  const vector<double> innovation = measurements - Matrix::Prod(m_standard_parameters->GetObservation(),state_prediction);
  // 4/7) Innovation covariance (S)
  const matrix<double> innovation_covariance
    = Matrix::MultiProd(m_standard_parameters->GetObservation(),covariance_prediction,trans(m_standard_parameters->GetObservation()))
    + m_standard_parameters->GetEstimatedMeasurementNoise();
  // 5/7) Kalman gain (K)
  
  if (Matrix::CalcDeterminant(innovation_covariance) == 0.0)
  {
    throw std::runtime_error("Innovation covariance became degenerate");
  }
  
  const matrix<double> kalman_gain
    = Matrix::MultiProd(covariance_prediction,trans(m_standard_parameters->GetObservation()),Matrix::Inverse(innovation_covariance));
  // 6/7) Update state prediction
  m_state_estimate = state_prediction + Matrix::Prod(kalman_gain,innovation);
  // 7/7) Update covariance prediction
  const identity_matrix<double> my_identity_matrix(kalman_gain.size1());
  m_covariance_estimate = Matrix::Prod(
    my_identity_matrix
      - Matrix::Prod(kalman_gain,m_standard_parameters->GetObservation()),
    covariance_prediction
  );

  //Store calculation for KalmanFilterExperiment
  
  m_last_standard_calculation->SetPredictedState(state_prediction); //1
  m_last_standard_calculation->SetPredictedCovariance(covariance_prediction); //2
  m_last_standard_calculation->SetInnovation(innovation); //3
  m_last_standard_calculation->SetMeasurement(measurements); //3
  m_last_standard_calculation->SetInnovationCovariance(innovation_covariance); //4
  m_last_standard_calculation->SetKalmanGain(kalman_gain); //5
  m_last_standard_calculation->SetUpdatedState(m_state_estimate); //6
  m_last_standard_calculation->SetUpdatedCovariance(m_covariance_estimate); //7
  assert(m_last_standard_calculation->IsComplete());
  
}

 

 

 

 

 

./CppKalmanFilter/standardkalmanfiltercalculationelements.h

 

#ifndef STANDARDKALMANFILTERCALCULATIONELEMENTS_H
#define STANDARDKALMANFILTERCALCULATIONELEMENTS_H

#include "kalmanfiltertype.h"
#include "kalmanfiltercalculationelements.h"

namespace ribi {
namespace kalman {

///The elements of a standard Kalman filter calculation
struct StandardKalmanFilterCalculationElements : public KalmanFilterCalculationElements
{
  explicit StandardKalmanFilterCalculationElements(
    //Base
    const boost::numeric::ublas::vector<double>& measurement = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::vector<double>& predicted_state = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::vector<double>& previous_state_estimate = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::vector<double>& updated_state = boost::numeric::ublas::vector<double>(),
    //This class
    const boost::numeric::ublas::vector<double>& innovation = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::matrix<double>& innovation_covariance = boost::numeric::ublas::matrix<double>(),
    const boost::numeric::ublas::matrix<double>& kalman_gain = boost::numeric::ublas::matrix<double>(),
    const boost::numeric::ublas::matrix<double>& predicted_covariance = boost::numeric::ublas::matrix<double>(),
    const boost::numeric::ublas::matrix<double>& previous_covariance_estimate = boost::numeric::ublas::matrix<double>(),
    const boost::numeric::ublas::matrix<double>& updated_covariance = boost::numeric::ublas::matrix<double>());

  ///Clear the calculation, will set IsComplete to false
  void Clear();

  ///Produce a deep copy of the derived class
  boost::shared_ptr<KalmanFilterCalculationElements> Clone() const;

  ///Obtain the innovation ('y_squiggle')
  const boost::numeric::ublas::vector<double>& GetInnovation() const
    { return m_innovation; }

  ///Obtain he innovation covariance ('S')
  const boost::numeric::ublas::matrix<double>& GetInnovationCovariance() const
    { return m_innovation_covariance; }

  ///Obtain the Kalman gain ('K')
  const boost::numeric::ublas::matrix<double>& GetKalmanGain() const
    { return m_kalman_gain; }

  ///Obtain the predicted error estimation covariance ('P_predicted')
  const boost::numeric::ublas::matrix<double>& GetPredicatedCovariance() const
    { return m_predicted_covariance; }

  ///Obtain the previous_covariance_estimate ('P_prev'/'P_n-1')
  const boost::numeric::ublas::matrix<double>& GetPreviousCovarianceEstimate() const
    { return m_previous_covariance_estimate; }

  ///Obtain the Kalman filter type as an enum
  KalmanFilterType GetType() const { return KalmanFilterType::standard; }

  ///Obtain the updated error estimation covariance (in the end of each timestep)
  const boost::numeric::ublas::matrix<double>& GetUpdatedCovariance() const
    { return m_updated_covariance; }

  ///Checks if the state is complete and valid
  bool IsComplete() const;

  ///Set the innovation ('y_squiggle')
  ///Fails if already set
  void SetInnovation(const boost::numeric::ublas::vector<double>& innovation);

  ///Set the innovation covariance ('S')
  ///Fails if already set
  void SetInnovationCovariance(const boost::numeric::ublas::matrix<double>& innovation_covariance);

  ///Set the Kalman gain ('K')
  ///Fails if already set
  void SetKalmanGain(const boost::numeric::ublas::matrix<double>& kalman_gain);

  ///Set the predicted error estimation covariance ('P_predicted')
  ///Calculated at step 2 of the algorithm:
  ///2) P_predicted = [...] P_n-1 [...]
  ///Fails if already set
  void SetPredictedCovariance(const boost::numeric::ublas::matrix<double>& predicted_covariance);

  ///Set the previous_covariance_estimate ('P_prev'/'P_n-1')
  ///Is read at step 2 of the algorithm:
  ///2) P_predicted = [...] P_n-1 [...]
  ///Fails if already set
  void SetPreviousCovarianceEstimate(const boost::numeric::ublas::matrix<double>& previous_covariance_estimate);

  ///Set the updated error estimation covariance (in the end of each timestep)
  ///Calculated at step 7 of the algorithm:
  ///2) P_n = [...] P_predicted
  ///Fails if already set
  void SetUpdatedCovariance(const boost::numeric::ublas::matrix<double>& updated_covariance);

  private:
  ///Can only be deleted by boost::checked_delete
  ~StandardKalmanFilterCalculationElements() noexcept {}
  friend void boost::checked_delete<>(StandardKalmanFilterCalculationElements*);

  ///The innovation ('y_squiggle')
  boost::numeric::ublas::vector<double> m_innovation;

  ///The innovation covariance ('S')
  boost::numeric::ublas::matrix<double> m_innovation_covariance;

  ///The Kalman gain ('K')
  boost::numeric::ublas::matrix<double> m_kalman_gain;

  ///The predicted error estimation covariance ('P_predicted')
  ///Calculated at step 2 of the algorithm:
  ///2) P_predicted = [...] P_n-1 [...]
  boost::numeric::ublas::matrix<double> m_predicted_covariance;

  ///The previous_covariance_estimate ('P_prev'/'P_n-1')
  ///Is read at step 2 of the algorithm:
  ///2) P_predicted = [...] P_n-1 [...]
  boost::numeric::ublas::matrix<double> m_previous_covariance_estimate;

  ///The updated error estimation covariance (in the end of each timestep)
  ///Calculated at step 7 of the algorithm:
  ///2) P_n = [...] P_predicted
  boost::numeric::ublas::matrix<double> m_updated_covariance;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STANDARDKALMANFILTERCALCULATIONELEMENTS_H

 

 

 

 

 

./CppKalmanFilter/standardkalmanfiltercalculationelements.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "standardkalmanfiltercalculationelements.h"
#pragma GCC diagnostic pop

#include <cassert>

ribi::kalman::StandardKalmanFilterCalculationElements::StandardKalmanFilterCalculationElements(
  //Base
  const boost::numeric::ublas::vector<double>& measurement,
  const boost::numeric::ublas::vector<double>& predicted_state,
  const boost::numeric::ublas::vector<double>& previous_state_estimate,
  const boost::numeric::ublas::vector<double>& updated_state,
  //This class
  const boost::numeric::ublas::vector<double>& innovation,
  const boost::numeric::ublas::matrix<double>& innovation_covariance,
  const boost::numeric::ublas::matrix<double>& kalman_gain,
  const boost::numeric::ublas::matrix<double>& predicted_covariance,
  const boost::numeric::ublas::matrix<double>& previous_covariance_estimate,
  const boost::numeric::ublas::matrix<double>& updated_covariance)
  : KalmanFilterCalculationElements(
      measurement,
      predicted_state,
      previous_state_estimate,
      updated_state
    ),
    m_innovation{innovation},
    m_innovation_covariance{innovation_covariance},
    m_kalman_gain{kalman_gain},
    m_predicted_covariance{predicted_covariance},
    m_previous_covariance_estimate{previous_covariance_estimate},
    m_updated_covariance{updated_covariance}
{
  #ifndef NDEBUG
  //size may be zero
  const std::size_t sz = m_innovation.size();
  //Base
  assert(sz == GetMeasurement().size());
  assert(sz == GetPredictedState().size());
  assert(sz == GetPreviousState().size());
  assert(sz == GetUpdatedState().size());
  //This class
  assert(sz == m_innovation.size());
  assert(sz == m_innovation_covariance.size1());
  assert(sz == m_innovation_covariance.size2());
  assert(sz == m_kalman_gain.size1());
  assert(sz == m_kalman_gain.size2());
  assert(sz == m_predicted_covariance.size1());
  assert(sz == m_predicted_covariance.size2());
  assert(sz == m_previous_covariance_estimate.size1());
  assert(sz == m_previous_covariance_estimate.size2());
  assert(sz == m_updated_covariance.size1());
  assert(sz == m_updated_covariance.size2());
  #endif
}

void ribi::kalman::StandardKalmanFilterCalculationElements::Clear()
{
  KalmanFilterCalculationElements::Clear();
  m_innovation = boost::numeric::ublas::vector<double>();
  assert(m_innovation.empty());
  m_innovation_covariance = boost::numeric::ublas::matrix<double>();
  m_kalman_gain = boost::numeric::ublas::matrix<double>();
  m_predicted_covariance = boost::numeric::ublas::matrix<double>();
  m_previous_covariance_estimate = boost::numeric::ublas::matrix<double>();
  m_updated_covariance = boost::numeric::ublas::matrix<double>();
}

boost::shared_ptr<ribi::kalman::KalmanFilterCalculationElements> ribi::kalman::StandardKalmanFilterCalculationElements::Clone() const
{
  const boost::shared_ptr<KalmanFilterCalculationElements> p(
    new StandardKalmanFilterCalculationElements(
      //Base
      this->GetMeasurement(),
      this->GetPredictedState(),
      this->GetPreviousState(),
      this->GetUpdatedState(),
      //Derived class
      this->GetInnovation(),
      this->GetInnovationCovariance(),
      this->GetKalmanGain(),
      this->GetPredicatedCovariance(),
      this->GetPreviousCovarianceEstimate(),
      this->GetUpdatedCovariance()
    )
  );
  assert(p);
  assert(p->GetType() == this->GetType());
  return p;
}

bool ribi::kalman::StandardKalmanFilterCalculationElements::IsComplete() const
{
  const std::size_t sz = m_innovation.size();
  return
       //sz != 0
       sz == m_innovation.size()
    && sz == m_innovation_covariance.size1()
    && sz == m_innovation_covariance.size2()
    && sz == m_innovation_covariance.size1()
    && sz == m_innovation_covariance.size2()
    && sz == m_kalman_gain.size1()
    && sz == m_kalman_gain.size2()
    && sz == GetMeasurement().size()
    && sz == m_predicted_covariance.size1()
    && sz == m_predicted_covariance.size2()
    && sz == GetPredictedState().size()
    && sz == m_previous_covariance_estimate.size1()
    && sz == m_previous_covariance_estimate.size2()
    && sz == GetPreviousState().size()
    && sz == m_updated_covariance.size1()
    && sz == m_updated_covariance.size2()
    && sz == GetUpdatedState().size();
}

void ribi::kalman::StandardKalmanFilterCalculationElements::SetInnovation(const boost::numeric::ublas::vector<double>& innovation)
{
  assert(m_innovation.empty());
  m_innovation = innovation;
}

void ribi::kalman::StandardKalmanFilterCalculationElements::SetInnovationCovariance(const boost::numeric::ublas::matrix<double>& innovation_covariance)
{
  assert(m_innovation_covariance.size1() == 0);
  assert(m_innovation_covariance.size2() == 0);
  m_innovation_covariance = innovation_covariance;
}

void ribi::kalman::StandardKalmanFilterCalculationElements::SetKalmanGain(const boost::numeric::ublas::matrix<double>& kalman_gain)
{
  assert(m_kalman_gain.size1() == 0);
  assert(m_kalman_gain.size2() == 0);
  m_kalman_gain = kalman_gain;
}

void ribi::kalman::StandardKalmanFilterCalculationElements::SetPredictedCovariance(const boost::numeric::ublas::matrix<double>& predicted_covariance)
{
  assert(m_predicted_covariance.size1() == 0);
  assert(m_predicted_covariance.size2() == 0);
  m_predicted_covariance = predicted_covariance;
}

void ribi::kalman::StandardKalmanFilterCalculationElements::SetPreviousCovarianceEstimate(const boost::numeric::ublas::matrix<double>& previous_covariance_estimate)
{
  assert(m_previous_covariance_estimate.size1() == 0);
  assert(m_previous_covariance_estimate.size2() == 0);
  m_previous_covariance_estimate = previous_covariance_estimate;
}

void ribi::kalman::StandardKalmanFilterCalculationElements::SetUpdatedCovariance(const boost::numeric::ublas::matrix<double>& updated_covariance)
{
  assert(m_updated_covariance.size1() == 0);
  assert(m_updated_covariance.size2() == 0);
  m_updated_covariance = updated_covariance;
}

 

 

 

 

 

./CppKalmanFilter/standardkalmanfilterfactory.h

 

#ifndef STANDARDKALMANFILTERFACTORY_H
#define STANDARDKALMANFILTERFACTORY_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "standardkalmanfilter.h"

namespace ribi {
namespace kalman {

///Factory for StandardKalmanFilter
struct StandardKalmanFilterFactory
{
  StandardKalmanFilterFactory();

  boost::shared_ptr<StandardKalmanFilter> Create(
    const boost::shared_ptr<const KalmanFilterParameters>& parameters
  ) const noexcept;

  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STANDARDKALMANFILTERFACTORY_H

 

 

 

 

 

./CppKalmanFilter/standardkalmanfilterfactory.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "standardkalmanfilterfactory.h"

#include <cassert>

#include "testtimer.h"
#pragma GCC diagnostic pop

ribi::kalman::StandardKalmanFilterFactory::StandardKalmanFilterFactory()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::shared_ptr<ribi::kalman::StandardKalmanFilter> ribi::kalman::StandardKalmanFilterFactory::Create(
  const boost::shared_ptr<const KalmanFilterParameters>& parameters) const noexcept
{
  assert(parameters);
  assert(parameters->GetType() == KalmanFilterType::standard);

  const boost::shared_ptr<StandardKalmanFilterCalculationElements> calculation{
    new StandardKalmanFilterCalculationElements
  };

  assert(calculation);

  const boost::shared_ptr<StandardKalmanFilter> kalman_filter{
    new StandardKalmanFilter(calculation,parameters)
  };
  assert(kalman_filter);
  assert(kalman_filter->GetType() == KalmanFilterType::standard);
  return kalman_filter;
}


#ifndef NDEBUG
void ribi::kalman::StandardKalmanFilterFactory::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/standardkalmanfilterparameters.h

 

#ifndef STANDARDKALMANFILTERPARAMETERS_H
#define STANDARDKALMANFILTERPARAMETERS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/checked_delete.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#pragma GCC diagnostic pop

#include "kalmanfilterparameters.h"

namespace ribi {
namespace kalman {

///Parameters for the standard Kalman filter
struct StandardKalmanFilterParameters : public KalmanFilterParameters
{
  explicit StandardKalmanFilterParameters(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::matrix<double>& estimated_measurement_noise,
    const boost::numeric::ublas::matrix<double>& estimated_process_noise_covariance,
    const boost::numeric::ublas::matrix<double>& initial_covariance_estimate,
    const boost::numeric::ublas::vector<double>& initial_state_estimate,
    const boost::numeric::ublas::matrix<double>& observation,
    const boost::numeric::ublas::matrix<double>& state_transition
  );

  ///Obtain the (estimated) measurement noise ('R')
  const boost::numeric::ublas::matrix<double>& GetEstimatedMeasurementNoise() const noexcept
    { return m_estimated_measurement_noise; }

  ///P: The initial estimation error covariance estimate
  const boost::numeric::ublas::matrix<double>& GetInitialCovarianceEstimate() const noexcept
    { return m_initial_covariance_estimate; }

  ///Obtain the process noise covariance ('Q')
  const boost::numeric::ublas::matrix<double>& GetEstimatedProcessNoiseCovariance() const noexcept
    { return m_estimated_process_noise_covariance; }

  ///Obtain the Kalman filter type as an enum
  KalmanFilterType GetType() const noexcept { return KalmanFilterType::standard; }

  ///Check if two parameter sets are equal with a fuzzy compare
  static bool IsAboutEqual(
    const StandardKalmanFilterParameters& lhs,
    const StandardKalmanFilterParameters& rhs) noexcept;

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Check if this parameter set has a certain type of KalmanFilterParameter
  static bool HasParameterType(const KalmanFilterParameterType type) noexcept;

  private:
  ///Can only be deleted by boost::checked_delete
  ~StandardKalmanFilterParameters() noexcept {}
  friend void boost::checked_delete<>(StandardKalmanFilterParameters*);

  ///R: Estimated measurement noise
  const boost::numeric::ublas::matrix<double> m_estimated_measurement_noise;

  ///Q: The estimated process noise covariance
  const boost::numeric::ublas::matrix<double> m_estimated_process_noise_covariance;

  ///P: The initial estimation error covariance estimate
  const boost::numeric::ublas::matrix<double> m_initial_covariance_estimate;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STANDARDKALMANFILTERPARAMETERS_H

 

 

 

 

 

./CppKalmanFilter/standardkalmanfilterparameters.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "standardkalmanfilterparameters.h"
#pragma GCC diagnostic pop

#include <cassert>

#include "matrix.h"

ribi::kalman::StandardKalmanFilterParameters::StandardKalmanFilterParameters(
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::matrix<double>& estimated_measurement_noise,
  const boost::numeric::ublas::matrix<double>& estimated_process_noise_covariance,
  const boost::numeric::ublas::matrix<double>& initial_covariance_estimate,
  const boost::numeric::ublas::vector<double>& initial_state_estimate,
  const boost::numeric::ublas::matrix<double>& observation,
  const boost::numeric::ublas::matrix<double>& state_transition)
  : KalmanFilterParameters{control,initial_state_estimate,observation,state_transition},
    m_estimated_measurement_noise{estimated_measurement_noise},
    m_estimated_process_noise_covariance{estimated_process_noise_covariance},
    m_initial_covariance_estimate{initial_covariance_estimate}
{
  #ifndef NDEBUG
  //Check for correct dimensionality
  const std::size_t sz = GetInitialStateEstimate().size();
  //assert(sz >= 0); //Inevitable for std::size_t
  assert(GetEstimatedMeasurementNoise().size1() == sz);
  assert(GetEstimatedMeasurementNoise().size2() == sz);
  assert(GetEstimatedProcessNoiseCovariance().size1() == sz);
  assert(GetEstimatedProcessNoiseCovariance().size2() == sz);
  assert(GetInitialCovarianceEstimate().size1() == sz);
  assert(GetInitialCovarianceEstimate().size2() == sz);
  #endif
}

std::string ribi::kalman::StandardKalmanFilterParameters::GetVersion() noexcept
{
  return "1.1";
}

std::vector<std::string> ribi::kalman::StandardKalmanFilterParameters::GetVersionHistory() noexcept
{
  return {
    "2013-04-28: version 1.0: initial version",
    "2013-05-03: version 1.1: inhertic from KalmanFilterParameters"
  };
}

bool ribi::kalman::StandardKalmanFilterParameters::HasParameterType(const KalmanFilterParameterType type) noexcept
{
  return
       type == KalmanFilterParameterType::control
    || type == KalmanFilterParameterType::estimated_measurement_noise
    || type == KalmanFilterParameterType::estimated_process_noise_covariance
    || type == KalmanFilterParameterType::initial_covariance_estimate
    || type == KalmanFilterParameterType::initial_state_estimate
    || type == KalmanFilterParameterType::observation
    || type == KalmanFilterParameterType::state_transition;
}

bool ribi::kalman::StandardKalmanFilterParameters::IsAboutEqual(const StandardKalmanFilterParameters& lhs, const StandardKalmanFilterParameters& rhs) noexcept
{
  return
       Matrix::MatricesAreAboutEqual(lhs.GetControl(),rhs.GetControl())
    && Matrix::MatricesAreAboutEqual(lhs.GetEstimatedMeasurementNoise(),rhs.GetEstimatedMeasurementNoise())
    && Matrix::MatricesAreAboutEqual(lhs.GetInitialCovarianceEstimate(),rhs.GetInitialCovarianceEstimate())
    && Matrix::VectorsAreAboutEqual(lhs.GetInitialStateEstimate(),rhs.GetInitialStateEstimate())
    && Matrix::MatricesAreAboutEqual(lhs.GetObservation(),rhs.GetObservation())
    && Matrix::MatricesAreAboutEqual(lhs.GetEstimatedProcessNoiseCovariance(),rhs.GetEstimatedProcessNoiseCovariance())
    && Matrix::MatricesAreAboutEqual(lhs.GetStateTransition(),rhs.GetStateTransition());
}

 

 

 

 

 

./CppKalmanFilter/standardwhitenoisesystem.h

 

#ifndef STANDARDWHITENOISESYSTEM_H
#define STANDARDWHITENOISESYSTEM_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/ublas/matrix.hpp>
#pragma GCC diagnostic pop

#include "standardwhitenoisesystemparameters.h"
#include "whitenoisesystem.h"
#include "whitenoisesystemparameters.h"

namespace ribi {
namespace kalman {

struct StandardWhiteNoiseSystem : public WhiteNoiseSystem
{
  ///Obtain the type as an enum
  WhiteNoiseSystemType GetType() const noexcept { return WhiteNoiseSystemType::standard; }

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Update reality, that is, let the real system (i.e. reality) go to its next state,
  ///without any input
  void GoToNextState();

  ///Update reality, that is, let the real system (i.e. reality) go to its next state
  void GoToNextState(const boost::numeric::ublas::vector<double>& input);

  ///Measure a value from this system with normally distributed noise
  boost::numeric::ublas::vector<double> Measure() const noexcept;

  private:
  ///StandardWhiteNoiseSystem can only be created by a StandardWhiteNoiseSystemFactory
  explicit StandardWhiteNoiseSystem(const boost::shared_ptr<const WhiteNoiseSystemParameters>& parameters);
  friend class StandardWhiteNoiseSystemFactory;

  ///Can only be deleted by boost::checked_delete
  ~StandardWhiteNoiseSystem() noexcept {}
  friend void boost::checked_delete<>(StandardWhiteNoiseSystem*);

  ///The parameters for the white noise system
  const boost::shared_ptr<const StandardWhiteNoiseSystemParameters> m_parameters;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STANDARDWHITENOISESYSTEM_H

 

 

 

 

 

./CppKalmanFilter/standardwhitenoisesystem.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "standardwhitenoisesystem.h"

#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/normal_distribution.hpp>
#pragma GCC diagnostic pop

#include "matrix.h"
#include "trace.h"
#include "standardwhitenoisesystemparameters.h"

ribi::kalman::StandardWhiteNoiseSystem::StandardWhiteNoiseSystem(
  const boost::shared_ptr<const WhiteNoiseSystemParameters>& parameters)
  : WhiteNoiseSystem{parameters},
    m_parameters{boost::dynamic_pointer_cast<const StandardWhiteNoiseSystemParameters>(parameters)}
{
  assert(m_parameters);
}

std::string ribi::kalman::StandardWhiteNoiseSystem::GetVersion() noexcept
{
  return "1.0";
}

std::vector<std::string> ribi::kalman::StandardWhiteNoiseSystem::GetVersionHistory() noexcept
{
  return {
    "2013-04-28: version 1.0: initial version"
  };
}

void ribi::kalman::StandardWhiteNoiseSystem::GoToNextState()
{
  //Create a no-input vector
  const auto sz = GetCurrentState().size();
  boost::numeric::ublas::vector<double> input(sz,0.0);
  return GoToNextState(input);
}

void ribi::kalman::StandardWhiteNoiseSystem::GoToNextState(const boost::numeric::ublas::vector<double>& input)
{
  //First do a perfect transition
  assert(input.size() == GetCurrentState().size());
  assert(m_parameters->GetStateTransition().size1() == GetCurrentState().size());
  assert(m_parameters->GetStateTransition().size2() == GetCurrentState().size());
  assert(m_parameters->GetControl().size1() == input.size());
  assert(m_parameters->GetControl().size2() == input.size());

  boost::numeric::ublas::vector<double> new_state
    = Matrix::Prod(m_parameters->GetStateTransition(),GetCurrentState())
    + Matrix::Prod(m_parameters->GetControl(),input);
  //Add process noise
  const auto sz = new_state.size();
  assert(new_state.size() == m_parameters->GetProcessNoise().size());
  for (std::size_t i = 0; i!=sz; ++i)
  {
    new_state(i) = GetRandomNormal(new_state(i),m_parameters->GetProcessNoise()(i));
  }
  SetNewCurrentState(new_state);
}

boost::numeric::ublas::vector<double> ribi::kalman::StandardWhiteNoiseSystem::Measure() const noexcept
{
  const auto sz = GetCurrentState().size();
  assert(GetCurrentState().size() == m_parameters->GetMeasurementNoise().size());
  boost::numeric::ublas::vector<double> measured(sz);
  for (std::size_t i = 0; i!=sz; ++i)
  {
    measured(i) = GetRandomNormal(GetCurrentState()(i),m_parameters->GetMeasurementNoise()(i));
  }
  return measured;
}

 

 

 

 

 

./CppKalmanFilter/standardwhitenoisesystemfactory.h

 

#ifndef STANDARDWHITENOISESYSTEMFACTORY_H
#define STANDARDWHITENOISESYSTEMFACTORY_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#pragma GCC diagnostic pop

#include "standardwhitenoisesystem.h"

namespace ribi {
namespace kalman {

struct StandardWhiteNoiseSystemFactory
{
  StandardWhiteNoiseSystemFactory();

  boost::shared_ptr<StandardWhiteNoiseSystem> Create(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition
  ) const noexcept;

  boost::shared_ptr<StandardWhiteNoiseSystem> Create(
    const boost::shared_ptr<WhiteNoiseSystemParameters>& parameters
  ) const noexcept;

  private:

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STANDARDWHITENOISESYSTEMFACTORY_H

 

 

 

 

 

./CppKalmanFilter/standardwhitenoisesystemfactory.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "standardwhitenoisesystemfactory.h"

#include "testtimer.h"
#pragma GCC diagnostic pop

ribi::kalman::StandardWhiteNoiseSystemFactory::StandardWhiteNoiseSystemFactory()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::shared_ptr<ribi::kalman::StandardWhiteNoiseSystem>
  ribi::kalman::StandardWhiteNoiseSystemFactory::Create(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition
  ) const noexcept
{
  boost::shared_ptr<const ribi::kalman::StandardWhiteNoiseSystemParameters> parameters{
    new StandardWhiteNoiseSystemParameters(
      control,
      initial_state,
      real_measurement_noise,
      real_process_noise,
      state_transition
    )
  };
  assert(parameters);
  const boost::shared_ptr<StandardWhiteNoiseSystem> system{
    new StandardWhiteNoiseSystem(parameters)
  };
  assert(system);
  return system;
}

boost::shared_ptr<ribi::kalman::StandardWhiteNoiseSystem>
  ribi::kalman::StandardWhiteNoiseSystemFactory::Create(
    const boost::shared_ptr<WhiteNoiseSystemParameters>& parameters
  ) const noexcept
{
  assert(parameters);
  assert(parameters->GetType() == WhiteNoiseSystemType::standard);
  const boost::shared_ptr<StandardWhiteNoiseSystemParameters> standard_parameters
    = boost::dynamic_pointer_cast<StandardWhiteNoiseSystemParameters>(parameters);
  assert(standard_parameters);
  const boost::shared_ptr<StandardWhiteNoiseSystem> system
    = Create(
      standard_parameters->GetControl(),
      standard_parameters->GetInitialState(),
      standard_parameters->GetMeasurementNoise(),
      standard_parameters->GetProcessNoise(),
      standard_parameters->GetStateTransition());
  assert(system);
  return system;
}

#ifndef NDEBUG
void ribi::kalman::StandardWhiteNoiseSystemFactory::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/standardwhitenoisesystemparameters.h

 

#ifndef STANDARDWHITENOISESYSTEMPARAMETERS_H
#define STANDARDWHITENOISESYSTEMPARAMETERS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/checked_delete.hpp>
#include "whitenoisesystemparameters.h"
#pragma GCC diagnostic pop

namespace ribi {
namespace kalman {

struct StandardWhiteNoiseSystemParameters : public WhiteNoiseSystemParameters
{
  explicit StandardWhiteNoiseSystemParameters(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition
  );

  ///Obtain the type as an enum
  WhiteNoiseSystemType GetType() const noexcept { return WhiteNoiseSystemType::standard; }

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Check two parameter sets for equality using a fuzzy comparison
  static bool IsAboutEqual(const StandardWhiteNoiseSystemParameters& lhs, const StandardWhiteNoiseSystemParameters& rhs) noexcept;

  private:
  ///Can only be deleted by boost::checked_delete
  ~StandardWhiteNoiseSystemParameters() noexcept {}
  friend void boost::checked_delete<>(StandardWhiteNoiseSystemParameters*);

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STANDARDWHITENOISESYSTEMPARAMETERS_H

 

 

 

 

 

./CppKalmanFilter/standardwhitenoisesystemparameters.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "standardwhitenoisesystemparameters.h"
#pragma GCC diagnostic pop

#include <cassert>
#include "matrix.h"

ribi::kalman::StandardWhiteNoiseSystemParameters::StandardWhiteNoiseSystemParameters(
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::vector<double>& initial_state,
  const boost::numeric::ublas::vector<double>& real_measurement_noise,
  const boost::numeric::ublas::vector<double>& real_process_noise,
  const boost::numeric::ublas::matrix<double>& state_transition)
  : WhiteNoiseSystemParameters(
      control,
      initial_state,
      real_measurement_noise,
      real_process_noise,
      state_transition)
{
  #ifndef NDEBUG
  #endif

}

std::string ribi::kalman::StandardWhiteNoiseSystemParameters::GetVersion() noexcept
{
  return "1.0";
}

std::vector<std::string> ribi::kalman::StandardWhiteNoiseSystemParameters::GetVersionHistory() noexcept
{
  return {
    "2013-04-28: version 1.0: initial version"
  };
}

bool ribi::kalman::StandardWhiteNoiseSystemParameters::IsAboutEqual(
  const StandardWhiteNoiseSystemParameters& lhs, const StandardWhiteNoiseSystemParameters& rhs) noexcept
{
  return
       Matrix::MatricesAreAboutEqual(lhs.GetControl(),rhs.GetControl())
    && Matrix::VectorsAreAboutEqual(lhs.GetInitialState(),rhs.GetInitialState())
    && Matrix::VectorsAreAboutEqual(lhs.GetMeasurementNoise(),rhs.GetMeasurementNoise())
    && Matrix::VectorsAreAboutEqual(lhs.GetProcessNoise(),rhs.GetProcessNoise())
    && Matrix::MatricesAreAboutEqual(lhs.GetStateTransition(),rhs.GetStateTransition());
}

 

 

 

 

 

./CppKalmanFilter/steadystatekalmanfilter.h

 

#ifndef STEADYSTATEKALMANFILTER_H
#define STEADYSTATEKALMANFILTER_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#pragma GCC diagnostic pop


#include "kalmanfilter.h"
#include "steadystatekalmanfilterparameters.h"
#include "steadystatekalmanfiltercalculationelements.h"

namespace ribi {
namespace kalman {

///A discrete time Kalman filter
struct SteadyStateKalmanFilter : public KalmanFilter
{
  SteadyStateKalmanFilter(const SteadyStateKalmanFilter&) = delete;
  SteadyStateKalmanFilter& operator=(const SteadyStateKalmanFilter&) = delete;

  ///Clear the calculation, will set IsComplete to false
  //void Clear();

  ///Get the Kalman filter last calculation elements
  boost::shared_ptr<KalmanFilterCalculationElements> GetLastCalculation() const noexcept
  { return m_last_calculation; }

  ///Obtain the Kalman filter parameters
  boost::shared_ptr<const KalmanFilterParameters> GetParameters() const noexcept
  { return m_parameters; }

  ///Obtain the number of values a state consists of
  int GetStateSize() const noexcept;

  ///Obtain the current prediction of the state ('x')
  const boost::numeric::ublas::vector<double>& GetStateEstimate() const noexcept
   { return m_state_estimate; }

  ///Obtain the Kalman filter type as an enum
  KalmanFilterType GetType() const noexcept { return KalmanFilterType::steady_state; }

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Give the filter a measurement and input, and it will update its predictions
  void SupplyMeasurementAndInput(
    const boost::numeric::ublas::vector<double>& measurements,
    const boost::numeric::ublas::vector<double>& input);

  ///Let the filter estimate the next state
  boost::numeric::ublas::vector<double> PredictState(
    const boost::numeric::ublas::vector<double>& input) const;

  private:
  ///Initialize the filter with a first measurent
  explicit SteadyStateKalmanFilter(
    const boost::shared_ptr<KalmanFilterCalculationElements>& calculation,
    const boost::shared_ptr<const KalmanFilterParameters>& parameters);
  friend class SteadyStateKalmanFilterFactory;

  ///Can only be deleted by boost::checked_delete
  ~SteadyStateKalmanFilter() noexcept {}
  friend void boost::checked_delete<>(SteadyStateKalmanFilter*);

  ///The last calculation
  const boost::shared_ptr<SteadyStateKalmanFilterCalculationElements> m_last_calculation;

  ///The downcasted parameters
  const boost::shared_ptr<const SteadyStateKalmanFilterParameters> m_parameters;

  ///x: The (current prediction of the) state
  boost::numeric::ublas::vector<double> m_state_estimate;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STEADYSTATEKALMANFILTER_H

 

 

 

 

 

./CppKalmanFilter/steadystatekalmanfilter.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "steadystatekalmanfilter.h"

#include <boost/numeric/conversion/cast.hpp>

#include "matrix.h"
#include "kalmanfilter.h"
#include "kalmanfilterparameters.h"
#include "steadystatekalmanfilterparameters.h"
#include "trace.h"
#pragma GCC diagnostic pop

ribi::kalman::SteadyStateKalmanFilter::SteadyStateKalmanFilter(
  const boost::shared_ptr<KalmanFilterCalculationElements>& calculation,
  const boost::shared_ptr<const KalmanFilterParameters>& parameters)
  : KalmanFilter{},
    m_last_calculation{boost::dynamic_pointer_cast<SteadyStateKalmanFilterCalculationElements>(calculation)},
    m_parameters{boost::dynamic_pointer_cast<const SteadyStateKalmanFilterParameters>(parameters)},
    m_state_estimate{dynamic_cast<const SteadyStateKalmanFilterParameters*>(parameters.get())->GetInitialStateEstimate()}
{
  assert(m_last_calculation);
  assert(m_parameters
    && "Initialize each Kalman filter type with the right type of parameters");
  #ifndef NDEBUG
  //Check for correct dimensionality
  const auto sz = m_state_estimate.size();
  assert(m_parameters->GetControl().size1() == sz);
  assert(m_parameters->GetControl().size2() == sz);
  assert(m_parameters->GetEstimatedOptimalKalmanGain().size1() == sz);
  assert(m_parameters->GetEstimatedOptimalKalmanGain().size2() == sz);
  assert(m_parameters->GetObservation().size1() == sz);
  assert(m_parameters->GetObservation().size2() == sz);
  assert(m_parameters->GetStateTransition().size1() == sz);
  assert(m_parameters->GetStateTransition().size2() == sz);
  assert(m_state_estimate.size() == sz);
  #endif

  //Initialize the last calculation with null values
  //NEW 2013-05-23
  //m_last_calculation->SetMeasurement(
  //  boost::numeric::ublas::zero_vector<double>(m_state_estimate.size()));
  //m_last_calculation->SetPredictedState(m_parameters->GetInitialStateEstimate());
  //m_last_calculation->SetPreviousStateEstimate(m_parameters->GetInitialStateEstimate());
  //m_last_calculation->SetUpdatedState(m_parameters->GetInitialStateEstimate());
}

int ribi::kalman::SteadyStateKalmanFilter::GetStateSize() const noexcept
{
  const int sz = boost::numeric_cast<int>(m_state_estimate.size());
  return sz;
}

std::string ribi::kalman::SteadyStateKalmanFilter::GetVersion() noexcept
{
  return "1.1";
}

std::vector<std::string> ribi::kalman::SteadyStateKalmanFilter::GetVersionHistory() noexcept
{
  return {
    "2013-05-01: version 1.0: initial version",
    "2013-05-06: version 1.1: inherits from KalmanFilter"
  };
}

boost::numeric::ublas::vector<double> ribi::kalman::SteadyStateKalmanFilter::PredictState(
  const boost::numeric::ublas::vector<double>& input) const
{
  const boost::numeric::ublas::matrix<double> term_a
    = m_parameters->GetStateTransition()
    - Matrix::Prod(
      m_parameters->GetEstimatedOptimalKalmanGain(),
      m_parameters->GetObservation()
    );

  //m_last_measured must be set before calling PredictState!
  assert(!m_last_calculation->GetMeasurement().empty());

  const boost::numeric::ublas::vector<double> state_prediction
    = Matrix::Prod(term_a,this->GetStateEstimate())
    + Matrix::Prod(m_parameters->GetControl(),input)
    + Matrix::Prod(m_parameters->GetEstimatedOptimalKalmanGain(),m_last_calculation->GetMeasurement());
  return state_prediction;
}

void ribi::kalman::SteadyStateKalmanFilter::SupplyMeasurementAndInput(
  const boost::numeric::ublas::vector<double>& measurements,
  const boost::numeric::ublas::vector<double>& input)
{
  
  assert(measurements.size() == input.size());

  //Store the calculation
  m_last_calculation->Clear();
  assert(!m_last_calculation->IsComplete());
  m_last_calculation->SetPreviousStateEstimate(this->GetStateEstimate());

  //m_last_calculation->SetMeasurement must be set before calling PredictState!
  m_last_calculation->SetMeasurement(measurements);

  // 1/1) State prediction
  const boost::numeric::ublas::vector<double> state_prediction = PredictState(input);
  m_state_estimate = state_prediction;

  //Store the calculation
  m_last_calculation->SetPredictedState(state_prediction);
  m_last_calculation->SetUpdatedState(GetStateEstimate());
  assert(m_last_calculation->IsComplete());
}

 

 

 

 

 

./CppKalmanFilter/steadystatekalmanfiltercalculationelements.h

 

#ifndef STEADYSTATEKALMANFILTERCALCULATIONELEMENTS_H
#define STEADYSTATEKALMANFILTERCALCULATIONELEMENTS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include "kalmanfiltercalculationelements.h"
#pragma GCC diagnostic pop

namespace ribi {
namespace kalman {

///The elements of a steady-state Kalman filter calculation
struct SteadyStateKalmanFilterCalculationElements : public KalmanFilterCalculationElements
{
  explicit SteadyStateKalmanFilterCalculationElements(
    const boost::numeric::ublas::vector<double>& measurement = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::vector<double>& predicted_state = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::vector<double>& previous_state_estimate = boost::numeric::ublas::vector<double>(),
    const boost::numeric::ublas::vector<double>& updated_state = boost::numeric::ublas::vector<double>());

  ///Produce a deep copy of the derived class
  boost::shared_ptr<KalmanFilterCalculationElements> Clone() const;

  ///Obtain the Kalman filter type as an enum
  KalmanFilterType GetType() const { return KalmanFilterType::steady_state; }

  ///Checks if the state is complete and valid
  bool IsComplete() const;

  private:
  ///Can only be deleted by boost::checked_delete
  ~SteadyStateKalmanFilterCalculationElements() noexcept {}
  friend void boost::checked_delete<>(SteadyStateKalmanFilterCalculationElements*);

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STEADYSTATEKALMANFILTERCALCULATIONELEMENTS_H

 

 

 

 

 

./CppKalmanFilter/steadystatekalmanfiltercalculationelements.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "steadystatekalmanfiltercalculationelements.h"
#pragma GCC diagnostic pop

#include <cassert>
#include "kalmanfiltercalculationelementsfactory.h"

ribi::kalman::SteadyStateKalmanFilterCalculationElements::SteadyStateKalmanFilterCalculationElements(
  const boost::numeric::ublas::vector<double>& measurement,
  const boost::numeric::ublas::vector<double>& predicted_state,
  const boost::numeric::ublas::vector<double>& previous_state_estimate,
  const boost::numeric::ublas::vector<double>& updated_state)

  : KalmanFilterCalculationElements(
      measurement,
      predicted_state,
      previous_state_estimate,
      updated_state)
{
  //... nothing to check
}

boost::shared_ptr<ribi::kalman::KalmanFilterCalculationElements> ribi::kalman::SteadyStateKalmanFilterCalculationElements::Clone() const
{
  const boost::shared_ptr<KalmanFilterCalculationElements> p {
    new SteadyStateKalmanFilterCalculationElements(
      //Base
      this->GetMeasurement(),
      this->GetPredictedState(),
      this->GetPreviousState(),
      this->GetUpdatedState()
      //Derived class
      //... no additional data members
    )
  };
  assert(p);
  assert(p->GetType() == this->GetType());
  return p;
}

bool ribi::kalman::SteadyStateKalmanFilterCalculationElements::IsComplete() const
{
  const std::size_t sz = GetMeasurement().size();
  return
       sz != 0
    && sz == GetMeasurement().size()
    && sz == GetPreviousState().size()
    && sz == GetUpdatedState().size();
}

 

 

 

 

 

./CppKalmanFilter/steadystatekalmanfilterfactory.h

 

#ifndef STEADYSTATEKALMANFILTERFACTORY_H
#define STEADYSTATEKALMANFILTERFACTORY_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/shared_ptr.hpp>
#pragma GCC diagnostic pop

#include "steadystatekalmanfilter.h"

namespace ribi {
namespace kalman {

///Factory for SteadyStateKalmanFilter
struct SteadyStateKalmanFilterFactory
{
  SteadyStateKalmanFilterFactory();

  boost::shared_ptr<SteadyStateKalmanFilter> Create(
    const boost::shared_ptr<const KalmanFilterParameters>& parameters
  ) const noexcept;

  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STEADYSTATEKALMANFILTERFACTORY_H

 

 

 

 

 

./CppKalmanFilter/steadystatekalmanfilterfactory.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "steadystatekalmanfilterfactory.h"

#include <cassert>
#include "kalmanfiltercalculationelementsfactory.h"

#include "testtimer.h"
#pragma GCC diagnostic pop

ribi::kalman::SteadyStateKalmanFilterFactory::SteadyStateKalmanFilterFactory()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::shared_ptr<ribi::kalman::SteadyStateKalmanFilter> ribi::kalman::SteadyStateKalmanFilterFactory::Create(
  const boost::shared_ptr<const KalmanFilterParameters>& parameters) const noexcept
{
  assert(parameters);
  assert(parameters->GetType() == KalmanFilterType::steady_state);

  const boost::shared_ptr<KalmanFilterCalculationElements> calculation {
    KalmanFilterCalculationElementsFactory().Create(parameters->GetType())
  };

  assert(calculation);

  const boost::shared_ptr<SteadyStateKalmanFilter> kalman_filter {
    new SteadyStateKalmanFilter(calculation,parameters)
  };

  assert(kalman_filter);
  assert(kalman_filter->GetType() == KalmanFilterType::steady_state);
  return kalman_filter;
}


#ifndef NDEBUG
void ribi::kalman::SteadyStateKalmanFilterFactory::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/steadystatekalmanfilterparameters.h

 

#ifndef STEADYSTATEKALMANFILTERPARAMETERS_H
#define STEADYSTATEKALMANFILTERPARAMETERS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/checked_delete.hpp>
#pragma GCC diagnostic pop

#include "kalmanfilterparameters.h"

namespace ribi {
namespace kalman {

///Parameters for the standard Kalman filter
struct SteadyStateKalmanFilterParameters : public KalmanFilterParameters
{
  explicit SteadyStateKalmanFilterParameters(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::matrix<double>& estimated_optimal_kalman_gain,
    const boost::numeric::ublas::vector<double>& initial_state_estimate,
    const boost::numeric::ublas::matrix<double>& observation,
    const boost::numeric::ublas::matrix<double>& state_transition
  );

  ///Calculate the optimal Kalman gain
  static boost::numeric::ublas::matrix<double> CalculateOptimalKalmanGain(
    const boost::numeric::ublas::matrix<double>& initial_covariance,
    const boost::numeric::ublas::matrix<double>& measurement_noise,
    const boost::numeric::ublas::matrix<double>& observation,
    const boost::numeric::ublas::matrix<double>& state_transition);

  ///Obtain the (estimated) optimal Kalman gain ('K')
  const boost::numeric::ublas::matrix<double>& GetEstimatedOptimalKalmanGain() const
    { return m_estimated_optimal_kalman_gain; }


  ///Obtain the Kalman filter type as an enum
  KalmanFilterType GetType() const { return KalmanFilterType::steady_state; }

  ///Check if two parameter sets are equal with a fuzzy compare
  static bool IsAboutEqual(const SteadyStateKalmanFilterParameters& lhs, const SteadyStateKalmanFilterParameters& rhs);

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Check if this parameter set has a certain type of KalmanFilterParameter
  static bool HasParameterType(const KalmanFilterParameterType type);

  private:
  ///Can only be deleted by boost::checked_delete
  ~SteadyStateKalmanFilterParameters() noexcept {}
  friend void boost::checked_delete<>(SteadyStateKalmanFilterParameters*);

  ///K: Estimated optimal Kalman gain
  const boost::numeric::ublas::matrix<double> m_estimated_optimal_kalman_gain;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // STEADYSTATEKALMANFILTERPARAMETERS_H

 

 

 

 

 

./CppKalmanFilter/steadystatekalmanfilterparameters.cpp

 



#include "steadystatekalmanfilterparameters.h"

#include <cassert>

#include "matrix.h"

ribi::kalman::SteadyStateKalmanFilterParameters::SteadyStateKalmanFilterParameters(
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::matrix<double>& estimated_optimal_kalman_gain,
  const boost::numeric::ublas::vector<double>& initial_state_estimate,
  const boost::numeric::ublas::matrix<double>& observation,
  const boost::numeric::ublas::matrix<double>& state_transition)
  : KalmanFilterParameters(control,initial_state_estimate,observation,state_transition),
    m_estimated_optimal_kalman_gain{estimated_optimal_kalman_gain}
{
  #ifndef NDEBUG
  //Check for correct dimensionality
  const std::size_t sz = GetInitialStateEstimate().size();
  //assert(sz >= 0); //Inevitable for std::size_t
  assert(GetEstimatedOptimalKalmanGain().size1() == sz);
  assert(GetEstimatedOptimalKalmanGain().size2() == sz);
  #endif
}

boost::numeric::ublas::matrix<double> ribi::kalman::SteadyStateKalmanFilterParameters::CalculateOptimalKalmanGain(
  const boost::numeric::ublas::matrix<double>& initial_covariance,
  const boost::numeric::ublas::matrix<double>& measurement_noise,
  const boost::numeric::ublas::matrix<double>& observation,
  const boost::numeric::ublas::matrix<double>& state_transition)
{
  const boost::numeric::ublas::matrix<double>& a = state_transition;
  const boost::numeric::ublas::matrix<double>& c = observation;
  const boost::numeric::ublas::matrix<double>& p = initial_covariance;
  const boost::numeric::ublas::matrix<double>& r = measurement_noise;

  const boost::numeric::ublas::matrix<double> term_a
    = Matrix::MultiProd(a,p,boost::numeric::ublas::trans(c));
  if (Matrix::CalcDeterminant(term_a + r) == 0.0)
  {
    throw std::runtime_error("Optimal Kalman gain matrix is degenerate");
  }
  const boost::numeric::ublas::matrix<double> k
    = Matrix::Prod(term_a,Matrix::Inverse(term_a + r));

  return k;

}

std::string ribi::kalman::SteadyStateKalmanFilterParameters::GetVersion() noexcept
{
  return "1.1";
}

std::vector<std::string> ribi::kalman::SteadyStateKalmanFilterParameters::GetVersionHistory() noexcept
{
  return {
    "2013-05-01: version 1.0: initial version"
    "2013-05-03: version 1.1: inherit from KalmanFilterParameters"
  };
}

bool ribi::kalman::SteadyStateKalmanFilterParameters::HasParameterType(const KalmanFilterParameterType type)
{
  return
       type == KalmanFilterParameterType::control
    || type == KalmanFilterParameterType::estimated_optimal_kalman_gain
    || type == KalmanFilterParameterType::initial_state_estimate
    || type == KalmanFilterParameterType::observation
    || type == KalmanFilterParameterType::state_transition;
}

bool ribi::kalman::SteadyStateKalmanFilterParameters::IsAboutEqual(
  const SteadyStateKalmanFilterParameters& lhs,
  const SteadyStateKalmanFilterParameters& rhs)
{
  return
       Matrix::MatricesAreAboutEqual(lhs.GetControl(),rhs.GetControl())
    && Matrix::MatricesAreAboutEqual(lhs.GetEstimatedOptimalKalmanGain(),rhs.GetEstimatedOptimalKalmanGain())
    && Matrix::VectorsAreAboutEqual(lhs.GetInitialStateEstimate(),rhs.GetInitialStateEstimate())
    && Matrix::MatricesAreAboutEqual(lhs.GetObservation(),rhs.GetObservation())
    && Matrix::MatricesAreAboutEqual(lhs.GetStateTransition(),rhs.GetStateTransition());
}

 

 

 

 

 

./CppKalmanFilter/whitenoisesystem.h

 

#ifndef WHITENOISESYSTEM_H
#define WHITENOISESYSTEM_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/shared_ptr.hpp>

#include "whitenoisesystemtype.h"
#include "whitenoisesystemparameters.h"

#pragma GCC diagnostic pop

namespace ribi {
namespace kalman {

struct WhiteNoiseSystem
{
  WhiteNoiseSystem(const WhiteNoiseSystem&) = delete;
  WhiteNoiseSystem& operator=(const WhiteNoiseSystem&) = delete;

  virtual ~WhiteNoiseSystem() noexcept {}

  ///The parameters
  const boost::shared_ptr<const WhiteNoiseSystemParameters>& GetParameters() const noexcept { return m_parameters; }

  ///Obtain the type as an enum
  virtual WhiteNoiseSystemType GetType() const noexcept = 0;

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Update reality, that is, let the real system (i.e. reality) go to its next state
  virtual void GoToNextState(const boost::numeric::ublas::vector<double>& input) = 0;

  ///Measure a value from this system with normally distributed noise
  virtual boost::numeric::ublas::vector<double> Measure() const = 0;

  ///Peek what the real value is
  virtual const boost::numeric::ublas::vector<double>& PeekAtRealState() const noexcept { return m_current_state; }

  protected:
  ///An ABC can only be constructed by derived classes
  explicit WhiteNoiseSystem(const boost::shared_ptr<const WhiteNoiseSystemParameters>& parameters);

  ///Obtain a random number from a normal distribution
  ///From http://www.richelbilderbeek.nl/CppGetRandomNormal.htm
  static double GetRandomNormal(const double mean = 0.0, const double sigma = 1.0) noexcept;

  ///The real state of the system
  const boost::numeric::ublas::vector<double>& GetCurrentState() const noexcept { return m_current_state; }

  ///Set the new current state
  void SetNewCurrentState(const boost::numeric::ublas::vector<double>& new_current_state);

  private:
  ///The real state of the system
  boost::numeric::ublas::vector<double> m_current_state;

  ///The parameters
  const boost::shared_ptr<const WhiteNoiseSystemParameters> m_parameters;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // WHITENOISESYSTEM_H

 

 

 

 

 

./CppKalmanFilter/whitenoisesystem.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "whitenoisesystem.h"

#include <cassert>

#include "testtimer.h"
#include "ribi_random.h"

#pragma GCC diagnostic pop

ribi::kalman::WhiteNoiseSystem::WhiteNoiseSystem(
  const boost::shared_ptr<const WhiteNoiseSystemParameters>& parameters
)
  : m_current_state{parameters->GetInitialState()},
    m_parameters{parameters}
{
  assert(m_parameters);
}

double ribi::kalman::WhiteNoiseSystem::GetRandomNormal(const double mean, const double sigma) noexcept
{
  return Random().GetNormal(mean,sigma);
}

std::string ribi::kalman::WhiteNoiseSystem::GetVersion() noexcept
{
  return "1.0";
}

std::vector<std::string> ribi::kalman::WhiteNoiseSystem::GetVersionHistory() noexcept
{
  return {
    "2013-05-03: version 1.0: initial version"
  };
}

void ribi::kalman::WhiteNoiseSystem::SetNewCurrentState(const boost::numeric::ublas::vector<double>& new_current_state)
{
  assert(m_current_state.size() == new_current_state.size());
  m_current_state = new_current_state;
}

#ifndef NDEBUG
void ribi::kalman::WhiteNoiseSystem::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    Random();
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemfactory.h

 

#ifndef WHITENOISESYSTEMFACTORY_H
#define WHITENOISESYSTEMFACTORY_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/shared_ptr.hpp>
#include "whitenoisesystem.h"
#pragma GCC diagnostic pop

#include "whitenoisesystemparameters.h"

namespace ribi {
namespace kalman {

struct WhiteNoiseSystemFactory
{
  WhiteNoiseSystemFactory();

  boost::shared_ptr<WhiteNoiseSystem> Create(
    const boost::shared_ptr<WhiteNoiseSystemParameters>& parameters) const noexcept;

  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // WHITENOISESYSTEMFACTORY_H

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemfactory.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "whitenoisesystemfactory.h"
#pragma GCC diagnostic pop

#include <cassert>
#include "gapsfilledwhitenoisesystemfactory.h"
#include "laggedwhitenoisesystemfactory.h"
#include "standardwhitenoisesystemfactory.h"
#include "testtimer.h"

ribi::kalman::WhiteNoiseSystemFactory::WhiteNoiseSystemFactory()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::shared_ptr<ribi::kalman::WhiteNoiseSystem> ribi::kalman::WhiteNoiseSystemFactory::Create(
  const boost::shared_ptr<WhiteNoiseSystemParameters>& parameters) const noexcept
{
  boost::shared_ptr<WhiteNoiseSystem> p;
  switch (parameters->GetType())
  {
    case WhiteNoiseSystemType::gaps_filled:
      p = GapsFilledWhiteNoiseSystemFactory().Create(parameters);
      break;
    case WhiteNoiseSystemType::lagged:
      p = LaggedWhiteNoiseSystemFactory().Create(parameters);
      break;
    case WhiteNoiseSystemType::standard:
      p = StandardWhiteNoiseSystemFactory().Create(parameters);
      break;
    case WhiteNoiseSystemType::n_types:
      assert(!"Unimplemented white noise system");
      throw std::logic_error(__func__);
  }
  assert(p);
  return p;
}

#ifndef NDEBUG
void ribi::kalman::WhiteNoiseSystemFactory::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemparameter.h

 

#ifndef WHITENOISESYSTEMPARAMETER_H
#define WHITENOISESYSTEMPARAMETER_H

#include <string>
#include <vector>
#include "whitenoisesystemparametertype.h"

namespace ribi {
namespace kalman {

///A single WhiteNoiseSystemParameter
///for example, the state transition matrix
/// - is a matrix
/// - is not a vector
/// - has a description like 'Matrix that [...]'
/// - has the name 'State transition matrix'
/// - has the symbol 'A'
struct WhiteNoiseSystemParameter
{
  WhiteNoiseSystemParameter();

  ///Obtain all WhiteNoiseSystemParameterType values, except n_parameters
  std::vector<WhiteNoiseSystemParameterType> GetAll() const noexcept;

  ///Is the type a matrix?
  bool IsMatrix(const WhiteNoiseSystemParameterType type) const noexcept;

  ///Is the type a vector?
  bool IsVector(const WhiteNoiseSystemParameterType type) const noexcept;

  ///Obtain the full name of a type, e.g. 'Matrix to capture the physics of the system'
  std::string ToDescription(const WhiteNoiseSystemParameterType type) const noexcept;

  ///Obtain the full name of a type, e.g. 'State transition'
  std::string ToName(const WhiteNoiseSystemParameterType type) const noexcept;

  ///Obtain the symbol of a type, e.g. 'A'
  std::string ToSymbol(const WhiteNoiseSystemParameterType type) const noexcept;

  private:
  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // WHITENOISESYSTEMPARAMETER_H

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemparameter.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "whitenoisesystemparameter.h"

#include <cassert>
#include <stdexcept>
#include <boost/numeric/conversion/cast.hpp>

#include "testtimer.h"
#pragma GCC diagnostic pop

ribi::kalman::WhiteNoiseSystemParameter::WhiteNoiseSystemParameter()
{
  #ifndef NDEBUG
  Test();
  #endif
}
std::vector<ribi::kalman::WhiteNoiseSystemParameterType>
  ribi::kalman::WhiteNoiseSystemParameter::GetAll() const noexcept
{
  const std::vector<WhiteNoiseSystemParameterType> v {
    WhiteNoiseSystemParameterType::control,
    WhiteNoiseSystemParameterType::initial_state_real,
    WhiteNoiseSystemParameterType::measurement_frequency,
    WhiteNoiseSystemParameterType::real_measurement_noise,
    WhiteNoiseSystemParameterType::real_process_noise,
    WhiteNoiseSystemParameterType::state_transition,
  };

  assert(boost::numeric_cast<int>(v.size()) == static_cast<int>(WhiteNoiseSystemParameterType::n_parameters)
    && "All parameters must be in");
  return v;
}

bool ribi::kalman::WhiteNoiseSystemParameter::IsMatrix(
  const WhiteNoiseSystemParameterType type) const noexcept
{
  switch (type)
  {
    case WhiteNoiseSystemParameterType::control: return true;
    case WhiteNoiseSystemParameterType::initial_state_real: return false;
    case WhiteNoiseSystemParameterType::measurement_frequency: return false;
    case WhiteNoiseSystemParameterType::real_measurement_noise: return false;
    case WhiteNoiseSystemParameterType::real_process_noise: return false;
    case WhiteNoiseSystemParameterType::state_transition: return true;
    case WhiteNoiseSystemParameterType::n_parameters:
      assert(!"n_parameters is not an implemented type of WhiteNoiseSystemParameterType");
      throw std::logic_error(__func__);
  }
  assert(!"Unimplemented type of WhiteNoiseSystemParameterType");
  throw std::logic_error(__func__);
}

bool ribi::kalman::WhiteNoiseSystemParameter::IsVector(
  const WhiteNoiseSystemParameterType type) const noexcept
{
  return !ribi::kalman::WhiteNoiseSystemParameter::IsMatrix(type);
}

std::string ribi::kalman::WhiteNoiseSystemParameter::ToDescription(
  const WhiteNoiseSystemParameterType type) const noexcept
{
  switch (type)
  {
    case WhiteNoiseSystemParameterType::control:
      return "Matrix for converting input to state change";
    case WhiteNoiseSystemParameterType::initial_state_real:
      return "Vector with the real initial state";
    case WhiteNoiseSystemParameterType::measurement_frequency:
      return "Vector containing after which number of timesteps a measurement is taken";
    case WhiteNoiseSystemParameterType::real_measurement_noise:
      return "Vector with the real standard deviations of the measurement noise per state";
    case WhiteNoiseSystemParameterType::real_process_noise:
      return "Vector with the real standard deviations of the process noise per state";
    case WhiteNoiseSystemParameterType::state_transition:
      return "Matrix that contains the internal physics of the system; the effect of current state on the next state";
    case WhiteNoiseSystemParameterType::n_parameters:
      assert(!"Unimplemented type of WhiteNoiseSystemParameterType");
      throw std::logic_error(__func__);
  }
  assert(!"Unimplemented type of WhiteNoiseSystemParameterType");
  throw std::logic_error(__func__);
}

std::string ribi::kalman::WhiteNoiseSystemParameter::ToName(
  const WhiteNoiseSystemParameterType type) const noexcept
{
  switch (type)
  {
    case WhiteNoiseSystemParameterType::control:
      return "Control";
    case WhiteNoiseSystemParameterType::initial_state_real:
      return "Real initial state";
    case WhiteNoiseSystemParameterType::measurement_frequency:
      return "Measurement frequencies";
    case WhiteNoiseSystemParameterType::real_measurement_noise:
      return "Real measurement noise";
    case WhiteNoiseSystemParameterType::real_process_noise:
      return "Real process noise";
    case WhiteNoiseSystemParameterType::state_transition:
      return "State transition";
    case WhiteNoiseSystemParameterType::n_parameters:
      assert(!"Unimplemented type of WhiteNoiseSystemParameterType");
      throw std::logic_error(__func__);
  }
  assert(!"Unimplemented type of WhiteNoiseSystemParameterType");
  throw std::logic_error(__func__);
}

std::string ribi::kalman::WhiteNoiseSystemParameter::ToSymbol(
  const WhiteNoiseSystemParameterType type) const noexcept
{
  switch (type)
  {
    case WhiteNoiseSystemParameterType::control:
      return "B";
    case WhiteNoiseSystemParameterType::initial_state_real:
      return "x";
    case WhiteNoiseSystemParameterType::measurement_frequency:
      return "f";
    case WhiteNoiseSystemParameterType::real_measurement_noise:
      return "R"; //Shouldn't be 'r', as it is a vector?
    case WhiteNoiseSystemParameterType::real_process_noise:
      return "Q"; //Shouldn't be 'q', as it is a vector?
    case WhiteNoiseSystemParameterType::state_transition:
      return "A";
    case WhiteNoiseSystemParameterType::n_parameters:
      assert(!"Unimplemented type of WhiteNoiseSystemParameterType");
      throw std::logic_error(__func__);
  }
  assert(!"Unimplemented type of WhiteNoiseSystemParameterType");
  throw std::logic_error(__func__);
}

#ifndef NDEBUG
void ribi::kalman::WhiteNoiseSystemParameter::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  {
    //Matrix();
    //const boost::shared_ptr<LaggedWhiteNoiseSystem> my_system
    //  = LaggedWhiteNoiseSystemFactory().Create(
    //    Matrix::CreateMatrix(1,1, { 1.0 } ), //control
    //    Matrix::CreateVector(     { 0.0 } ), //initial_state,
    //    0,
    //    Matrix::CreateVector(     { 0.0 } ), //real_measurement_noise
    //    Matrix::CreateVector(     { 0.0 } ), //real_process_noise
    //    Matrix::CreateMatrix(1,1, { 1.0 } )  //state_transition
    //);
  }
  const TestTimer test_timer(__func__,__FILE__,1.0);
}
#endif

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemparameters.h

 

#ifndef WHITENOISESYSTEMPARAMETERS_H
#define WHITENOISESYSTEMPARAMETERS_H

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#pragma GCC diagnostic pop

#include "whitenoisesystemtype.h"


namespace ribi {
namespace kalman {

///ABC of the parameters of a white noise system
struct WhiteNoiseSystemParameters
{
  WhiteNoiseSystemParameters(const WhiteNoiseSystemParameters&) = delete;
  WhiteNoiseSystemParameters& operator=(const WhiteNoiseSystemParameters&) = delete;

  virtual ~WhiteNoiseSystemParameters() noexcept {}

  ///The control matrix to determine the influence of the input (in GoToNextState)
  const boost::numeric::ublas::matrix<double>& GetControl() const noexcept
    { return m_control; }

  ///Obtain the real initial state
  const boost::numeric::ublas::vector<double>& GetInitialState() const noexcept
    { return m_initial_state; }

  ///The real standard deviation of the noise in the state transition
  ///(used in WhiteNoiseSystem::GoToNextState)
  const boost::numeric::ublas::vector<double>& GetProcessNoise() const noexcept
    { return m_real_process_noise; }

  ///The real amount of noise in the system
  ///A noise of zero indicates a system that can be measured accurately to infinite precision
  const boost::numeric::ublas::vector<double>& GetMeasurementNoise() const noexcept
    { return m_real_measurement_noise; }

  ///The state transitions in the system
  ///(used in WhiteNoiseSystem::GoToNextState)
  const boost::numeric::ublas::matrix<double>& GetStateTransition() const noexcept
    { return m_state_transition; }

  ///Obtain the type as an enum
  virtual WhiteNoiseSystemType GetType() const noexcept = 0;

  ///Obtain the version of this class
  static std::string GetVersion() noexcept;

  ///Obtain the version history of this class
  static std::vector<std::string> GetVersionHistory() noexcept;

  ///Check two parameter sets for equality using a fuzzy comparison
  static bool IsAboutEqual(const WhiteNoiseSystemParameters& lhs, const WhiteNoiseSystemParameters& rhs) noexcept;

  protected:
  ///An ABC can only be constructed by derived classes
  explicit WhiteNoiseSystemParameters(
    const boost::numeric::ublas::matrix<double>& control,
    const boost::numeric::ublas::vector<double>& initial_state,
    const boost::numeric::ublas::vector<double>& real_measurement_noise,
    const boost::numeric::ublas::vector<double>& real_process_noise,
    const boost::numeric::ublas::matrix<double>& state_transition);

  private:
  ///The control matrix to determine the influence of the input (in GoToNextState)
  const boost::numeric::ublas::matrix<double> m_control;

  ///The real initial state
  const boost::numeric::ublas::vector<double> m_initial_state;

  ///The real amount of noise in the system
  ///A noise of zero indicates a system that can be measured accurately to infinite precision
  const boost::numeric::ublas::vector<double> m_real_measurement_noise;

  ///The standard deviation of the noise in the state transition (in GoToNextState)
  const boost::numeric::ublas::vector<double> m_real_process_noise;

  ///The state transitions in the system, used in GoToNextState
  const boost::numeric::ublas::matrix<double> m_state_transition;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif
};

} //~namespace kalman
} //~namespace ribi

#endif // WHITENOISESYSTEMPARAMETERS_H

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemparameters.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "whitenoisesystemparameters.h"
#pragma GCC diagnostic pop

#include <cassert>
#include "matrix.h"

ribi::kalman::WhiteNoiseSystemParameters::WhiteNoiseSystemParameters(
  const boost::numeric::ublas::matrix<double>& control,
  const boost::numeric::ublas::vector<double>& initial_state,
  const boost::numeric::ublas::vector<double>& real_measurement_noise,
  const boost::numeric::ublas::vector<double>& real_process_noise,
  const boost::numeric::ublas::matrix<double>& state_transition)
  : m_control{control},
    m_initial_state{initial_state},
    m_real_measurement_noise{real_measurement_noise},
    m_real_process_noise{real_process_noise},
    m_state_transition{state_transition}
{
  #ifndef NDEBUG
  //Check for correct dimensionality
  const size_t sz = initial_state.size();
  //assert(sz >= 0); //Inevitable for std::size_t
  assert(m_control.size1() == sz);
  assert(m_control.size2() == sz);
  assert(m_initial_state.size() == sz);
  assert(m_real_measurement_noise.size() == sz);
  assert(m_real_process_noise.size() == sz);
  assert(m_state_transition.size1() == sz);
  assert(m_state_transition.size2() == sz);
  #endif
}

std::string ribi::kalman::WhiteNoiseSystemParameters::GetVersion() noexcept
{
  return "1.0";
}

std::vector<std::string> ribi::kalman::WhiteNoiseSystemParameters::GetVersionHistory() noexcept
{
  return {
    "2013-04-28: version 1.0: initial version"
  };
}

bool ribi::kalman::WhiteNoiseSystemParameters::IsAboutEqual(
  const WhiteNoiseSystemParameters& lhs, const WhiteNoiseSystemParameters& rhs) noexcept
{
  return
       Matrix::MatricesAreAboutEqual(lhs.GetControl(),rhs.GetControl())
    && Matrix::VectorsAreAboutEqual(lhs.GetInitialState(),rhs.GetInitialState())
    && Matrix::VectorsAreAboutEqual(lhs.GetMeasurementNoise(),rhs.GetMeasurementNoise())
    && Matrix::VectorsAreAboutEqual(lhs.GetProcessNoise(),rhs.GetProcessNoise())
    && Matrix::MatricesAreAboutEqual(lhs.GetStateTransition(),rhs.GetStateTransition());
}

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemparametertype.h

 

#ifndef WHITENOISESYSTEMPARAMETERTYPE_H
#define WHITENOISESYSTEMPARAMETERTYPE_H

namespace ribi {
namespace kalman {

enum class WhiteNoiseSystemParameterType
{
  control,
  initial_state_real,
  measurement_frequency,
  real_measurement_noise,
  real_process_noise,
  state_transition,
  n_parameters
};

bool operator<(const WhiteNoiseSystemParameterType lhs, const WhiteNoiseSystemParameterType rhs);
bool operator==(const WhiteNoiseSystemParameterType lhs, const WhiteNoiseSystemParameterType rhs);
bool operator!=(const WhiteNoiseSystemParameterType lhs, const WhiteNoiseSystemParameterType rhs);

} //~namespace kalman
} //~namespace ribi

#endif // WHITENOISESYSTEMPARAMETERTYPE_H

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemparametertype.cpp

 

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
#include "whitenoisesystemparametertype.h"

#include <cassert>
#include <boost/numeric/conversion/cast.hpp>
#pragma GCC diagnostic pop

bool ribi::kalman::operator<(const WhiteNoiseSystemParameterType lhs, const WhiteNoiseSystemParameterType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) < static_cast<int>(rhs);
}

bool ribi::kalman::operator==(const WhiteNoiseSystemParameterType lhs, const WhiteNoiseSystemParameterType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) == static_cast<int>(rhs);
}

bool ribi::kalman::operator!=(const WhiteNoiseSystemParameterType lhs, const WhiteNoiseSystemParameterType rhs)
{
  return !(lhs == rhs);
}

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemtype.h

 

#ifndef WHITENOISESYSTEMTYPE_H
#define WHITENOISESYSTEMTYPE_H

#include <string>

namespace ribi {
namespace kalman {

enum class WhiteNoiseSystemType
{
  gaps_filled,
  lagged,
  standard,
  n_types
};

bool operator==(const WhiteNoiseSystemType lhs, const WhiteNoiseSystemType rhs);
bool operator!=(const WhiteNoiseSystemType lhs, const WhiteNoiseSystemType rhs);
bool operator<(const WhiteNoiseSystemType lhs, const WhiteNoiseSystemType rhs);

} //~namespace kalman
} //~namespace ribi

#endif // WHITENOISESYSTEMTYPE_H

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemtype.cpp

 



#include "whitenoisesystemtype.h"

bool ribi::kalman::operator==(const WhiteNoiseSystemType lhs, const WhiteNoiseSystemType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) == static_cast<int>(rhs);
}

bool ribi::kalman::operator!=(const WhiteNoiseSystemType lhs, const WhiteNoiseSystemType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) != static_cast<int>(rhs);
}

bool ribi::kalman::operator<(const WhiteNoiseSystemType lhs, const WhiteNoiseSystemType rhs)
{
  //Must cast enum class to integer, because of a bug
  //in GCC version 4.4.0:
  //http://gcc.gnu.org/bugzilla/show_bug.cgi?id=38064
  return static_cast<int>(lhs) < static_cast<int>(rhs);
}

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemtypes.h

 

#ifndef WHITENOISESYSTEMTYPES_H
#define WHITENOISESYSTEMTYPES_H

#include <string>
#include <vector>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/bimap.hpp>
#pragma GCC diagnostic pop

#include "whitenoisesystemtype.h"

namespace ribi {
namespace kalman {

struct WhiteNoiseSystemTypes
{
  WhiteNoiseSystemTypes();
  std::vector<WhiteNoiseSystemType> GetAllTypes() const noexcept;
  std::string ToStr(const WhiteNoiseSystemType type) const noexcept;
  WhiteNoiseSystemType ToType(const std::string& s) const;

  private:
  static boost::bimap<WhiteNoiseSystemType,std::string> m_map;
  static boost::bimap<WhiteNoiseSystemType,std::string> CreateMap() noexcept;

  #ifndef NDEBUG
  static void Test() noexcept;
  #endif

};

} //~namespace kalman
} //~namespace ribi

#endif // WHITENOISESYSTEMTYPES_H

 

 

 

 

 

./CppKalmanFilter/whitenoisesystemtypes.cpp

 

#include "whitenoisesystemtypes.h"

boost::bimap<ribi::kalman::WhiteNoiseSystemType,std::string> ribi::kalman::WhiteNoiseSystemTypes::m_map;

ribi::kalman::WhiteNoiseSystemTypes::WhiteNoiseSystemTypes()
{
  #ifndef NDEBUG
  Test();
  #endif
}

boost::bimap<ribi::kalman::WhiteNoiseSystemType,std::string> ribi::kalman::WhiteNoiseSystemTypes::CreateMap() noexcept
{

  boost::bimap<WhiteNoiseSystemType,std::string> m;
  m.insert(boost::bimap<WhiteNoiseSystemType,std::string>::value_type(
    WhiteNoiseSystemType::gaps_filled,"gaps_filled"));
  m.insert(boost::bimap<WhiteNoiseSystemType,std::string>::value_type(
    WhiteNoiseSystemType::lagged,"lagged"));
  m.insert(boost::bimap<WhiteNoiseSystemType,std::string>::value_type(
    WhiteNoiseSystemType::standard,"standard"));
  return m;
}

std::vector<ribi::kalman::WhiteNoiseSystemType>
  ribi::kalman::WhiteNoiseSystemTypes::GetAllTypes() const noexcept
{
  const std::vector<WhiteNoiseSystemType> v {
    WhiteNoiseSystemType::gaps_filled,
    WhiteNoiseSystemType::lagged,
    WhiteNoiseSystemType::standard
  };
  assert(static_cast<int>(v.size()) == static_cast<int>(WhiteNoiseSystemType::n_types));
  return v;
}

#ifndef NDEBUG
void ribi::kalman::WhiteNoiseSystemTypes::Test() noexcept
{
  {
    static bool is_tested{false};
    if (is_tested) return;
    is_tested = true;
  }
  for (const auto& t: WhiteNoiseSystemTypes().GetAllTypes())
  {
    const std::string s = WhiteNoiseSystemTypes().ToStr(t);
    assert(!s.empty());
    const auto u = WhiteNoiseSystemTypes().ToType(s);
    assert(u == t);
  }
}
#endif

std::string ribi::kalman::WhiteNoiseSystemTypes::ToStr(const WhiteNoiseSystemType type) const noexcept
{
  if (m_map.left.empty()) { m_map = CreateMap(); }
  assert(!m_map.left.empty());
  assert(m_map.left.count(type) == 1);
  const std::string s = m_map.left.find(type)->second;
  return s;
}

ribi::kalman::WhiteNoiseSystemType ribi::kalman::WhiteNoiseSystemTypes::ToType(const std::string& s) const
{
  if (m_map.right.empty()) { m_map = CreateMap(); }
  assert(!m_map.right.empty());
  assert(m_map.right.count(s) == 1);
  const auto t = m_map.right.find(s)->second;
  return t;
}

 

 

 

 

 

Go back to Richel Bilderbeek's C++ page.

Go back to Richel Bilderbeek's homepage.

 

Valid XHTML 1.0 Strict

This page has been created by the tool CodeToHtml